C++ Cartographer的入口node main详细讲解 / 张生荣

啃一下谷歌优秀的激光SLAM开源框架-Cartographer. 这个框架算法简单,但是程序部分太多需要学习的地方了.不论是整体框架的结构,还是数据的使用,都是非常优美的.不愧是大公司啊.接下来记录一下每天学习的内容和心得,督促自己坚持下去!

node_main.cc是整个Cartographer程序的入口，用来调用整个Cartographer进程。以最基础的单线雷达和轮速计为例。

整体的代码开始是在Run函数中实现的。

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详细讲解的文章就介绍到这了,更多相关C++ node_main内容请搜索我们以前的文章或继续浏览下面的相关文章希望大家以后多多支持我们！

C++ Cartographer的入口node main详细讲解

目录

Run函数

读取配置参数

构建地图构建器

Node类的初始化

开始轨迹与结束轨迹

相关推荐

随机推荐