C++ Cartographer的入口node main源码分析

本篇内容介绍了“C++ Cartographer的入口node main源码分析”的有关知识，在实际案例的操作过程中，不少人都会遇到这样的困境，接下来就让小编带领大家学习一下如何处理这些情况吧！希望大家仔细阅读，能够学有所成！

Run函数

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 开启监听tf的独立线程
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  // c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
  // 读取Lua文件内容,把Lua文件内容给到node_options和trajectory_options
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中实现，工厂函数
                                                                                //在这里,实例化一个MapBuilder, 而MapBuilder是MapBuilderInterface的子类                                                                             //MapBuilder的AddTrajectoryBuilder实例化了CollatedTrajectoryBuilder 
  // c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象, 
  // 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..
  // 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象, 
  // 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
  // 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.
  // Node类的初始化, 开启订阅,发布topic和service,将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
  // 如果加载了pbstream文件, 就执行这个函数,为定位
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }
  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }
  ::ros::spin();
  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();
  // 当所有的轨迹结束时, 再执行一次全局优化
  node.RunFinalOptimization();
  // 如果save_state_filename非空, 就保存pbstream文件
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }
}
}  // namespace
}  // namespace cartographer_ros

Run函数主要做了一下几件事：

• 读取Lua配置文件中的内容，确定节点构造的方式和轨迹构造的方式与参数。

• 实例化map_builder，map_builder是完整的SLAM算法类，包含了前端和后端。具体时间方式是通过工厂模式。

• 初始化Node，通过初始化Node，开启订阅，发布topic与service，还将topic带的传感器数据传入MapBuilder。

• 判断是否为定位还是建图，并开启轨迹

• 死循环，不停地接受topic并运行Cartographer

• 结束时停止所用传感器数据的订阅，并且执行一次全局优化，保存pbstream地图文件

读取配置参数

其中std::tie很有意思,可以实现多个不同类型的返回值. 很多时候我们想通过一个函数丢出去多个结果,但一个函数只能有一个返回值,于是我们可以用std::make_tuple把多个返回值打包成std::tuple类型的数据,这时候返回值只是tuple类型了,所以没有违反只能返回一个返回值的规定.这点很类似Python中的pickle和tuple,啥都可以装在一起丢出去. 实现文件在node_options.cc

/**
 * @brief 加载lua配置文件中的参数
 * 
 * @param[in] configuration_directory 配置文件所在目录
 * @param[in] configuration_basename 配置文件的名字
 * @return std::tuple<NodeOptions, TrajectoryOptions> 返回节点的配置与轨迹的配置
 */
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
    const std::string& configuration_directory,
    const std::string& configuration_basename) {
  // 获取配置文件所在的目录
  auto file_resolver =
      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
          std::vector<std::string>{configuration_directory});
  // 读取配置文件内容到code中
  const std::string code =
      file_resolver->GetFileContentOrDie(configuration_basename);
  // 根据给定的字符串, 生成一个lua字典
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
      code, std::move(file_resolver));
  // 创建元组tuple,元组定义了一个有固定数目元素的容器, 其中的每个元素类型都可以不相同
  // 将配置文件的内容填充进NodeOptions与TrajectoryOptions, 并返回
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));
}

构建地图构建器

Cartographer_ros和Cartographer是两个部分,一个是数据处理与分配,一个才是真正的Cartographer算法代码的部分,代码上把ros和算法库分得很开,让我们移植和开发很容易.那么如何让ros数据和Cartographer算法建立联系呢?第一步就是地图构建器.

地图构建器的大致作用是调用Cartographer的算法.

地图构建器通过配置文件中node_options中map_builder_options部分去初始化一个地图.这个地图构建器的作用以后再说.先来看看他是怎么实现的.

由node_main.cc调用map_builder中的CreateMapBuilder函数,这个函数只有一个参数,就是上一行从lua中读取的配置文件内容. 进入map_builder.cc中:

// 工厂函数,生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

发现这个就是一个接口函数. 但这个函数也有用到一些cpp的技巧,值得学习:

返回值是一个unique_ptr的MapBuilder类型的类,而返回类型却定于为MapBuilder的父类MapBuilderInterface类,这在cpp中是允许的,而且这样做更能让返回值类型更加有包容性,实现工厂模式.

