C++实现RVIZ 2D POSE ESTIMATE 功能设置机器人初始坐标

关于rviz中小车初始点的设置问题
一般有两种方法：
1打开rviz 在其上方工具栏中有2D Pose estimate，用来设置大概的初始点
2一般在amcl.launch文件中也会定义初始点，大多设为0 0 0

对于方法1
ros官网上是这么介绍的
When starting up, the TurtleBot does not know where it is. To provide it its approximate location on the map: Click the “2D Pose Estimate” button Click on the map where the TurtleBot approximately is and drag in the direction the TurtleBot is pointing. You will see a collection of arrows which are hypotheses of the position of the TurtleBot. The laser scan should line up approximately with the walls in the map. If things don’t line up well you can repeat the procedure.

注意 如果不设置比较准确的初始点的话。如下图，会导致导航过程中rviz中的小车和仿真（gazebo）中小车位置偏差很大，甚至最后都到不了目标点。

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

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

cartographer_ros定位功能位姿获取与重定位设置_datou_zjq的博客-CSDN博客

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

~$ 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发出的数据格式

~$ 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<geometry_msgs::PoseWithCovarianceStamped>("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;
}

5.rviz重定位设置

订阅/initialpose话题

rviz中的“2D Pose Estimate”可发布/initialpose话题，通过点击地图位置可以发布相应位置的topic，包括x,y和theta。

_pose_init_sub = nh.subscribe("/initialpose", 1000, &NavNode::init_pose_callback, this);

重定位设置

重定位功能通过调用API设置，参考API。

void NavNode::init_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;
    double theta = tf2::getYaw(msg->pose.pose.orientation);
    ros::NodeHandle nh;

    ros::ServiceClient client_traj_finish = nh.serviceClient<cartographer_ros_msgs::FinishTrajectory>("finish_trajectory");
    cartographer_ros_msgs::FinishTrajectory srv_traj_finish;
    srv_traj_finish.request.trajectory_id = traj_id;
    if (client_traj_finish.call(srv_traj_finish))
    {
        ROS_INFO("Call finish_trajectory %d success!", traj_id);
    }
    else
    {
        ROS_INFO("Failed to call finish_trajectory service!");
    }

    ros::ServiceClient client_traj_start = nh.serviceClient<cartographer_ros_msgs::StartTrajectory>("start_trajectory");
    cartographer_ros_msgs::StartTrajectory srv_traj_start;
    srv_traj_start.request.configuration_directory = "xxx";//.lua文件所在路径
    srv_traj_start.request.configuration_basename = "xxx.lua";//lua文件
    srv_traj_start.request.use_initial_pose = 1;
    srv_traj_start.request.initial_pose = msg->pose.pose;
    srv_traj_start.request.relative_to_trajectory_id = 0;
    if (client_traj_start.call(srv_traj_start))
    {
        // ROS_INFO("Status ", srv_traj_finish.response.status)
        ROS_INFO("Call start_trajectory %d success!", traj_id);
        traj_id++;
    }
    else
    {
        ROS_INFO("Failed to call start_trajectory service!");
    }
}

Cartographer定位：【移动机器人技术】cartographer定位模式下的功能开发（二）：订阅/initialpose_A MAN NAMED MAGIC的博客-CSDN博客

详细代码参考：cartographer_ros定位功能位姿获取与重定位设置_datou_zjq的博客-CSDN博客  (cartographer_ros定位功能位姿获取与重定位设置)