本篇内容介绍了“C++ Cartographer的入口node main源码分析”的有关知识，在实际案例的操作过程中，不少人都会遇到这样的困境，接下来就让小编带领大家学习一下如何处理这些情况吧！希望大家仔细阅读，能够学有所成！
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_rosRun函数主要做了一下几件事：
读取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类的作用主要是传感器数据的获取和处理,让数据与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源码分析”的内容就介绍到这里了，感谢大家的阅读。如果想了解更多行业相关的知识可以关注亿速云网站，小编将为大家输出更多高质量的实用文章！
免责声明：本站发布的内容（图片、视频和文字）以原创、转载和分享为主，文章观点不代表本网站立场，如果涉及侵权请联系站长邮箱：is@yisu.com进行举报，并提供相关证据，一经查实，将立刻删除涉嫌侵权内容。
