前言
因为cartographer中的trajectory_builder、global_trajectory_builder、sensor_collator这些东西很痛苦。 所以在这里梳理一下cartographer传感器数据流。
说得很高，其实就是梳理冰山一角。希望读者能阅读map_builder.cc和collated_trajectory_builder.cc能给予一定的帮助。
这篇文章引用了很多cartographer源代码。因为是自己注释的版本，会有很多**//注释混乱**，不要太注意代码引用中的注释。因为毕竟这篇文章只是为了窥视豹子。
因为使用了很多文件，我先在这里列出一个文件列表，让读者找到文件的目录进行查阅。
cartographer_ros部分
- ：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc
cartographer部分
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/map_builder.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/trajectory_collator.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/data.h
- ：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.cc
目录
0.从ros引入 1.先说壳 2.再说壳中物 3.再看回调 4.总结
0.从ros引入
毕竟先得看ros壳体可以清除传感器的信息流
Node.cc
首先看node.cc
首先看用户请求增加轨迹，然后服务端service_servers_会回调函数Node::HandleStartTrajectory
service_servers_.push_back(node_handle_.advertiseService( kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); HandleStartTrajectory 该函数定义为：
bool Node::HandleStartTrajectory( ::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_);
TrajectoryOptions options;
if (!FromRosMessage(request.options, &options) ||
!ValidateTrajectoryOptions(options)) {
const std::string error = "Invalid trajectory options.";
LOG(ERROR) << error;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message = error;
} else if (!ValidateTopicNames(request.topics, options)) {
const std::string error = "Invalid topics.";
LOG(ERROR) << error;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message = error;
} else {
response.trajectory_id = AddTrajectory(options, request.topics);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message = "Success.";
}
return true;
}
上述函数除了先行判断以外，最重要的是调用的Node::AddTrajectory函数，用来增加一条轨迹。
Node::AddTrajectory 输入：options配置项、request.topics请求（即传感器topic）
然后我们查看Node::AddTrajectory的定义，输入Wie配置项和传感器信息的topics（不是指的传感器消息，而是以后用来建立订阅者的传感器话题）
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options); // 跟位姿估计有关
AddSensorSamplers(trajectory_id, options); // 传感器数据采集相关
LaunchSubscribers(options, topics, trajectory_id); // 重点关注这个
is_active_trajectory_[trajectory_id] = true;
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
主要注意这一句——建壳操作：
const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
即使用MapBuilderBridge::AddTrajectory函数，以此增加了一条轨迹。
map_builder_bridge_在map_builder_bridge.h中的定义为：
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
注意，由于是新建轨迹，因此并没有接受传感器数据。新建一个而已。
所以我们先看map_builder_bridge.cc中是如何实现它的
1.先讲“壳”
map_builder_bridge.cc
// 重要函数特殊对待
/** @brief 增加一条轨迹 @param expected_sensor_ids 期望的传感器id？ @param trajectory_options 轨迹的配置参数 */
int MapBuilderBridge::AddTrajectory( // 重要函数！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！增加一条轨迹
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_->AddTrajectoryBuilder( // 这已经是cartographer项目中的代码了
expected_sensor_ids, trajectory_options.trajectory_builder_options,
::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, // OnLocalSlamResult函数
::std::placeholders::_1, ::std::placeholders::_2,
::std::placeholders::_3, ::std::placeholders::_4,
::std::placeholders::_5));
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] =
cartographer::common::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); // 返回一个新的哈希容器，哈希容器比原来多一个pair，这个pair包含了这条轨迹的id及其配置参数
CHECK(emplace_result.second == true);
return trajectory_id;
}
其输入参数有： （1） 不仅是能检测距离的传感器如laser、kinect，其他有用的传感器也会包含进来，如里程计、imu （2），定义于 /home/mjy/dev/carto_ws/src/cartographer/configuration_files/trajectory_builder.lua 主要是决定了采用3d-slam还是2d-slam （3） ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, // OnLocalSlamResult函数 ::std::placeholders::_1, ::std::placeholders::_2, ::std::placeholders::_3, ::std::placeholders::_4, ::std::placeholders::_5)); 用bind绑了一下。这个函数到后面我们会知道是一个。 暂时先不管
看完输入参数，我们来看具体实现：
map_builder.cc
这已经是cartographer文件夹中的东西了。 file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback)
其函数定义中能看出传感器流端倪（注意，只是端倪，但并没有接受传感器数据）的是：
// -----------------------------------3d情况---------------------------------------------------------------
if (options_.use_trajectory_builder_3d()) {
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder; // /mapping/internal/3d/local_trajectory_builder_3d.h
if (trajectory_options.has_trajectory_builder_3d_options()) {
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>( // 新建一个local_trajectory_builder
trajectory_options.trajectory_builder_3d_options(),
SelectRangeSensorIds(expected_sensor_ids)); // 找到那些能检测距离的传感器，如laser，kinect
}
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get())); // 将接口类型强转为3d位姿图类型
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>( //真正地创建一个TrajectoryBuilder，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
sensor_collator_.get(), trajectory_id, expected_sensor_ids,
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()),
local_slam_result_callback))); // 需要传入回调local_slam_result_callback
} else {
// -----------------------------------2d情况---------------------------------------------------------------
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; // local slam 的builder，值得点进去看看这个类
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>( // 新建一个local_trajectory_builder
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids)); // 找到那些能检测距离的传感器，如laser，kinect
}
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
// 传入的参数sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator，具体是什么类型是由mapbuilder的构造函数决定
trajectory_builders_.push_back( //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
common::make_unique<CollatedTrajectoryBuilder>( // CollatedTrajectoryBuilder这里面我写了回调的总体思路
sensor_collator_.get(), trajectory_id, expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id, // GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback))); // 需要传入回调local_slam_result_callback
// 上述回调先是由trajectory_builder_interface.h中定义了一个函数指针
// using LocalSlamResultCallback =
// std::function<void(int /* trajectory ID */, common::Time,
// transform::Rigid3d /* local pose estimate */,
// sensor::RangeData /* in local frame */,
// std::unique_ptr<const InsertionResult>)>;
// 上述回调的具体实现在：
// 由于trajectory_builder是属于CollatedTrajectoryBuilder类型，所以在collated_trajectory_builder.cc文件中：
让我们只看2d情况吧：
// -----------------------------------2d情况---------------------------------------------------------------
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; // local slam 的builder，值得点进去看看这个类
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>( // 新建一个local_trajectory_builder
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids)); // 找到那些能检测距离的传感器，如laser，kinect
}
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
// 传入的参数sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator，具体是什么类型是由mapbuilder的构造函数决定
trajectory_builders_.push_back( //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
common::make_unique<CollatedTrajectoryBuilder>( // CollatedTrajectoryBuilder这里面我写了回调的总体思路
sensor_collator_.get(), trajectory_id, expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id, // GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback))); // 需要传入回调local_slam_result_callback
// 上述回调先是由trajectory_builder_interface.h中定义了一个函数指针
// using LocalSlamResultCallback =
// std::function<void(int /* trajectory ID */, common::Time,
// transform::Rigid3d /* local pose estimate */,
// sensor::RangeData /* in local frame */,
// std::unique_ptr<const InsertionResult>)>;
// 上述回调的具体实现在：
// 由于trajectory_builder是属于CollatedTrajectoryBuilder类型，所以在collated_trajectory_builder.cc文件中：
请着重关注这一部分：
trajectory_builders_.push_back( //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
common::make_unique<CollatedTrajectoryBuilder>( // CollatedTrajectoryBuilder这里面我写了回调的总体思路
sensor_collator_.get(), trajectory_id, expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id, // GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback))); // 需要传入回调local_slam_result_callback
以上代码真正建立了一个CollatedTrajectoryBuilder类型的trajectory_builder，并pushback进trajectory_builders_容器。
其中建立trajectory_builder的时候用到了CollatedTrajectoryBuilder的构造函数。
所以我们点进CollatedTrajectoryBuilder类型中，看一看端倪吧。
collated_trajectory_builder.cc
重点看构造函数，因为上面用到了它的构造函数：
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) // 传入的这个wrapped（打包的）_trajectory_builder即GlobalTrajectoryBuilder类型
: sensor_collator_(sensor_collator), // 取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
std::unordered_set<std::string> expected_sensor_id_strings;
for (const auto& sensor_id : expected_sensor_ids) {
expected_sensor_id_strings.insert(sensor_id.id);
}
// sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator
// HandleCollatedSensorData为其回调函数
// 这里很重要！
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));
});
}
重点关注最后一部分，这是建立轨迹的核心：
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));
});
首先，sensor_collator_的类型取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator
现在我们假设sensor_collator_为TrajectoryCollator类型吧。
（1）trajectory_id轨迹的id （2）expected_sensor_id_strings 传感器id，也不仅仅指距离传感器 （3）一个回调函数壳HandleCollatedSensorData，注意这里的data并不是指现在构造的时候需要传入data，而只是一个回调函数壳而已。以后data进来以后才生效。（好好理解这句话，很重要）
现在我们点进trajectory_collator.cc，来看看sensor_collator_->AddTrajectory的具体实现
trajectory_collator.cc
// 在trajectory_builder被建立时，通过CollatedTrajectoryBuilder的构造函数中，sensor_collator_进行调用
void TrajectoryCollator::AddTrajectory(
const int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const Callback& callback) {
// 这里有个回调
CHECK_EQ(trajectory_to_queue_.count(trajectory_id), 0);
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{
trajectory_id, sensor_id};
/* * struct QueueKey { int trajectory_id; std::string sensor_id; bool operator<(const QueueKey& other) const { return std::forward_as_tuple(trajectory_id, sensor_id) < std::forward_as_tuple(other.trajectory_id, other.sensor_id); } }; */
// trajectory_to_queue_为哈希容器，存放<轨迹索引， 顺序队列>
// AddQueue为OrderedMultiQueue类的成员函数
// 向队列（以<queue_key，queue>为pair的map容器）中加入一个pair，其中queue_key为queue_key，queue的回调callback为输入的回调函数
trajectory_to_queue_[trajectory_id].AddQueue(
queue_key, [callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
trajectory_to_queue_keys_[trajectory_id].push_back(queue_key);
}
}
仔细看一下： 先对所有有用的，传进来的传感器进行循环。比如我先拿到了传感器A 对于A，先分配一个queue_key，queue_key是一个pair，{trajectory_id, sensor_id}，代表了这个传感器位于哪条轨迹，是有用传感器的第几个传感器
然后使用AddQueue函数，向trajectory_to_queue_容器中的第trajectory_id个元素（一个元素即一个（为什么叫多列之后你会知道））加入一个关于A的，成为A*。
让我们仔细看一下AddQueue的实现吧： AddQueue为OrderedMultiQueue类的成员函数
ordered_multi_queue.cc
多列子A*包含两部分，分别为queue_key和Queue
请着重关注Queue
Queue的结构定义于.h文件中：
struct Queue {
common::BlockingQueue<std::unique_ptr<Data>> queue;
Callback callback;
bool finished = false;
};
可以发现其结构包括一个小写queue，和一个callback （1）对于queue： 可以看出是一个数据队列。 （2）对于callback： 数据加进来以后会产生回调。即HandleCollatedSensorData
然后回到函数的实现：
// 被trajectory_collator.cc调用
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = std::