MapBuilder这个类是SLAM算法的入口类十分重要,用来初始化pose_graph,创建轨迹等.会在另一篇中详细介绍.

Node类的初始化

Node类的作用主要是传感器数据的获取和处理,让数据与MapBuilder构建联系,从而使获取的raw sensor data能够灌入Cartographer算法库,实现定位建图等功能.

在node_main.cc中初始化方式如下:

  // Node类的初始化, 开启订阅,发布topic和service,将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

这一行代码也有值得学习的地方,就是std::move这个函数,他通过把某个实例化的类变为右值引用然后直接转移给某个对象,从而实现高效的"转移".

举个简单的不太恰当的例子,你想要我的西瓜,有两种方式,一个是我不远千里坐车给你,还有一种是给西瓜贴上你的名字,别人问我就说我说了不算,问你去. std::move就是后者(如有错请指出哈).所以这样可以直接从一个对象转移到另一对象(贴名字),取消了不必要的临时对象的创建拷贝与销毁(运输西瓜需要位子还要搬上搬下). 对于占用很大的类的转移就很节约开销(一亿吨西瓜咋运啊).大致就这个意思.

Node类的内容在node.cc中,主要作用是实现传感器数据的订阅发布以及初始处理, 以及传递给mapbuilder.具体内容在后面会详细介绍.

开始轨迹与结束轨迹

在上面实例化了Node类之后,我们就可以调用node中的方法去建图. 建图就不用加载地图了,毕竟是建图,所以直接调用node开始轨迹,然后在进入ros中的死循环,不停地接受新的数据,处理并运算,输出结果, 直到按下ctrl+c去终止程序,跳出死循环,执行结束输入数据和进行最终优化.

其实看程序就可以知道,Cartographer的建图和定位是一样的,只是建图的时候不加载地图并且在结束的时候保存地图,定位的时候加载地图,可以不保存地图,也可不进行最终优化.其实我测试的不进行最终优化也是可以的,毕竟定位是实时的,就算最终优化使之前的定位结果有变化,机器人也回不去了.所以我认为是可以去掉的.

  // 如果加载了pbstream文件, 就执行这个函数,为定位
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }
  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }
  ::ros::spin();
  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();
  // 当所有的轨迹结束时, 再执行一次全局优化
  node.RunFinalOptimization();
  // 如果save_state_filename非空, 就保存pbstream文件
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }

LoadState作用是加载地图文件.这个地图不同于可以可视化的地图,这个地图里面包含了位姿图pose_graph,传感器数据和landmark_pose等其他信息,不单单是一个地形图一样的地图.调用的最终函数是Cartographer算法部分的map_builder.cc中的同名函数,调用流程一环套一环(Cartographer整体框架就是这样,复杂但都是必要的).调用的流程如下:

只有最后一层的map_builder.cc才是Cartographer算法部分的内容,才是真正实现加载地图的功能. 这部分程序又臭又长,大家可以自己看看,实现功能加载posegraph和旧地图的传感器数据与landmark.

StartTrajectoryWithDefaultTopics实际上是调用了node.cc的AddTrajectory,去让map_builder创建一个轨迹,并且新增位姿估计器,传感器数据采样器,订阅topic以及调用回调函数的功能. 这个函数建立了数据与算法的统一. 详细会在Node中解析.

FinishAllTrajectories调用node.cc中的FinishTrajectoryUnderLock去结束传感器订阅,然后调用map_builder的FinishTrajectory()进行轨迹的结束

node::RunFinalOptimization调用map_builder的pose_graph的RunFinalOptimization实现结束建图后所有位姿图的最终优化.

由此可见, Node类通过类方法,实现了传感器数据的处理与使用.具体的方式是用了sensor_bridge和map_builder_bridge,把传感器数据转换并且给了Cartographer的算法部分, 实现了建图与定位.

“C++ Cartographer的入口node main源码分析”的内容就介绍到这里了，感谢大家的阅读。如果想了解更多行业相关的知识可以关注亿速云网站，小编将为大家输出更多高质量的实用文章！