本文目的

本文主要记录调试运行demo中cartographer_paper_deutsches_museum.bag中代码的流程,不具体分析每个函数的功能而是理清调用关系.

首先cartographer ros入口

1.node_main.cc

在node_main中, 主要定义了一个Node node 和 map_builder, 这两个则主要就是为了链接cartographer_ros和cartographer. auto map_builder = cartographer::common::make_unique<:mapping::mapbuilder>(node_options.map_builder_options); Node node(node_options, std::move(map_builder), &tf_buffer); 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::spin(); // ros一直循环,直到ctrl+c或者ROS主线程退出才会执行后面的语句 node.FinishAllTrajectories(); node.RunFinalOptimization(); if (!FLAGS_save_state_filename.empty()) { node.SerializeState(FLAGS_save_state_filename); // 序列化 }

下面就主要从这node中进行分析如何一步步的从cartographer_ros到cartographer.

实际运行roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/home/lee/Downloads/cartographer_paper_deutsches_museum.bag之后除了获取一些配置文件信息之后最先输出的就是添加一条轨迹ID=0这一步是怎么来的呢,下面进行分析:

2. node.h && node.cc

主要功能就是wires up ROS topic to SLAM , 处理激光,里程计, IMU等传感器数据, 开始建图,结束建图,以及最后优化.

1, node.h

定义了几个关键变量基本都是map或者set类型, 表示一个轨迹一个相对应的变量 MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); std::map extrapolators_; std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set<:string> subscribed_topics_; std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_);

1, StartTrajectoryWithDefaultTopics 建立轨迹 void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); AddTrajectory(options, DefaultSensorTopics()); }

这里面有一个AddTrajectory函数主要功能:

trajectory_builder_interface.h

1, ComputeExpectedSensorIds:返回订阅的传感器集合, RANGE IMU ODOMETRY LANDMARK等 std::set<:mapping::trajectorybuilderinterface::sensorid>

2, map_builder_bridge_.AddTrajectory

map_builder_bridge

map_builder_bridge顾名思义就是cartographer_ros和cartographer中map_builder的一个桥梁,由它引入到cartographer的map_builder.c中, 第一幅图中打印的消息就是由LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";得到

3, AddExtrapolator

4,AddSensorSamplers

5,LaunchSubscribers

4. map_builder.cc int MapBuilder::AddTrajectoryBuilder(const std::set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback)

AddTrajectoryBuilder这个函数中包含3D和2D的部分,本文主要看2D的情况所以只提取2D中的代码段: { std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { local_trajectory_builder = common::make_unique(trajectory_options.trajectory_builder_2d_options(),SelectRangeSensorIds(expected_sensor_ids)); } DCHECK(dynamic_cast(pose_graph_.get())); trajectory_builders_.push_back(common::make_unique( sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), local_slam_result_callback))); if (trajectory_options.has_overlapping_submaps_trimmer_2d()) { const auto& trimmer_options = trajectory_options.overlapping_submaps_trimmer_2d(); pose_graph_->AddTrimmer(common::make_unique( trimmer_options.fresh_submaps_count(), trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options() .submaps_options() .grid_options_2d() .resolution()), trimmer_options.min_added_submaps_count())); } }

map_builder的主要功能就是综合TrajectoryBuilders (for local submaps) 和 a PoseGraph (for loop closure).从上面代码中可以看到几个变量和函数

1,local_trajectory_builder: LocalTrajectoryBuilder2D

2,trajectory_builders_ : std::vector<:unique_ptr>>

3,CollatedTrajectoryBuilder

4,CreateGlobalTrajectoryBuilder2D

5,pose_graph_: PoseGraph2D

接下来就需要对这几个进行分析了即进入到第二个部分cartographer.

cartographer模块

上面我们已经知道map_builder的功能就是同时处理前端和后端, 主要利用其中trajectory_builders_,一条轨迹就是一次建图的过程, 比如推着小车走一遍建立地图就是一轨迹, 如果需要再走一遍就可以再开启一条轨迹,ID++ trajectory_builders_.push_back(common::make_unique(sensor_collator_.get(), trajectory_id,expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), local_slam_result_callback)));

我们来扒皮抽筋

其中有一个CreateGlobalTrajectoryBuilder2D()函数, 一个CollatedTrajectoryBuilder类,我们可以逐一跟进

第一CreateGlobalTrajectoryBuilder2D()是为了返回一个GlobalTrajectoryBuilder std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback) { return common::make_unique< GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback); }

GlobalTrajectoryBuilder和CollatedTrajectoryBuilder都继承于TrajectoryBuilderInterface,其中在collated_trajectory_builder.h CollatedTrajectoryBuilder( sensor::CollatorInterface* sensor_collator, int trajectory_id, const std::set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder);

wrapped_trajectory_builder就通过为用子类指针赋值基类,使得最终调用为GlobalTrajectoryBuilder的方法,我们需要进入到这个类中进行分析

1. collated_trajectory_builder