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

2023-07-05 12:43

短信预约 -IT技能 免费直播动态提醒

本篇内容介绍了“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 );  }}}  // 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

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中:

// 工厂函数,生成接口APIstd::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 );  }

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

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