cartographer源码解析（三）node_main.cc中的FinishAllTrajectories()_cartographer finish

描述

Run()函数的最后有两个关键句（我认为的），一个是FinishAllTrajectories，另外一个是RunFinalOptimization

这篇会分析第一个，也就是函数FinishAllTrajectories

第一部分会分析FinishAllTrajectories中每行程序的声明位置，第二部分会分析FinishAllTrajectories实现的功能

一、FinishAllTrajectories()的基本逻辑

FinishAllTrajectories()源码如下

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);
    }
  }
}
1
2
3
4
5
6
7
8
9
10

这段代码大概可以分为三个部分，下面依次来分析

第一层 absl::MutexLock lock(&mutex_);

absl::MutexLock lock(&mutex_);
1

进行加锁，为了使用变量map_builder_bridge_的安全

第二层 map_builder_bridge_.GetTrajectoryStates()

map_builder_bridge_定义在cartographer_ros/node.h中

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
1

其中MapBuilderBridge类的定义在cartographer_ros/map_builder_bridge.h中
代码中的GUARDED_BY(mutex_)的功能是为了保证线程安全，使用该属性后，线程要使用相应变量，必须先锁定mutex_。也就是说要使用map_builder_bridge_之前要先锁定mutex_，而absl::Mutex mutex_也在cartographer_ros/map_builder_bridge.h进行了定义。

事实上，absl::Mutex mutex_在cartographer的很多头文件中都进行了定义，以完成线程安全这一功能

GetTrajectoryStates()的源码在下面

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;
}
1
2
3
4
5
6
7
8
9
10
11
12

• 返回值

  返回值是一个std::map格式，key是int类型，value是::cartographer::mapping::PoseGraphInterface::TrajectoryState类型

• auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();

  1. map_builder_是MapBuilderBridge类的私有成员变量，它的定义在cartographer_ros/map_builder_bridge.h中，std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

  std::unique_ptr是排他性指针，一个资源只可以被指向一次

  2. map_builder_的声明是cartographer::mapping::MapBuilderInterface，它是一个实例类，定义在cartographer/mapping/map_builder_interface.h中，它的成员函数中有一个定义virtual mapping::PoseGraphInterface* pose_graph() = 0;，

  3. PoseGraphInterface定义在cartographer/mapping/pose_graph_interface.h中，GetTrajectoryStates是PoseGraphInterface的成员函数，
     virtual std::map<int, TrajectoryState> GetTrajectoryStates() const = 0;，函数的功能就是返回轨迹的状态

  4. 向trajectory_states插入了多个键值对，键值对的key被设置为了sensor_bridge.first，值被设置为了::cartographer::mapping::PoseGraphInterface::TrajectoryState::ACTIVE

第三层 if包括的语句

这个if语句包含的内容看起来比较容易

简单来说

entry是map_builder_返回轨迹状态的遍历

TrajectoryState是是PoseGraphInterface的成员变量，是一个枚举类型，定义在cartographer/mapping/pose_graph_interface.h中，enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };

if的判断条件是，如果entry.second == TrajectoryState::ACTIVE)，也就是当前轨迹状态是ACTIVE时

执行的操作是两步

第一步

trajectory_id = entry.first;，将当前轨迹状态map类型的id，赋值给一个const int常量trajectory_id

第二步

CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,cartographer_ros_msgs::StatusCode::OK);

CHECK_EQ()是glog里的API，功能是判断两个参数是否相等

CHECK_EQ()第一个参数是FinishTrajectoryUnderLock(trajectory_id).code，它的定义在cartographer_ros/node.h中，是node类的一个成员函数，声明为cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);

StatusResponse定义在cartographer_ros_msgs/StatusResponse.msg文件内，该消息有两个成员

uint8 code
string message
1
2

EXCLUSIVE_LOCKS_REQUIRED(mutex_);中的EXCLUSIVE_LOCKS_REQUIRED函数，是作用于方法或者函数上的属性，它表明了调用线程必须独享给定的监护权。可以指定不止一个监护权。监护权必须在函数的入口处、出口处同时被声明。（我还没有去学习）

CHECK_EQ()第二个参数是cartographer_ros_msgs::StatusCode::OK，StatusCode定义在cartographer_ros_msgs/StatusCode.msg文件内，该消息有以下的成员

uint8 OK=0
uint8 CANCELLED=1
uint8 UNKNOWN=2
uint8 INVALID_ARGUMENT=3
uint8 DEADLINE_EXCEEDED=4
uint8 NOT_FOUND=5
uint8 ALREADY_EXISTS=6
uint8 PERMISSION_DENIED=7
uint8 RESOURCE_EXHAUSTED=8
uint8 FAILED_PRECONDITION=9
uint8 ABORTED=10
uint8 OUT_OF_RANGE=11
uint8 UNIMPLEMENTED=12
uint8 INTERNAL=13
uint8 UNAVAILABLE=14
uint8 DATA_LOSS=15
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

二、FinishAllTrajectories()的实现功能

待补充