1.Cartographer文件树
cartographer算法包括cartographer
和cartographer_ros
两部分,其中cartographer
为算法的核心实现,cartographer_ros
为算法和ROS的对接的桥梁,方便用户使用。
.
├── cartographer
│ ├── bazel
│ │ └── third_party
│ ├── cartographer
│ │ ├── cloud
│ │ ├── common
│ │ ├── ground_truth
│ │ ├── io
│ │ ├── mapping
│ │ ├── metrics
│ │ ├── sensor
│ │ └── transform
│ ├── cmake
│ │ └── modules
│ ├── configuration_files
│ ├── docs
│ │ ├── assets
│ │ └── source
│ └── scripts
└── cartographer_ros
├── cartographer_ros
│ ├── cartographer_ros
│ ├── configuration_files
│ ├── launch
│ ├── scripts
│ └── urdf
├── cartographer_ros_msgs
│ ├── msg
│ └── srv
├── cartographer_rviz
│ ├── cartographer_rviz
│ └── ogre_media
├── docs
│ └── source
└── scripts
2.Cartographer初始化
- 从
src/cartographer_ros/cartographer_ros/launch/backpack_2d.launch
进入,可以看到cartographer_node
节点的启动
<launch>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
- 可以发现
cartographer_node
是在src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc
中创建的,这里主要关注函数cartographer_ros::Run()
。
int main(int argc, char** argv) {
// note: 初始化glog库
google::InitGoogleLogging(argv[0]);
// 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
// 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
google::ParseCommandLineFlags(&argc, &argv, true);
/**
* @brief glog里提供的CHECK系列的宏, 检测某个表达式是否为真
* 检测expression如果不为真, 则打印后面的description和栈上的信息
* 然后退出程序, 出错后的处理过程和FATAL比较像.
*/
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
// ros节点的初始化
::ros::init(argc, argv, "cartographer_node");
// 一般不需要在自己的代码中显式调用
// 但是若想在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等, 可以显式调用该函数.
::ros::start();
// 使用ROS_INFO进行glog消息的输出
cartographer_ros::ScopedRosLogSink ros_log_sink;
// 开始运行cartographer_ros
cartographer_ros::Run();
// 结束ROS相关的线程, 网络等
::ros::shutdown();
}
- 在
cartographer_ros::Run()
函数中创建了map_builder
和node
,node
控制着整个系统,而map_builder
如名字一样,用于地图的构建,另外map_builder
这个变量移动到MapBuilderBridge
中的std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_
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配置文件中的内容, 为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);
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
......
}
- 从
node
的构造开始,初始的流程如图所示