前言
因为cartographer中的trajectory_builder、global_trajectory_builder、sensor_collator这些东西搞得苦不堪言。
因此在这里梳理一下cartographer传感器数据流。
说的很高大上，其实也就是冰山一角的梳理。希望读者在阅读map_builder.cc和collated_trajectory_builder.cc的时候能给予一定的帮助。
本文引用了很多cartographer的源代码。因为是自己注释过的版本，因此会有很多**//杂乱的注释**，大家不用过多关注那些代码引用里的注释。因为毕竟本文只是以管窥豹。
因为其中用到了许多文件，因此我在这里先列一个文件列表，以便读者寻找文件所在目录进行查阅。
cartographer_ros部分
- Node.cc：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc
- map_builder_bridge.cc：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc
- sensor_bridge.h：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc
cartographer部分
- map_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/map_builder.cc
- collated_trajectory_builder.h：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h
- collated_trajectory_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.cc
- trajectory_collator.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/trajectory_collator.cc
- ordered_multi_queue.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc
- global_trajectory_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc
- data.h：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/data.h
- local_slam_result_2d.cc：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;
}
其中让我们重点关注：map_builder_->AddTrajectoryBuilder
其输入参数有：
（1）expected_sensor_ids 不仅是能检测距离的传感器如laser、kinect，其他有用的传感器也会包含进来，如里程计、imu
（2）trajectory_options.trajectory_builder_options 配置选项，定义于
/home/mjy/dev/carto_ws/src/cartographer/configuration_files/trajectory_builder.lua
主要是决定了采用3d-slam还是2d-slam
（3）bind了一个函数
::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类型吧。
上面的sensor_collator_->AddTrajectory函数中，输入参数为：
（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
然后回到AddQueue函数的实现：
// 被trajectory_collator.cc调用
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = std::move(callback);
}
其实就是向queues_中加入新成员A*
小梳理一下 ----------------------------------------------------------------------------------------------------
sensor_collator_属于 trajectory_collator 类，它维护了一个容器trajectory_to_queue_，这个容器的每一个元素代表了一条轨迹对应的一个“多对象顺序队列”即ordered_multi_queue，简称“多列”。也就是说一个轨迹对应一个多列。
我用trajectory_id先索引到当前轨迹的多列，然后调用了多列的成员函数AddQueue。每一个传感器都会调用一次同样的多列，因为是同一条轨迹进行循环嘛
这个多列维护了一个容器queues_。queues_的成员是一个个的多列子，即多列维护的容器中的一个个儿子，他们是pair的形式：<QueueKey, Queue>。QueueKey体现了轨迹id和传感器id。Queue体现了传感器数据队列和回调函数。
小梳理完毕 ----------------------------------------------------------------------------------------------------
传入的回调函数即为sensor_collator的HandleCollatedSensorData函数，其 实现在collated_trajectory_builder.cc文件中。
我们将HandleCollatedSensorData函数称为
“上层回调”
重点是HandleCollatedSensorData的实现中，其中以下语句：
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
// 重点看这个，支撑起回调函数HandleCollatedSensorData的主要作用：向位姿图中插入data
其实就是向位姿图中插入data。wrapped_trajectory_builder_是传入的CreateGlobalTrajectoryBuilder2D函数构造而来
具体见上述map_builder.cc的节选，即：
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id, // GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback)))
CreateGlobalTrajectoryBuilder2D函数的具体实现为：
global_trajectory_builder.cc
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
return common::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
}
“下层回调”
往上回溯，这个下层回调就是曾经被bind的那个函数：OnLocalSlamResult
之所以叫下层回调，是因为上层回调HandleCollatedSensorData函数套了一个wrapped_trajectory_builder_，它是GlobalTrajectoryBuilder类型，这个类型绑定了上述的回调local_slam_result_callback。
先不讲下层回调，以后会涉及到，因为它涉及了数据进来以后的操作，所以在“建立壳”这一节讲并不合理。
这篇文章的最后会发现，上层回调和下层回调根本没有关系，也就是这样起名是不合理的。但是这是最后发现的，因此这里先做个提醒。
**** 至此，讲完了如何建立“壳”
也就是说，上述的一些东西还没涉及到data，也就是真正的传感器数据进来以后是如何操作的。之前只是建立了一些“壳”一样的东西等待传感器的进入，包括队列成员的设置呀，回调函数的设置呀，都是壳。
然后也只讲述可上层回调，没有进一步深入下层回调。
下面深入一些，进一步考虑传感器数据，即向壳中倒入传感器数据。
2.再讲“壳中物”
首先，回到Node::AddTrajectory函数，看看通过AddTrajectory建完壳后有什么操作：
Node.cc
```csharp
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;
}
和输入的传感器数据有关联的是LaunchSubscribers
这个函数LaunchSubscribers启动了各类传感器消息订阅者，并开始订阅传感器消息
让我们看看它的具体实现节选：
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic,
&node_handle_, this),
topic});
}
topic即为传入的传感器话题。上述节选实现了imu传感器消息的订阅者
回调函数为：HandleImuMessage
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
好！！！我们看到了，当真正的传感器数据进来以后，回调函数是如何处理的。
核心是这一句：sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
sensor_bridge_ptr是由auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);得到的
我们回到map_builder_bridge.cc
map_builder_bridge.cc
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { // 返回指定轨迹的sensor_bridge
return sensor_bridges_.at(trajectory_id).get();
}
sensor_bridges_是个SensorBridge容器
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
接下来看SensorBridge类型中的HandleImuMessage函数
sensor_bridge.h
void SensorBridge::HandleImuMessage(const std::string& sensor_id, // 将imu信息转换为carto可处理的形式，并被trajectory_builder_加入
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity});
}
}
终于
我们找到了最最最重要的，将cartographer_ros的传感器消息进行处理，并传入cartographer中的重要函数HandleImuMessage的实现
trajectory_builder_->AddSensorData是将数据传入cartographer的trajectory_builder_。因为trajectory_builder_类型是：
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
在cartographer中，trajectory_builder的真正类型是collated_trajectory_builder，它继承了上述的TrajectoryBuilderInterface
所以让我们回到cartographer下的：
collated_trajectory_builder.h
找到了AddSensorData的实现：
void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override {
AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
}
调用了AddData：
collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
sensor_collator_这个东西是不是很眼熟？
没错，他就是在我们第一讲 1.先讲“壳” 中的传感器收集者。最后会知道，它在cartographer中是所有的传感器数据的管理者。
其类型之前说过：
sensor_collator_的类型取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator
还是假设他为：TrajectoryCollator类型。然后，看如何实现AddSensorData函数
trajectory_collator.cc
// 将传感器数据根据trajectory_id加入队列
void TrajectoryCollator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
trajectory_to_queue_.at(trajectory_id)
.Add(std::move(queue_key), std::move(data));
}
然后你会惊喜的发现！又出现了Queue这个令人兴奋的单词！trajectory_to_queue_这是个容器，之前说过：
这个容器的每一个元素代表了一条轨迹对应的一个“多对象顺序队列”，简称“多列”。也就是说一个轨迹对应一个多列。
Add函数就是多列的成员函数，让我们看看它：
ordered_multi_queue.cc
// 被trajectory_collator.cc调用，给队列中的队员以数据
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
auto it = queues_.find(queue_key);
if (it == queues_.end()) {
LOG_EVERY_N(WARNING, 1000)
<< "Ignored data for queue: '" << queue_key << "'";
return;
}
it->second.queue.Push(std::move(data));
Dispatch();
}
会继续惊喜的发现：it->second.queue.Push(std::move(data));这一句将数据，推入了，多列维护的queues_容器中的某一多列子<QueueKey, Queue>的second元素即：Queue的queue中。
前面说过，queue维护了这一个传感器的数据队列：
common::BlockingQueue<std::unique_ptr> queue;
至此，壳中物可以被添加到壳子当中了。
可以看到，在一条轨迹中，同一个传感器一段时间序列的一系列数据是在同一个queue中维护的。
暂时总结：
我们知道sensor_collator_只有一个，但是它维护了一个容器：trajectory_to_queue_，总的来说，
这个容器包储存了所有的数据，即，不同轨迹的不同传感器的不同时间传入的数据
也就是说，在cartographer中，sensor_collator_是管理所有的传感器数据的最大BOSS。
3.再看回调
前面讲过一个上层回调，还有一个下层回调。我们将理论完善一些。
先回顾上层回调：* 上层回调的引出
再回顾下层回调：* 下层回调的引出
当把数据加入到queue中的时候，我们优先引发上层回调。即HandleCollatedSensorData。
然后这个函数的实现引发了global_trajectoryBuilder这个东西产生下层回调。我们具体分析一下。
从HandleCollatedSensorData的实现开始，其中以下语句：
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
// 重点看这个，支撑起回调函数HandleCollatedSensorData的主要作用：向位姿图中插入data
data为sensor::Data类型，看看其成员函数：
data.h
virtual void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;
仅仅是个虚函数。需要被继承。
- LocalSlamResult2D (mapping/internal/2d/local_slam_result_2d.h)继承了LocalSlamResultData并实现了AddToTrajectoryBuilder.
- 在LocalSlamResultData的函数中有分别调用了TrajectoryBuilderInterface的成员函数和PoseGraph2D的成员函数。
所以你就暂时认为data就是local_slam_result_2d类型即可（因为还有3d的相应类型嘛，暂不考虑）
local_slam_result_2d.cc
void LocalSlamResult2D::AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) {
trajectory_builder->AddLocalSlamResultData(
common::make_unique<LocalSlamResult2D>(*this));
}
回想上面，我们传入的是wrapped_trajectory_builder_，之前说过它是一个GlobalTrajectoryBuilder2D类型。这个类型也是继承了TrajectoryBuilderInterface而来的。
所以我们得看GlobalTrajectoryBuilder2D的成员函数——AddLocalSlamResultData
global_trajectory_builder.cc
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
"local_trajectory_builder_ present.";
local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
}
我们会发现这个上层回调实际上是将data加入位姿图中。
回到mapping::LocalSlamResultData类型，查看其下的AddToPoseGraph函数。
local_slam_result_2d.cc
void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph));
CHECK_GE(local_slam_result_data_.submaps().size(), 1);
CHECK(local_slam_result_data_.submaps(0).has_submap_2d());
std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
for (const auto& submap_proto : local_slam_result_data_.submaps()) {
submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
}
static_cast<PoseGraph2D*>(pose_graph)
->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(local_slam_result_data_.node_data())),
trajectory_id, submaps);
}
最终下层回调追踪失败
也就是说，这个下层回调不是因为上层回调产生的，上层回调就是把数据加入位姿图中而已。并不引起下层回调。
也就是说，下层回调也就不能叫做下层回调。
我们现在将上层回调改名为回调A
我们现在将下层回调改名为回调B
回调A是数据加入了以后产生的，即自动将数据加入位姿图中。
回调B会在其他地方被用到，是用来在一个laser的前端匹配-估计位姿结束后决定是否将这个laser加入submap中进行的回调。但是要记住的是，它和GlobalTrajectoryBuilder这个东西是绑定的，回调B是由GlobalTrajectoryBuilder维护的。
二者没有联系。
4.总结
1.先讲壳这一节，我们给出了一个壳子。
2.再讲壳中物这一节，我们开启了订阅者们，订阅者们嗷嗷待哺等待数据到来，一旦数据到来则进入回调，会产生向壳中填入壳中物的效果。即将数据加入壳中，并产生回调A，即将刚进来的数据加入位姿图。
扫一扫
172
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
