【移动机器人技术】cartographer定位模式下的功能开发（一）：设置起始位姿

google开发的cartographer开源包，既可以用来做SLAM建图，又可以用来做纯定位。
在纯定位工作模式下，默认机器人的起点位姿位map坐标系原点位姿，如果机器人实际放置在一个固定的起始点，且此点不在map系原点，该如何操作呢？
在实时定位工作过程中，若从外部信息获取到了机器人的准确位姿，如何像move_base中的amcl模块一样，通过订阅/initialpose话题达到修正定位结果的目的呢？
本文记录了整个操作流程。

1 初始位姿的设置

本方法较笨拙，但可以实现功能。

1.1 在launch文件中设置初始位姿参数

  <param name="/set_inital_pose_x" type="double" value = "0.991371298387"/>
  <param name="/set_inital_pose_y" type="double" value = "0.011122568834"/>
  <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.0123087794108"/>
  <param name="/set_inital_pose_ow" type="double" value = "-0.999924244105"/>

注：参数名称前的“/”符号，表示该参数为全局变量，本launch文件启动的所有node都已使用。

1.2 在node文件中使用配置参数

在文件node_main.cc头部编写rosparam参数获取函数：

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;
}

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

// 获取轨迹配置项指针
trajectory_options_handle = &(trajectory_options);

// 获取配置参数
ros::NodeHandle pnh("~");
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;
    
//更改轨迹配置项中的初始位姿值
*trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose()
    = cartographer::transform::ToProto(cartographer_ros::ToRigid3d(init_pose));

以下是源码中加载配置参数并启动新轨迹的代码：

// 开启Trajectory
if (FLAGS_start_trajectory_with_default_topics) {
  node.StartTrajectoryWithDefaultTopics(trajectory_options);
}

经过以下操作可以完成初始位姿的配置。

1.3 错误初始位姿配置案例

有些博文介绍了以下这个错误的配置方式：

• 在launch文件中启动cartographer_node节点的参数arg选项中增加一条：

-set_inital_pose {to_trajectory_id=0,relative_pose={translation={0.0,0.0,0.},rotation={0.,0.,2.75,timestamp=0}}}

• 在node_options.h,node_options.cc中增加函数实现带初始位姿配置

std::tuple<NodeOptions, TrajectoryOptions> LoadOptions_with_initpose(
    const std::string& configuration_directory,
    const std::string& configuration_basename,
    const std::string& init_pose_filename) {

    auto file_resolver = cartographer::common::make_unique<
    cartographer::common::ConfigurationFileResolver>(
    std::vector<std::string>{configuration_directory});
    const std::string code =
    file_resolver->GetFileContentOrDie(configuration_basename);

//   std::string initial_pose =
//     file_resolver->GetFileContentOrDie(init_pose_filename);
    std::string initial_pose = init_pose_filename;
    
    auto lua_parameter_dictionary =
    cartographer::common::LuaParameterDictionary::NonReferenceCounted(
          code, std::move(file_resolver));

    auto initial_trajectory_pose_file_resolver =
        cartographer::common::make_unique<
            cartographer::common::ConfigurationFileResolver>(
            std::vector<std::string>{configuration_directory});
    auto initial_trajectory_pose =
        cartographer::common::LuaParameterDictionary::NonReferenceCounted(
            "return " + initial_pose,
            std::move(initial_trajectory_pose_file_resolver));

  return std::make_tuple(CreateNodeOptions(lua_parameter_dictionary.get()),
                              CreateTrajectoryOptions(lua_parameter_dictionary.get(),
                                   initial_trajectory_pose.get()));
}

• 在node_main.cc中调用此配置函数
  (1) 在文件头部分加入参数定义
  DEFINE_string(
  set_inital_pose, “”,
  “set a initial pose of new trajectory.”);
  (2) 在文件Run()函数中调用

if (!FLAGS_set_inital_pose.empty()) {
    std::tie(node_options, trajectory_options) =
   	 LoadOptions_with_initpose(FLAGS_configuration_directory, FLAGS_configuration_basename, FLAGS_set_inital_pose);
}
else
{
	std::tie(node_options, trajectory_options) =
   	 LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
}

通过以下操作，设置的是trajectory 0与trajectory 1的相对位姿关系，即纯定位的新建轨迹起点位姿与已经建图完毕的冻结状态的轨迹位姿关系。这个位姿关系的设定，会影响下面介绍的第二项功能的实现。