设置定位中的初始位姿
使用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
如下:
1 | int32 trajectory_id |
trajectory_id为1的路径结束,cartographer定位暂停。
然后再开始设定的初始位姿的建图,调用rosservice call /start_trajectory
,cartographer_ros_msgs/srv/StartTrajectory.srv
如下:
1 | cartographer_ros_msgs/TrajectoryOptions options |
此时建立的子图在submap中的 trajectory为2
更方便的方法是修改源码cartographer_ros/start_trajectory_main.cc
,加入#include"cartographer_ros_msgs/FinishTrajectory.h"
,在Run()
函数开头部分中加入:
1 | ros::Duration(0.3).sleep(); |
调用
start_trajectry
的前提是location的node已经启动,在实际中由于硬件计算能力的不同,node的启动速度也不同,给予start_trajectory
一个延时可以保证程序正常运行,在此设定0.3秒
编译完成后,在home_no_odom_localization.launch
中添加:
1 | <node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory" |
重点是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,执行
1 | 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里判断是否定位成功是不可能的。
一次纯定位时,无论怎么调整参数都失败,重启机器人后正常了。