梳理cartographer传感器数据流
前言
因为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.hif (trajectory_options.has_trajectory_builder_3d_options()) {local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>( // 新建一个local_trajectory_buildertrajectory_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继承了接口TrajectoryBuildersensor_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_buildertrajectory_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继承了接口TrajectoryBuildercommon::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_buildertrajectory_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继承了接口TrajectoryBuildercommon::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继承了接口TrajectoryBuildercommon::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::Collatortrajectory_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);
}
简单来说就是GlobalTrajectoryBuilder的一个构造函数。这里也有个回调函数：**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_bridgereturn 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* consttrajectory_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，即将刚进来的数据加入位姿图。
梳理cartographer传感器数据流相关推荐
- 「物联网架构」MQTT 传感器数据流异常检测的深度学习KSQL UDF
用于传感器分析的KSQL UDF.利用KSQL的新的API特性,用Java轻松地构建UDF / UDAF函数,从而使用Apache Kafka进行连续流处理.用例:联网汽车--使用深度学习的实时流媒体 ...
- cartographer坐标系_cartographer个人对框架解读
cartographer的代码主要包括两个部分:cartographer和cartographer_ros. 一.cartographer和cartographer_ros的关系 cartograph ...
- cartographer源码分析（4）sensor_bridge.cc以及sensor_bridge.h
包含了: 1.里程计的预处理函数 2.GPS的预处理函数 3.IMU的预处理函数 4.点云的预处理函数 以上函数都调用了AddSensorData函数.具体的数据流程:传感器的ROS节点/Playba ...
- 嵌入式传感器是智能移动增长的关键
嵌入式传感器是智能移动增长的关键 Embedded sensors are key to smart mobility growth 由于移动即服务(mobility-as-a-service,Maa ...
- 产业|嵌入式传感器将是未来机器人等技术增长的核心
来源:EEWORLD 移动即服务(MaaS)被认为是智能移动的一个关键要素,而机器人汽车技术将是智能移动的一个重要因素,它又高度依赖于嵌入式传感器. 根据市场研究和战略咨询公司Yole develop ...
- 湿度传感器如何工作？
湿度传感器是一种电子设备,可测量和报告其所部署的周围环境(例如空气.土壤或密闭空间)的湿度和气温. 湿度测量表明空气中水蒸气的浓度.它们以适当的电子信号的形式提供测量结果.此外,它们还报告相对湿度,即 ...
- Cartographer中对激光雷达运动畸变的处理方法分析
任务动机:梳理cartographer处理激光雷达运动畸变的原理,并针对特殊数据特性的雷达数据做相应适配,进而提升建图效果. 任务描述:查阅cartographer源码中激光雷达运动畸变的处理流程,对 ...
- 作为传感器集线器的MCU_凯利讯半导体
今天的手机.平板电脑和其他无线数字设备现在包括提供位置.方向.高度.接近和触摸感知能力的各种传感器.这些传感器每个都提供一个数据输出流,这些数据流必须由主机系统的主处理器捕获或处理,或者由一个芯片来优 ...
- 基于惯性传感器的上肢康复训练评估
基于惯性传感器的上肢康复训练评估 神经肌肉系统疾病患者,往往需要进行长期的康复训练恢复自由活动的能力或保持肌肉活性.在肢体康复训练中,临床医生给患者开一系列康复训练的处方,无论是在患者面前演示还是移动 ...
最新文章
- 通过反射动态调用webservices
- 【Hibernate】Hibernate实体关系映射——双边的多对多关系
- mybatis的逆向工程
- Qt中基类widget的各个事件函数中包含了ignore（）的调用
- jsp中String path = request.getContextPath()的作用
- 【html】【19】高级篇--大事件时间轴
- 学游戏3D建模，选机构要注意哪些方面？
- （7）FPGA面试题Latch和Register区别
- 【转】QT实现不规则窗体
- 3 万字 51 张图教你 CPU、内存、操作系统硬核知识！
- Animal-AI 2.0.0发布了！快来测试你的智能体吧。
- Android studio ： 模拟器 真机 运行失败（mac电脑）
- (博主可帮找错)Servlet.service() for servlet [dispatcherServlet] path [] threw exception feign.Feig,可截图私聊博主
- vi编辑器替换字符串命令
- 怎么批量打印html,Word如何批量打印
- Kindle Paperwhite2测评剧本.
- flash移植到android上,Flash Web Game移植到Android平台需要注意的地方
- 腾讯企业邮箱的邮局服务器设置,网易邮件客户端如何配置腾讯企业邮箱 并设置服务器邮件同步-♚付涛纪实阁♚...
- 关于IT公司技术委员会职责
- BUUCTF--[0CTF 2016]piapiapia
热门文章
- RUN__IT # 一个简单的爬妹子代码送福利（正则表达式总结）
- JAVA简易推箱子小游戏制作
- 武汉星起航跨境：美元汇率上升，旺季爆单，美国站卖家迎来好消息
- 个人网站风云榜：近三年草根站长名人录
- NUMERIC(10,4) 和DECIMAL(10, 4) 的区别和用法？
- 企业领袖必读的10本管理学书籍
- 实习周报 | 3rd week | 2021.4.19 - 4.23 | 文本摘要，ES语义搜索，SOTA SA
- [ActionScript 3.0] AS3.0 调试出现安全沙箱冲突错误解决办法
- mac必备的文件管理器：Path Finder Mac中文版
- 【对标TensorFlow】阿里公开内部超大规模分布式机器学习平台，对此你怎么看？