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消息格式：

https://blog.csdn.net/fengdehuhuan122/article/details/105019570/

（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定位：https://blog.csdn.net/weixin_43259286/article/details/106644615

詳細代碼參考：https://blog.csdn.net/fengdehuhuan122/article/details/105019570/  (cartographer_ros定位功能位姿擷取與重定位設定)