cartographer的localization模式设置初始位姿

    cartographer的localization模式机器人的初始位姿默认为建图时的起点位姿。理论上机器人运动一段时间后通过新局部子图与全局地图相匹配能获取较为理想的位姿，故无需考虑机器人启动位姿。如果能知道机器人启动时的初始位姿，对机器人的导航还是有所帮助。故在此还是提供一个设置初始位姿的方法。

    主要调用了github上作者们提供的initial_pose的接口。个人的理解可能不够透彻，方法绕了个圈子。期待未来能获得更简单的方法。作者们的issue：

    https://github.com/googlecartographer/cartographer/pull/606

    https://github.com/googlecartographer/cartographer/pull/642

    一、调用思路。

    localization载入的地图在submap中的trajectory为0。启动location，系统默认从起点开始建立子图进行匹配，此时，该子图在submap中的trajectory为1。为了使机器人在给定的初始位姿建图匹配，应该先终止系统默认的建图（调用rosservice call /finish_trajectory，具体参数参见cartographer_ros_msgs/srv/FinishTrajectory.srv），终止trajectory为1的建图。然后再开始设定好初始位姿的建图（调用rosservice call /start_trajectory，具体参数参见cartographer_ros_msgs/srv/StartTrajectory.srv），此时建立的子图在submap中的trajectory为2。

    二、调用实现。

    在程序上对cartographer_ros的源码做一些改动。

    改动位于/cartographer_ros/start_trajectory_main.cc中：

   （1）加入#include"cartographer_ros_msgs/FinishTrajectory.h"；

   （2）在run中加入终止当前建子图的代码。

bool Run() {
  ros::Duration(3).sleep();
  ros::NodeHandle node_handle;
  
  ros::ServiceClient finish_trajiectry_client =
      node_handle.serviceClient<cartographer_ros_msgs::FinishTrajectory>(
          kFinishTrajectoryServiceName);
  cartographer_ros_msgs::FinishTrajectory f_srv;
  f_srv.request.trajectory_id = 1;
  finish_trajiectry_client.call(f_srv);
  
  ros::ServiceClient client =
      node_handle.serviceClient<cartographer_ros_msgs::StartTrajectory>(
          kStartTrajectoryServiceName);
  cartographer_ros_msgs::StartTrajectory srv;

...

}

    catkin_make_isolated --install --use-ninja编译使改动生效。

    在此需要说明的是调用start_trajectry的前提是location的node已经启动，在实际中由于硬件计算能力的不同，node的启动速度也不同，给予start_trajectory一个时延可以较好的保证程序正常运行，时延值的大小根据实际情况调节。上述程序中为3s。

    三、调用方法。

    在launch文件中调用cartographer_start_trajectory的node。node的几个参数：

    1、-configuration_directory：lua配置文件存放的路径（注意为文件夹路径）；

    2、-configuration_base_name：定位所使用的lua配置文件名（注意仅为文件名）；

    3、-initial_pose：设置初始位姿，to_trajectory_id=0不应改变。具体位置设定于relative_pose。

    一个例子如下（设定初始位姿为建图起点位姿）：

   <node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory" 
      args= "
      -configuration_directory /home/cabin/cartographer_0_3_0_ws/src/cartographer_ros/cartographer_ros/configuration_files 
      -configuration_basename backpack_2d_xiaobao_localization.lua 
      -initial_pose {to_trajectory_id=0,relative_pose={translation={0.0,0.0,0.},rotation={0.,0.,0.,}}}" />

    catkin_make_isolated --install --use-ninja编译使改动生效。

    之后运行launch文件进行定位即可。