系统入口——cartographer_node
在上一章中,我们了解到实际与定位建图相关的节点只有cartographer_node, 而其入口main函数则定义在node_main.cc中。 让我们先直接拉到该文件的底部,查看它的main函数。
1. main函数
如下面的代码片段所示,在main函数的一开始先初始化了谷歌的日志系统glog, 并通过gflags解析运行cartographer_node时的运行参数。
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
然后检查是否在运行参数中指定了配置文件和目录,若没有程序会报错退出。
CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing.";
接着在完成ROS系统的初始化工作后,调用函数Run()开始进行定位建图,最后如果程序正常退出则关闭ROS系统。目前没有细查下面第8行中的ros_log_sink的作用,它作为一个局部变量定义在这里似乎没什么意义。
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}
2. 运行参数
我们已经看到cartographer_node使用gflags解析运行参数,在使用函数ParseCommandLineFlags初始化运行参数之前,需要先通过gflags的宏来定义参数对象。 我们回到node_main.cc文件开始的地方,可以看到这几个参数对象的定义。
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, second is always the Cartographer installation to allow including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the configuration file.");
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file to load, containing a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(save_state_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
gflags支持多种不同的数据类型,通过DEFINE_type形式的宏来体现。支持的数据类型有bool, string, int32, int64, uint64, double等。 上面的代码片段中通过DEFINE_string和DEFINE_bool定义了string和bool类型的参数。这类的宏有三个参数,一次是运行参数名、缺省值、帮助信息。
当参数定义之后,就可以通过FLAGS_param_name的形式访问对应的参数了。比如刚刚在main函数中,通过FLAGS_configuration_directory获取配置文件所在的目录, FLAGS_configuration_basename记录了配置文件的名字。这里的参数对象的定义和使用都是在同一个源文件中的没有什么特殊的操作,但是如果需要在其他文件中使用这里的参数对象, 还需要先通过宏DECLARE_type来声明这些参数对象,再使用。
关于gflags的介绍就到这里,更详细的内容可以参考官方文档。
3. 业务逻辑
刚才的main函数告诉我们cartographer_node通过函数Run来完成所有的定位和建图的逻辑。下面是该函数的片段:
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
首先使用tf2定义了ROS的坐标系统监听器,它用于监听ROS系统中发布的坐标变换(tf)消息,并将之缓存在tf_buffer中。 kTfBufferCacheTimeInSeconds描述了缓存的时间长度。
tf是ROS系统中常用的坐标变换库,它通过订阅和发布坐标变换消息来维护系统的坐标系。 我们在机器人操作系统——Robot Operating System(ROS)系列文章中介绍过早期的tf广播者和监听者的使用方式。目前ROS已经不推荐使用tf库了,它们在推广tf2的使用,但其基本思想都是一样的,而且同一个系统中还是可以运行两种库的。
接着函数Run调用定义在node_options.cc中的函数LoadOptions加载配置文件。该函数的两个参数分别是我们在刚刚提到的配置文件目录和名称的运行参数。关于配置文件以及解析方式的源码我们将有一篇文章专门介绍。
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
然后构建了建图器map_builder和ROS封装对象node。
auto map_builder = cartographer::common::make_unique<cartographer::mapping::MapBuilder>(node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer);
如果在运行cartographer_node的时候通过命令行参数load_state_filename指定了保存有SLAM状态的文件,那么就加载该状态文件继续运行。 目前还猜不出来参数start_trajectory_with_default_topics的作用,根据名字和参数定义时的帮助消息来看,大致意思是使用默认的主题直接开始第一条轨迹的跟踪。 我们将在后面分析Node对象的时候再来深究它的功能。
if (!FLAGS_load_state_filename.empty())
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
if (FLAGS_start_trajectory_with_default_topics)
node.StartTrajectoryWithDefaultTopics(trajectory_options);
完成上述的初始化和必要的变量定义之后,就可以开启ROS的逻辑循环了,node对象将通过消息回调多线程等机制完成整个定位建图过程。
::ros::spin();
如果在ROS的逻辑循环过程中,触发了退出机制,Run函数就会接着执行如下的语句,完成最终的路径和地图的优化。
node.FinishAllTrajectories();
node.RunFinalOptimization();
最后如果运行参数要求保存系统状态,则将当前的系统状态存到参数save_state_filename所指定的文件中。
if (!FLAGS_save_state_filename.empty())
node.SerializeState(FLAGS_save_state_filename);
}
4. 完
通过对源文件node.cc的分析,我们猜测对象node实际控制着系统的业务逻辑,而对象map_builder则用于完成建图。