描述

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);
    }
  }
}

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

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

absl::MutexLock lock(&mutex_);

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

第二层 map_builder_bridge_.GetTrajectoryStates()

map_builder_bridge_定义在cartographer_ros/node.h

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

其中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;
}
  • 返回值

          返回值是一个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语句包含的内容看起来比较容易

简单来说

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

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

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

3.执行的操作是两步

第一步

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

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

二、FinishAllTrajectories()的实现功能

待补充