上一篇是由于node_main.cc中调用了node类的构造函数，所以展开阅读了下node.h和node.cc文件，从而大致了解node类内各种函数的主要功能。本篇将回到node_main.cc文件，继续读完剩余的代码。从node类的构造函数往下阅读，并了解如何具体应用node类的成员函数。
1.LoadState函数
if (!FLAGS_load_state_filename.empty()) {
//加载数据包数据;
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}在node_main.cc函数的前面中，若是load_state_filename设置为非空，则调用node类的LoadState函数加载数据包。如下是具体实现：
void Node::LoadState(const std::string& state_filename,
const bool load_frozen_state) {
absl::MutexLock lock(&mutex_);// 互斥锁
map_builder_bridge_.LoadState(state_filename, load_frozen_state);
}可以看到调用了MapBuilderBridge类的LoadState函数，跟着深入到MapBuilderBridge类，在map_builder_bridge.h中可以查看到其声明为：
void LoadState(const std::string& state_filename, bool load_frozen_state);参数一为加载数据包名称，参数二为是否加载已保存的非优化路径状态。
接下来转到map_builder_bridge.cc文件中查看具体实现：
void MapBuilderBridge::LoadState(const std::string& state_filename,
bool load_frozen_state) {
// Check if suffix of the state file is ".pbstream".
const std::string suffix = ".pbstream";
CHECK_EQ(state_filename.substr(
std::max<int>(state_filename.size() - suffix.size(), 0)),
suffix)
<< "The file containing the state to be loaded must be a "
".pbstream file.";
LOG(INFO) << "Loading saved state '" << state_filename << "'...";
cartographer::io::ProtoStreamReader stream(state_filename);
map_builder_->LoadState(&stream, load_frozen_state);
}从这里我们可以发现，MapBuilderBridge类的LoadState函数调用了 MapBuilder的成员函数 LoadState 来加载一个pbstream 文件，所以具体细节问题留到后续更新的cartographer底层代码阅读时讲解。map_builder_是接口MapBuilderInterface 的实例化对象,而根据是 2d 还是 3d 情况,其具体实现会略有不同，至于如何具体化为2D或者3D对象，在后面我也会慢慢细说。
2.StartTrajectoryWithDefaultTopics函数
if (FLAGS_start_trajectory_with_default_topics) {
//以默认话题开始规划路径;
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}同理，在node_main.cc函数的前面也是定义了start_trajectory_with_default_topics，默认情况下为true，cartographer的ROS上层也是从这里开始以默认话题进行轨迹的更新，这里稍带提一句，默认的话题名称设置可以到node_constants.h里面进行设置，如下所示：
// Default topic names; expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
constexpr char kMultiEchoLaserScanTopic[] = "echoes";
constexpr char kPointCloud2Topic[] = "points2";
constexpr char kImuTopic[] = "imu";
constexpr char kOdometryTopic[] = "odom";
constexpr char kNavSatFixTopic[] = "fix";
constexpr char kLandmarkTopic[] = "landmark";
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
constexpr char kOccupancyGridTopic[] = "map";
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kTrackedPoseTopic[] = "tracked_pose";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
constexpr char kReadMetricsServiceName[] = "read_metrics";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
constexpr char kConstraintListTopic[] = "constraint_list";
constexpr double kConstraintPublishPeriodSec = 0.5;
constexpr double kTopicMismatchCheckDelaySec = 3.0;
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;下面回到node.h里面有关StartTrajectoryWithDefaultTopics函数的声明：
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);参数只有一个，是有关轨迹的配置信息，也就是在trajectory_builder.lua文件里面的配置。下面看node.cc文件内的具体实现：
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options);
}可以看到简单三行代码，先是设置互斥锁，防止多个对象同时访问该资源产生冲突，然后是调用node类的成员函数ValidateTrajectoryOptions对配置信息进行验证，根据2D和3D的不同进行是否拥有相关设置信息的验证，否则返回false，最后是根据配置信息添加轨迹信息，下面为两个函数的具体实现。
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
return options.trajectory_builder_options
.has_trajectory_builder_2d_options();
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
return options.trajectory_builder_options
.has_trajectory_builder_3d_options();
}
return false;
}从node类的AddTrajectory函数我们可以看到主要调用了ComputeExpectedSensorIds、MapBuilderBridge类的AddTrajectory函数、AddExtrapolator、AddSensorSamplers、LaunchSubscribers函数，这里将继续挖掘MapBuilderBridge类的AddTrajectory函数，其余函数的功能可以参照上一篇，主要是返回具体topic名称、添加位姿估计器、添加传感器采样器、ROS topic订阅与回调函数。
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}同样先看map_builder_bridge.h文件内的声明：
int AddTrajectory(
const std::set<
::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options);输入参数为指定的topic名称集合、轨迹配置信息，下面看map_builder_bridge.cc文件内的具体实现：
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
//调用了 map_builder_->AddTrajectoryBuilder，传入的数据有传感器的 id,trajectory 的一些配置参数,以及一个lambda表达式
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);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}可以清楚的看到，依旧是稳健地调用了MapBuilder类的AddTrajectoryBuilder函数，其余主要是打印错误信息以及检查相应参数变量是否正确，该函数返回的是添加的轨迹编号。
3.FinishAllTrajectories函数
node.h文件内的声明：
void FinishAllTrajectories();很简单，接下来看下node.cc文件内的具体实现：
void Node::FinishAllTrajectories() {
absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
if (entry.second == TrajectoryState::ACTIVE) {
const int trajectory_id = entry.first;
CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
cartographer_ros_msgs::StatusCode::OK);
}
}
}主要调用了MapBuilderBridge类的GetTrajectoryStates函数和node类内的FinishTrajectoryUnderLock函数。
（1）MapBuilderBridge类的GetTrajectoryStates函数依旧与MapBuilder类中的posegraph（用于全局优化）的GetTrajectoryStates函数相关，返回的是存储轨迹状态的map容器。
std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();
// Add active trajectories that are not yet in the pose graph, but are e.g.
// waiting for input sensor data and thus already have a sensor bridge.
for (const auto& sensor_bridge : sensor_bridges_) {
trajectory_states.insert(std::make_pair(
sensor_bridge.first,
::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE));
}
return trajectory_states;
}（2）FinishTrajectoryUnderLock函数前面检查了一下是否可以关掉,指定 id 是否存在,是否已经被 Finished 了等情况后,如果一切正常,则停止订阅 Topic、清除 id 及其他与该 trajectory 相关的量。最后调用 map_builder_bridge_中的FinishTrajectory 函数。
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
const int trajectory_id) {
cartographer_ros_msgs::StatusResponse status_response;
//对是否可以结束相应路径进行判断，并进行异常处理
if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
status_response.message = absl::StrCat("Trajectory ", trajectory_id,
" already pending to finish.");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
LOG(INFO) << status_response.message;
return status_response;
}
// First, check if we can actually finish the trajectory.
status_response = TrajectoryStateToStatus(
trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
return status_response;
}
// Shutdown the subscribers of this trajectory.
// A valid case with no subscribers is e.g. if we just visualize states.
if (subscribers_.count(trajectory_id)) {
for (auto& entry : subscribers_[trajectory_id]) {
entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
}
map_builder_bridge_.FinishTrajectory(trajectory_id);
trajectories_scheduled_for_finish_.emplace(trajectory_id);
status_response.message =
absl::StrCat("Finished trajectory ", trajectory_id, ".");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
return status_response;
}map_builder_bridge_中的FinishTrajectory 函数主要细节藏在MapBuilder类的FinishTrajectory函数。
void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
// Make sure there is a trajectory with 'trajectory_id'.
CHECK(GetTrajectoryStates().count(trajectory_id));
map_builder_->FinishTrajectory(trajectory_id);
sensor_bridges_.erase(trajectory_id);
}到此，cartographer的上层基本已经介绍完毕，俗话说的好，“魔鬼总是藏在细节中”，通过阅读我们发现MapBuilderBridge这个类很形象，完美地承担了“桥梁”的作用，搭建了我们从上层通向底层算法的路，接下来，我将仔细解读MapBuilder这个类是如何一步一步完成我们最终的SLAM的。
