描述
这段时间的任务一直是阅读cartographer,这篇主要分析cartographer的代码运行顺序,再好好理清一下cartographer的运行逻辑
顺序
我在代码中cout一些信息,帮助我们梳理一下代码的运行时的进进出出
这里会梳理出全部的关键程序语句
-
node_main.cc
开始运行launch文件后,就进入Run()函数
功能:开始运行建图节点
-
node_options.cc
在node_main.cc的Run()中,执行了LoadOption()函数
std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
功能:加载所有的lua文件参数
-
map_builder.cc
auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
CreateMapBuilder()在map_builder.cc
功能:创建了一个建图实例
-
node_main.cc
if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); }
功能:开始建图
-
node.cc
从node_main.cc进入node.cc的StartTrajectoryWithDefaultTopics
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { absl::MutexLock lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); AddTrajectory(options); }
功能:StartTrajectoryWithDefaultTopics函数中去执行AddTrajectory的新建轨迹功能
AddTrajectory()函数有很多个步骤,下面依次来梳理
-
node.cc
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids = ComputeExpectedSensorIds(options)
函数的定义在这儿
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const { using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; using SensorType = SensorId::SensorType; std::set<SensorId> expected_topics; // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. for (const std::string& topic : ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : ComputeRepeatedTopicNames( kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. if (node_options_.map_builder_options.use_trajectory_builder_3d() || (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { expected_topics.insert(SensorId{SensorType::IMU, kImuTopic}); } // Odometry is optional. if (options.use_odometry) { expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic}); } // NavSatFix is optional. if (options.use_nav_sat) { expected_topics.insert( SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic}); } // Landmark is optional. if (options.use_landmarks) { expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic}); } return expected_topics; }
三个for循环,第二个for语句运行了,topic为echoes
四个if语句,第一个if语句运行了,topic为imu该函数的结果是向expected_topic中,添加了两个话题,它们的id一个叫做echoes,另一个叫做imu,它们的type一个是RANGE,另一个是IMU
功能:确定建图过程中要订阅的topic名字 -
node.cc
const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
功能:进一步调用MapBuilderBridge的函数AddTrajectory
-
map_builder_bridge.cc
const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, [this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr< const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); });
功能:进一步调用MapBuilder的函数AddTrajectoryBuilder
-
map_builder.cc
int MapBuilder::AddTrajectoryBuilder( const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback)
功能:在这个函数中,会判断是2D建图还是3D建图
-
map_builder.cc
直接进入else语句
if (options_.use_trajectory_builder_3d()) { ..... } else { std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( trajectory_options.trajectory_builder_2d_options(), SelectRangeSensorIds(expected_sensor_ids)); } DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())); trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter))); }
这段有两个个关键语句,依次来拆开看
-
map_builder.cc
最开始是一个if语句
std::vector<std::string> SelectRangeSensorIds( const std::set<MapBuilder::SensorId>& expected_sensor_ids) { std::vector<std::string> range_sensor_ids; for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) { if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) { range_sensor_ids.push_back(sensor_id.id); } } return range_sensor_ids; }
传进来的参数expected_sensor_ids,实际上就是上面第6小节分析过的。阅读这段代码,可以看出最后return的range_sensor_ids就是RANGE类型的echoes
-
map_builder.cc
紧接if语句之后
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter)));
核心语句是
absl::make_unique<CollatedTrajectoryBuilder>
,构建了一个CollatedTrajectoryBuilder的指针,代码在collated_trajectory_builder.cc中 -
collated_trajectory_builder.cc
这是CollatedTrajectoryBuilder的构造函数
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, sensor::CollatorInterface* const sensor_collator, const int trajectory_id, const std::set<SensorId>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) : sensor_collator_(sensor_collator), collate_landmarks_(trajectory_options.collate_landmarks()), collate_fixed_frame_(trajectory_options.collate_fixed_frame()), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now()) { absl::flat_hash_set<std::string> expected_sensor_id_strings; for (const auto& sensor_id : expected_sensor_ids) { if (sensor_id.type == SensorId::SensorType::LANDMARK && !collate_landmarks_) { continue; } if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE && !collate_fixed_frame_) { continue; } expected_sensor_id_strings.insert(sensor_id.id); } sensor_collator_->AddTrajectory( trajectory_id, expected_sensor_id_strings, [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); }
第一个for循环中含有两个if,都没有进到if中。因此只执行了for循环中的唯一一句insert语句,insert中的参数
sensor_id.id
的值依靠for循环,产生了两个:echoes和imu
因此核心语句是指针sensor_collator_ 的AddTrajectory() -
Collator.cc
sensor_collator_指针的声明在collated_trajectory_builder.h中
sensor::CollatorInterface* const sensor_collator_;
CollatorInterface类中的AddTrajectory是一个虚函数。Collator是CollatorInterface的子类,在Collator.cc中对AddTrajectory进行了重写
Collator.cc中AddTrajectory的定义
void Collator::AddTrajectory( const int trajectory_id, const absl::flat_hash_set<std::string>& expected_sensor_ids, const Callback& callback) { for (const auto& sensor_id : expected_sensor_ids) { const auto queue_key = QueueKey{trajectory_id, sensor_id}; queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr<Data> data) { callback(sensor_id, std::move(data)); }); queue_keys_[trajectory_id].push_back(queue_key); } }
cout可以发现,for循环了两次,两次的sensor_id分别是echoes、imu,trajectory_id一直是0
功能:向队列queue_keys里,压入了两个QueueKey
-
collated_trajectory_builder.cc
结束了Collator.cc的AddTrajectory()
返回上一层,也同时结束了collated_trajectory_builder.cc中的CollatedTrajectoryBuilder的构造函数 -
map_builder.cc
结束了CollatedTrajectoryBuilder的构造函数
返回上一层map_builder.cc继续执行AddTrajectoryBuilder() -
map_builder.cc
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get());
进入MaybeAddPureLocalizationTrimmer函数后,没有进行任何操作,相当于被跳过了
-
map_builder.cc
if (trajectory_options.has_initial_trajectory_pose()) { const auto& initial_trajectory_pose = trajectory_options.initial_trajectory_pose(); pose_graph_->SetInitialTrajectoryPose( trajectory_id, initial_trajectory_pose.to_trajectory_id(), transform::ToRigid3(initial_trajectory_pose.relative_pose()), common::FromUniversal(initial_trajectory_pose.timestamp())); }
这段if也没有进去,跳过了
-
map_builder.cc
proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto; for (const auto& sensor_id : expected_sensor_ids) { *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id); } *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = trajectory_options; all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
这篇先不展开去找函数定义了。结合这段函数的上下文可以得到这段函数的意义
功能:这段语句的前面一大段向expected_sensor_ids添加了两个话题,一个echoes,另外一个imu。这段语句将两个话题,配合函数输入的参数trajectory_options(这个变量中包含了从lua文件读出的参数),一起添加到了all_trajectory_builder_options_中
-
node.cc
运行完18小节解释的代码,会返回node.cc,继续执行node.cc中的AddTrajectory()
AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options);
这两句话这篇不解释了,以前的一篇解释过
-
node.cc
LaunchSubscribers(options, trajectory_id);
这句话订阅echoes和imu话题