【Cartographer魔改】cartographer定位模式下设置起始位姿——快速重定位

【Cartographer魔改】cartographer定位模式下设置起始位姿——快速重定位

在使用cartographer的纯定位工作模式下，默认机器人的起点位姿位map坐标系原点位姿。
如果当机器人启动定位launch时不在初始位置附近，cartograpeher默认从map坐标系原点位姿附近开始搜索，当地图较大时重定位会花费较多时间，甚至失败。
因此，本文通过修改cartograpeher源码的方式，修改定位模式的初始位姿，从而达到快速重定位的目的。
主要借鉴了博文： https://blog.csdn.net/weixin_43259286/article/details/106643625
的方法，但其中有些小错误，对此稍作修改。

1.设置初始位姿

把大象装进冰箱，总共需要三步：

1.1 修改源码

在cartographer_ros/cartographer_ros/cartographer_ros文件夹下找到node_main.cc，在其头部添加头文件和rosparam参数获取函数：

#include "cartographer_ros/msg_conversion.h"

template<typename T>
T getParam_Func(ros::NodeHandle& pnh,
                    const std::string& param_name, const T & default_val)
{
  T param_val;
  pnh.param<T>(param_name, param_val, default_val);
  return param_val;
}

这一部分主要是获取launch文件中的参数。

然后，在void Run()函数中，在 Node node(node_options, std::move(map_builder), &tf_buffer,FLAGS_collect_metrics);
这一行下面添加如下代码：

// 获取轨迹配置项指针
auto trajectory_options_handle = &(trajectory_options);  //原博文这里少了个auto
ros::NodeHandle pnh("~");
// 获取配置参数
bool localization = getParam_Func<bool>(pnh, "/localization", 0);      //是否为纯定位模式
double pos_x = getParam_Func<double>(pnh, "/set_inital_pose_x", 0.0); 
double pos_y = getParam_Func<double>(pnh, "/set_inital_pose_y", 0.0); 
double pos_z = getParam_Func<double>(pnh, "/set_inital_pose_z", 0.0); 
double pos_ox = getParam_Func<double>(pnh, "/set_inital_pose_ox", 0.0); 
double pos_oy = getParam_Func<double>(pnh, "/set_inital_pose_oy", 0.0); 
double pos_oz = getParam_Func<double>(pnh, "/set_inital_pose_oz", 0.0); 
double pos_ow = getParam_Func<double>(pnh, "/set_inital_pose_ow", 1.0); 
geometry_msgs::Pose init_pose;
init_pose.position.x = pos_x;
init_pose.position.y = pos_y;
init_pose.position.z = pos_z;
init_pose.orientation.x = pos_ox;
init_pose.orientation.y = pos_oy;
init_pose.orientation.z = pos_oz;
init_pose.orientation.w = pos_ow;

if(localization == true)
{
//更改轨迹配置项中的初始位姿值
*trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose()
    = cartographer::transform::ToProto(cartographer_ros::ToRigid3d(init_pose));
}

这一部分为更改初始位姿，pos_x … pos_ow分别对应初始位姿的x,y,z和四元数。bool localization 判断是否为纯定位模式，原博文中没有此项，但若不添加这一判断会导致建图时报错。

1.2 修改launch文件

在launch文件头部添加：

  <param name="/localization" type="bool" value = "1"/>
  <param name="/set_inital_pose_x" type="double" value = "0"/>
  <param name="/set_inital_pose_y" type="double" value = "0"/>
  <param name="/set_inital_pose_z" type="double" value = "0.0"/>
  <param name="/set_inital_pose_ox" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oy" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oz" type="double" value = "0"/>
  <param name="/set_inital_pose_ow" type="double" value = "1"/>

其中把x,y,z,ox,oy,oz,ow改成你想设定的初值即可。这里由于是纯定位模式的launch文件，故设置localization为1，在建图的launch文件中添加

 <param name="/localization" type="bool" value = "0"/>

即可。

1.3 编译

cd到cartographer的文件夹下，catkin_make_isolated --install --use-ninja.

亲测如果给定一个与真实位置相差不大的初值，重定位会比不修改快很多。如果地图很大的话或许会更明显。

参考：https://blog.csdn.net/weixin_43259286/article/details/106643625