【移动机器人技术】cartographer定位模式下的功能开发（二）：订阅/initialpose

google开发的cartographer开源包，既可以用来做SLAM建图，又可以用来做纯定位。
在实时定位工作过程中，若从外部信息获取到了机器人的准确位姿，如何像move_base中的amcl模块一样，通过订阅/initialpose话题达到修正定位结果的目的呢？
本文记录了整个操作流程。

2 实时修正定位位姿

2.1 编写回调函数

在文件node_main.cc的头部编写回调函数：
函数中订阅了/initialpose后，结束了当前活动的轨迹，设置轨迹配置选项中的起点为话题值，重新开启节点。

void Reset_InitPose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) {

  // 关闭当前运行的Trajectories
  node_handle->FinishAllTrajectories();

  // 给轨迹设置起点 msg->pose.pose
  // start trajectory with initial pose
  *trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose()
    = cartographer::transform::ToProto(cartographer_ros::ToRigid3d(msg->pose.pose));

  // 重新开启Trajectory
  if (FLAGS_start_trajectory_with_default_topics) 
  {
    node_handle->StartTrajectoryWithDefaultTopics(*trajectory_options_handle);
  }
}

提示：如编译报错，提示“ error: ‘ToRigid3d’ is not a member of ‘cartographer_ros’ ”，则增加头文件包含，#include “cartographer_ros/msg_conversion.h”。

2.2 订阅/initialpose

• 首先定义全局变量
  在文件node_main.cc头部位置

cartographer_ros::Node* node_handle;
cartographer_ros::TrajectoryOptions* trajectory_options_handle;

• 初始化全局变量
  在文件node_main.cc的void Run()中：

trajectory_options_handle = &(trajectory_options);
node_handle = &(node);

• 订阅话题
  在文件node_main.cc的void Run()函数中添加如下代码：

ros::Subscriber initPose_sub = node.node_handle()->subscribe("/initialpose", 1, Reset_InitPose_callback);