Excerpt |
---|
本页介绍了sync_set_stcm的用法, 包含如何通过sync_set_stcm服务上传stcm地图到机器。 |
本页内容
Table of Contents |
---|
运行环境准备
软件平台
- Ubuntu 16.04 AMD
- ROS Kinetic
硬件平台
(以下任选其一)
- Slamware 套装 (基于Slamware导航方案的用户机器人系统)
- Apollo/Ares/Athena等底盘系统
编译运行
1.下载Slamware ROS SDK以及ROS例程。
2.将ROS sdk中的src文件夹放入一个空的工作目录,如:catkin_ws。将ROS 例程中的slamware_ros_sample文件夹也放入src文件夹下。使用catkin工具初始化工作空间。
Code Block |
---|
cd catkin_ws/src catkin_init_workspace |
3.编译
Code Block |
---|
cd .. catkin_make |
4.配置工作空间系统环境
Code Block |
---|
source devel/setup.bash |
5.启动slamware_ros_sdk_server_node节点
Code Block |
---|
roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=10.6.128.141 //如果是AP模式,ip_address是192.168.11.1 |
6.重新打开一个终端窗口,重复第4步,并启动view_slamware_ros_sdk_server_node节点
Code Block | ||
---|---|---|
| ||
roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch |
7.重新打开一个终端窗口,重复第4步,并启动sync_set_stcm_node节点
Code Block |
---|
roslaunch slamware_ros_sample sync_set_stcm.launch //launch文件中有定义默认的map_file_name参数,指明需要上传的地图名称和所在路径。如果不用默认参数,可自行传参。 |
8.此时观察rviz界面下,如果成功找到地图文件,地图便会被上传,rviz的地图也会同步发生变化。反之则会报错。
代码描述
Code Block | ||||
---|---|---|---|---|
| ||||
int main (int argc, char **argv) { std::string map_file_name; long data_size; ros::init(argc, argv, "sync_set_stcm_node"); ros::NodeHandle nh("~"); ros::ServiceClient client = nh.serviceClient<slamware_ros_sdk::SyncSetStcm>("/slamware_ros_sdk_server_node/sync_set_stcm"); slamware_ros_sdk::SyncSetStcm srv; //get file name from paramter server if (!nh.getParam("map_file_name",map_file_name)) { ROS_ERROR("invalied file path......"); return 1; } ROS_INFO("map file path: %s",map_file_name.c_str()); //open map file and get data std::ifstream fin(map_file_name ,std::ios::in | std::ios::binary | std::ios::ate); if(!fin.is_open()) { ROS_ERROR("open map file failed......"); return 1; } ROS_INFO("open map file success,reading data......"); data_size = fin.tellg(); fin.seekg(0,std::ios::beg); srv.request.raw_stcm.resize(data_size); fin.read(reinterpret_cast<char *>(srv.request.raw_stcm.data()),data_size*sizeof(srv.request.raw_stcm.front())); fin.close(); ROS_INFO("raw_stcm data size: %zu",srv.request.raw_stcm.size()); //set robot pose srv.request.robot_pose.position.x = 0; srv.request.robot_pose.position.y = 0; srv.request.robot_pose.position.z = 0; srv.request.robot_pose.orientation.x = 0; srv.request.robot_pose.orientation.y = 0; srv.request.robot_pose.orientation.z = 0; srv.request.robot_pose.orientation.w = 1; //call service if (client.call(srv)) { ROS_INFO("set stcm map success......"); } else { ROS_ERROR("Failed to set stcm map......"); return 1; } return 0; } |