本页介绍了set_initial_pose的用法, 包含如何在rviz中使用2D Pose Estimate设置机器的位姿。
本页内容
运行环境准备
软件平台
- Ubuntu 16.04 X86
- ROS Kinetic
硬件平台
(以下任选其一)
- Slamware 套装 (基于Slamware导航方案的用户机器人系统)
- Apollo/Ares/Athena等底盘系统
编译运行
1.下载Slamware ROS SDK以及ROS例程。
2.将ROS sdk中的src文件夹放入一个空的工作目录,如:catkin_ws。将ROS 例程中的slamware_ros_sample文件夹也放入src文件夹下。使用catkin工具初始化工作空间。
cd catkin_ws/src catkin_init_workspace
3.编译
cd .. catkin_make
4.配置工作空间系统环境
source devel/setup.bash
5.启动slamware_ros_sdk_server_node节点
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节点
roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch
7.重新打开一个终端窗口,重复第4步,并启动set_initial_pose_node节点
roslaunch slamware_ros_sample set_initial_pose.launch
8.点击Rviz中的2D Pose Estimate,在界面中想要设置为小车位置的地方左键点击,按住左键移动更改角度,松开鼠标后,位姿设置成功。(下图中,左图为设置前效果,右图为设置后效果。)
代码描述
声明一个全局的Publisher变量
ros::Publisher set_pose_pub;
初始化节点,定义publisher和subcriber变量
ros::init(argc, argv, "set_initial_pose_node"); ros::NodeHandle nh("~"); set_pose_pub = nh.advertise<geometry_msgs::Pose>("/slamware_ros_sdk_server_node/set_pose", 10); ros::Subscriber initial_pose_sub = nh.subscribe("/initialpose", 10, initialPoseCallback);
接收到initialpose消息后,打印出消息内容,并发布
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& msg) { ROS_INFO("receive initialpose: "); ROS_INFO("position(xyz): %.3f %.3f %.3f", msg.pose.pose.position.x , msg.pose.pose.position.y , msg.pose.pose.position.z); ROS_INFO("orientation(xyzw): %.3f %.3f %.3f %.3f", msg.pose.pose.orientation.x , msg.pose.pose.orientation.y , msg.pose.pose.orientation.z , msg.pose.pose.orientation.w); geometry_msgs::Pose _pose_msg(msg.pose.pose); set_pose_pub.publish(_pose_msg); }