本页介绍了set_initial_pose的用法, 包含如何在rviz中使用2D Pose Estimate设置机器的位姿。

本页内容

运行环境准备

  • 软件平台

    • Ubuntu 16.04 X86
    • ROS Kinetic

  • 硬件平台

          （以下任选其一）

      • Slamware 套装 （基于Slamware导航方案的用户机器人系统）
      • Apollo/Ares/Athena等底盘系统

例程下载

ROS-例程下载

编译运行

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，在界面中想要设置为小车位置的地方左键点击，按住左键移动更改角度，松开鼠标后，位姿设置成功。（下图中，左图为设置前效果，右图为设置后效果。）


代码描述

ros::Publisher set_pose_pub;
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);
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);
}