设置定位中的初始位姿

使用cartographer的定位模式时， 机器人的初始位姿默认为建图时的起点位姿，机器人运动一段时间后，通过新局部子图与全局地图相匹配，以获取较为理想的位姿，故无需考虑机器人启动位姿，这个定位过程可以在rviz中观察到。

如果不在初始位姿附近，有可能不能定位成功，所以设置启动时的初始位姿，可以加速定位。 初始位姿不能像AMCL那样在Rviz里用箭头给定

定位模式载入的地图在submap中的trajectory id为0。开始定位后，系统默认从起点开始建立子图进行匹配，此时，该子图在submap中的trajectory id为1。要使用start_trajectory服务，必须先终止旧的trajectory为1的建图，调用rosservice call /finish_trajectory， cartographer_ros_msgs/srv/FinishTrajectory.srv如下：

int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

trajectory_id为1的路径结束，cartographer定位暂停。

然后再开始设定的初始位姿的建图，调用rosservice call /start_trajectory，cartographer_ros_msgs/srv/StartTrajectory.srv如下：

cartographer_ros_msgs/TrajectoryOptions options
cartographer_ros_msgs/SensorTopics topics
---
cartographer_ros_msgs/StatusResponse status
int32 trajectory_id

此时建立的子图在submap中的 trajectory为2

更方便的方法是修改源码cartographer_ros/start_trajectory_main.cc，加入#include"cartographer_ros_msgs/FinishTrajectory.h"，在Run()函数开头部分中加入：

ros::Duration(0.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);

调用start_trajectry的前提是location的node已经启动，在实际中由于硬件计算能力的不同，node的启动速度也不同，给予start_trajectory一个延时可以保证程序正常运行，在此设定0.3秒

编译完成后，在home_no_odom_localization.launch中添加：

<node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory"
      args= "
      -configuration_directory $(find cartographer_ros)/configuration_files
      -configuration_basename home_localization.lua
      -initial_pose {to_trajectory_id=0,relative_pose={translation={0.27,1.12,-0.146},rotation={0,0,1.486,timestamp=0}}}"/>

initial_pose 后面有空格，但大括号内不能有一个空格，否则运行时lua报错

重点是initial_pose部分： 位姿 is relative to trajectory 0的原点，这是参数to_trajectory_id的作用。ID只能是地图当前所用的，一般是0，最好用/get_trajectory_states服务检查；

relative_pose就是要指定的位姿；timestamp=0非常重要，如果不写这个timestamp，cartographer会默认为当前的时间，结果传入的pose会根据时间做一个位姿变化；rotation是欧拉角（弧度）。

机器人将以initial_pose为起始位姿开辟一条新的trajectory，其id为2。 查看trajectory_node_list话题的ns属性，获知当前运行的trajectory_id。 把机器人开到初始位姿附近，启动launch之后，很快就能定位成功。如果机器人启动的位姿离给定的太远，那么定位过程跟从原点寻找差不多

对于新版本的cartographer_ros，执行

rosservice call /start_trajectory "configuration_directory: '' configuration_basename: '' use_initial_pose: false initial_pose: position: {x: 0.0, y: 0.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} relative_to_trajectory_id: 0"

如果有2条trajectory，也就是建过2次图，此时再纯定位，添加的 trajectory id是3. 仍然是上面的步骤，但是机器人位姿会快速跳动，需要让机器人移动以成功定位。

小强机器人使用 cartographer 在2条trajectory下的定位

要加速定位，需要调整参数POSE_GRAPH.constraint_builder.sampling_ratio， 增大POSE_GRAPH.global_sampling_ratio

纯定位的过程

补充

目前从SLAM里判断是否定位成功是不可能的。

一次纯定位时，无论怎么调整参数都失败，重启机器人后正常了。