C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标

1. 首先查看设置初始坐标的话题 为 /intialpose ，查看消息类型和格式从而决定怎么给它发数据

（1）首先打开一个可以自动导航的项目文件，打开rviz，点击2D Pose Estimate 进行初始位姿矫正 ，查看/initialpose消息格式：

sun@sun-pc:~$ rostopic info /initialpose 
Type: geometry_msgs/PoseWithCovarianceStamped

Publishers: 
 * /rviz (http://sun-pc:40347/)

Subscribers: 
 * /amcl (http://sun-pc:35701/)

 （2）然后查看消息数据格式

sun@sun-pc:~$ rosmsg show geometry_msgs/PoseWithCovarianceStamped 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance

2.监听rviz发出的数据格式

sun@sun-pc:~$ rostopic echo /initialpose 
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 2
  stamp: 
    secs: 825
    nsecs: 700000000
  frame_id: "map"
pose: 
  pose: 
    position: 
      x: 39.8066101074
      y: 41.3922195435
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.0116650747515
      w: 0.999931960701
  covariance: [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 
                0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
                0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
                0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
---

3.仿照rviz消息格式

position：为坐标信息，对2d来说只有x和y值。

orientation为四元数格式，参考相关链接：四元数与欧拉角之间的转换

对2d平面的移动机器人感官上易于理解的就是朝向信息，即欧拉角中绕z轴旋转的偏航角。

俯仰角和滚转角为0，故x和y均为0，即只有w和z值。

若偏行角为alpha，则w = cos（alpha/2），z = sin（alpha/2）。

4.编写发布节点程序

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "math.h"
#define PI 3.1415926

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pose_estimate_2d");
  ros::NodeHandle nh;
  ros::Publisher initial_pose_pub = nh.advertise("initialpose", 10);
  ros::Rate loop_rate(1);
  //define 2d estimate pose
  double alpha = PI/2;//radian value
  double x_pos = 43.0231246948;
  double y_pos = 41.5323944092;

  while (ros::ok())
  {
    geometry_msgs::PoseWithCovarianceStamped pose_msg;

    pose_msg.header.stamp = ros::Time::now();
    pose_msg.header.frame_id = "map";
    pose_msg.pose.pose.position.x = x_pos;
    pose_msg.pose.pose.position.y = y_pos;
    pose_msg.pose.covariance[0] = 0.25;
    pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
    pose_msg.pose.pose.orientation.z = sin(alpha/2);
    pose_msg.pose.pose.orientation.w = cos(alpha/2);

    initial_pose_pub.publish(pose_msg);
    ROS_INFO("Setting to :(%f,%f)",x_pos,y_pos);
    ros::spinOnce();

    loop_rate.sleep();
  }

  return 0;
}

欢迎留言交流～