从代码理解 cartographer 3 --- 理解 Range data 数据的流入
最主要的数据的流入是范围数据, IMU数据 和 Odom数据的流入, 但是这几个数据的流入进来之后到底如何保存, 流程如何.
我希望能够理解清除. 希望能在这里讲清楚.
最最最重要的, 如何理解这些东西, 还是要这一个类的包含的图.
起码对于我了解和写东西来说, 会很有用.
node:
sensor_samplers_:
extrapolators_:
map_builder_bridge_:
map_builder_:
pose_graph_:
trajectory_builders_: CollatedTrajectoryBuilder, CreateGlobalTrajectoryBuilder2D(),
wrapped_trajectory_builder_: GlobalTrajectoryBuilder
pose_graph_:PoseGraph2D
local_trajectory_builder_: LocalTrajectoryBuilder2D
range_data_collator_：RangeDataCollator
real_time_correlative_scan_matcher_: RealTimeCorrelativeScanMatcher2D
ceres_scan_matcher_: CeresScanMatcher2D
extrapolator_:
active_submaps_:
sensor_collator_: (和map_builder_是一致的)
HandleCollatedSensorData:(函数)
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())
sensor_collator_: Collator 或 TrajectoryCollator
OrderedMultiQueue
sensor_bridges_:SensorBridge
tf_bridge_
trajectory_builder_: (初始化是 map_builder_->GetTrajectoryBuilder(trajectory_id)), CollatedTrajectoryBuilder
wrapped_trajectory_builder_: GlobalTrajectoryBuilder
pose_graph_:PoseGraph2D
local_trajectory_builder_: LocalTrajectoryBuilder2D
range_data_collator_：RangeDataCollator
real_time_correlative_scan_matcher_: RealTimeCorrelativeScanMatcher2D
ceres_scan_matcher_: CeresScanMatcher2D
extrapolator_:
active_submaps_:
pose_graph_: PoseGraph2D, 都是上面传进来的指针
sensor_collator_: Collator 或 TrajectoryCollator
queue_: OrderedMultiQueue,IMU数据放到了这儿, odometry同样也是, laser的数据也是
雷达的信息的回调, 第一个开始的函数是:
1.
void Node::HandleLaserScanMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
这儿没处理, 简要地调用.
map_builder_bridge_.sensor_bridge(trajectory_id)的 map_builder_bridge_.sensor_bridge(trajectory_id)
这儿传入的消息, 是msg的格式.
在整个过程, 都要注意数据的格式, 因为这和相关调用的函数要对应上, 否则会错乱的.
2.
void SensorBridge::HandleLaserScanMessage( const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
第二个被调用的就是 SensorBridge 下面的 HandleLaserScanMessage.
这儿最主要的工作就是把格式 msg 转变为格式 cartographer::sensor::PointCloudWithIntensities.
在转换格式之后, 它就从 SensorBridge::HandleLaserScanMessage 跳到了 SensorBridge::HandleLaserScan 里面去
3.
void SensorBridge::HandleLaserScan( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::PointCloudWithIntensities& points)
这儿进行了num_subdivisions_per_laser_scan_ 的细分. 这个细分, 是什么意思哪?
从说明书当中, 可以找到这样的描述:
对于每个收到的雷达扫描, 会被分为多少个point cloud. 仔细划分一个雷达波为更小的部分,
是因为机器人在运动的时候, 不同方向上, 所获得的雷达波是不同的.仔细的划分,我们能够通过累积这些细分的point cloud. 然后可以使用scan matching来进行匹配.
这儿从 PointCloudWithIntensities 改变成 timedPointCloud 的格式
在这个函数之后, 就会调用SensorBridge::HandleRangefinder 的每个划分
4
void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges)
这儿进行的操作,
1) 查找这个平面到tracking_frame的一个转换. 这个主要用于, 因为sensor的这个东西,能放的地方很多, 需要一个刚体的转换到机器人本体那儿
2) 进行了数据的转换, 第一个转换是 TransformTimedPointCloud, 因为要把数据, 从sensor的平面, 到机器人的平面. 然后就是从 timedPointCloud 转到了 TimedPointCloudData.
3) 最后进行了 trajectory_builder_->AddSensorData 的操作. 调用的就是 CollatedTrajectoryBuilder 的 AddSensorData 的调用
5
CollatedTrajectoryBuilder void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); }
这儿主要的, 是将一个 sensor::TimedPointCloudData 的数据, 变换成了 Dispatchable 的格式.
Dispatchable 是一个类模板, 所有类型的数据, 都会被放在 data_ 里面, 这个的作用, 是会在后面显现的.
具体的就是, 你会靠这个去判断调用的是 GlobalTrajectoryBuilder 里面的哪个函数.
在这个函数之后, 就会调用 CollatedTrajectoryBuilder 的 AddData 函数.
6
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) { sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); }
这个函数也是相当的简单, 只是调用了
sensor_collator_ (在这假设为Collator这个) 的 AddSensorData 函数
7
void Collator::AddSensorData(const int trajectory_id, std::unique_ptr<Data> data) { QueueKey queue_key{trajectory_id, data->GetSensorId()}; queue_.Add(std::move(queue_key), std::move(data)); }
所以, 在这个函数里, 收集者, 会把数据往队列里面去添加.
然后会调用 OrderedMultiQueue :: add() 的函数
8
void OrderedMultiQueue::Add(const QueueKey& queue_key, std::unique_ptr<Data> data) { auto it = queues_.find(queue_key);// 找到sensor if (it == queues_.end()) { // 第一次可能没有找到, 或者是不期望的东西 LOG_EVERY_N(WARNING, 1000) << "Ignored data for queue: '" << queue_key << "'"; return; } it->second.queue.Push(std::move(data)); // 压入数据 Dispatch(); }
这个add () 函数, 确实会是吧这个数据, 添加到队列下面, 如果认真看这个队列, 它确实会按时间整理好这个数据.
最后会调用 OrderedMultiQueue::Dispatch() 的函数
这个函数, 就是分发的意思. 到底怎么分发 , 我们继续往下看
9
void OrderedMultiQueue::Dispatch() { while (true) { const Data* next_data = nullptr; Queue* next_queue = nullptr; QueueKey next_queue_key; for (auto it = queues_.begin(); it != queues_.end();) { const auto* data = it->second.queue.Peek<Data>(); if (data == nullptr) { if (it->second.finished) { queues_.erase(it++); continue; } CannotMakeProgress(it->first); return; } if (next_data == nullptr || data->GetTime() < next_data->GetTime()) { next_data = data; next_queue = &it->second; next_queue_key = it->first; } CHECK_LE(last_dispatched_time_, next_data->GetTime()) << "Non-sorted data added to queue: '" << it->first << "'"; ++it; } if (next_data == nullptr) { CHECK(queues_.empty()); return; } // If we haven't dispatched any data for this trajectory yet, fast forward // all queues of this trajectory until a common start time has been reached. const common::Time common_start_time = GetCommonStartTime(next_queue_key.trajectory_id); if (next_data->GetTime() >= common_start_time) { // Happy case, we are beyond the 'common_start_time' already. last_dispatched_time_ = next_data->GetTime(); next_queue->callback(next_queue->queue.Pop()); } else if (next_queue->queue.Size() < 2) { if (!next_queue->finished) { // We cannot decide whether to drop or dispatch this yet. CannotMakeProgress(next_queue_key); return; } last_dispatched_time_ = next_data->GetTime(); next_queue->callback(next_queue->queue.Pop()); } else { // We take a peek at the time after next data. If it also is not beyond // 'common_start_time' we drop 'next_data', otherwise we just found the // first packet to dispatch from this queue. std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop(); if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) { last_dispatched_time_ = next_data->GetTime(); next_queue->callback(std::move(next_data_owner)); } } } }
这里的函数, 其实和 OrderedMultiQueue 这个数据结构有比较大的关系了.
我介绍我的理解:
这个类的主要作用就是管理多个有序的传感器数据, 主要的体现就是 std::map<QueueKey, Queue> queues__
它会形成这么一个组织结构:
key1(sensor_1): queue
key2(sensor_2): queue
queue里面是按时间的数据的组织, 他的结构是
struct Queue { common::BlockingQueue<std::unique_ptr<Data>> queue; Callback callback; bool finished = false; };
然后里面的其他的成员是:
common_start_time_per_trajectory_:轨迹id及对应创建轨迹时间
last_dispatched_time_:上一次分发的时间
根据这些知识, 可以解读一下dispatch()
三个 next_*, 其实是找出下一个数据的变量需要保存的东西
然后下面一个 for 的循环 首先是遍历所有的传感器, 找出下一个数据.
主要害怕出错的判断是, 是否已经完成了, 这个数据的时间是否已经超时(比当前数据的时间, 比当前轨道的时间 ),是否累积的数据太少了, 或者找出下一个时间符合的数据.
如果这找出来的数据, 找出得顺利, 那么就调用 这个数据得 callback 函数.
那么, 是在哪儿引入了这个callback函数呢.
真正添加了这个callback函数的是在 CollatedTrajectoryBuilder 构造函数.
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) : sensor_collator_(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_->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)); }); }
那么, 什么时候构造 CollatedTrajectoryBuilder 的呢? 如果之前你有印象的话, 是在 MapBuilder::AddTrajectoryBuilder 的构造函数里面的.
trajectory_builders_.push_back( common::make_unique<CollatedTrajectoryBuilder>( sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback)));
在这一句话, 就会为这一个 trajectory 建立一个 CollatedTrajectoryBuilder . 然后就会调用之上的 CollatedTrajectoryBuilder 的构造函数.
然后, CollatedTrajectoryBuilder 的构造函数的时候, 就会调用 sensor_collator_(Collator)的 AddTrajectory 的函数.
void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids, const Callback& callback) { for (const auto& sensor_id : expected_sensor_ids) { const auto queue_key = QueueKey{trajectory_id, sensor_id}; queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr<Data> data) { callback(sensor_id, std::move(data)); }); queue_keys_[trajectory_id].push_back(queue_key); }
要注意的是上面的queue的结构,每一个传感器一个队列, 并且每个传感器都有一个回调的函数.
可以从代码看出来, 在 Collator 的 AddTrajectory 函数里面, 他们为每个所需要的传感器, 都会创建一个queue的队列, 并且把上面的callback放入进去.
到这儿, 我总结一下, 从雷达产生的Range数据, 会到这列的queue进行保存, 同时, 也是由于每一个数据的到来, 会驱使对已经保存的数据进行处理了.
经历过的对象是 ros::Node 到 ros::SensorBridge 到 CollatedTrajectoryBuilder 到 Collator 里面的queue.
到这儿总结的就是: 每次进来的数据, 都到了queue那儿坐了一下, 然后, 会被 callback 进行处理.
那么, 到这儿, 就得看下, 这个回调到底怎么处理了.
10.
从第9的分析, 可以知道回调函数的实际处理是 CollatedTrajectoryBuilder::HandleCollatedSensorData
void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { // sensor_id 和 这个东西的数据传进来 auto it = rate_timers_.find(sensor_id); // 找出这个 sensor_id的时间 if (it == rate_timers_.end()) { // 如果还没有时间, 则插入头部 it = rate_timers_ .emplace( std::piecewise_construct, std::forward_as_tuple(sensor_id), std::forward_as_tuple( common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; } it->second.Pulse(data->GetTime()); if (std::chrono::steady_clock::now() - last_logging_time_ > common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { // 如果超过一定时间,就进行更新 for (const auto& pair : rate_timers_) { LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); } last_logging_time_ = std::chrono::steady_clock::now(); } data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); }
在除了最后一句之外的前面
找出这个传感器的一个频率的timer, rate_timer, 如果还没有创建, 那么就创建一个.
然后就会给这个timer传递一个脉冲, 其实所谓的脉冲, 你可以看作一个统计信息, 方便统计时间的一个频率.
以便后来方便更新.
我知道大概是这样的作用, 但是为什么要这么做, 我是没看出来. (我还挺好奇,为啥要放这样的一个timer, 不是直接处理)
然后就要调用 data::AddToTrajectoryBuilder 这个.
但是 这里的 data 的类型是什么? 其实我之前一直步知道这有多重要. 所以没区分.
这里, 在 Range data 这个数据类型, 最后是会被 变成 Dispatchable 的数据类型了.
所使用的是 std::unique_ptr<sensor::Data> 的指针. sensor::Data 是一个基类, 可以指向任何的数据类型.
所以, 我啰啰嗦嗦地介绍, 就是为了说明, 这里调用的是 Dispatchable 下面的 AddToTrajectoryBuilder.
而且要注意的是, 它传递的指针是什么? 是 wrapped_trajectory_builder_.get().
我在上面总结过 wrapped_trajectory_builder_ 的类型是 GlobalTrajectoryBuilder (注意和 CollatedTrajectoryBuilder 区分, 我当时这两个弄昏了)
而且因为我自己, 是啥都看一下, 我很早就了解, GlobalTrajectoryBuilder 里面, 是有更新子图部分, 有全局构图部分.
我看到这儿的时候, 我终于知道, 你这个数据怎么进入子图了. (如果有看过我那乱糟糟的笔记, 你会发现我查找在哪insertintosubmap那时狠抓狂)
所以, 看到这, 我心里其实很乐. 我知道起码有一个问题的答案, 我已经摸到门边了.
(如果有人也那么乐, 欢迎留言告诉我, 交个朋友)
11
void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override { trajectory_builder->AddSensorData(sensor_id_, data_); }
这就是 Dispatchable 的 AddToTrajectoryBuilder.
它主要进行的, 就是调用 AddSensorData.
鉴于传递给 trajectory_builder 的是 GlobalTrajectoryBuilder 的对象, 所以调用的是 GlobalTrajectoryBuilder:: AddSensorData
搞清楚这个, 真的很重要, C++的这些继承和模板的特性是不是对于构造架构很好,我还没能体会, 但搞不清楚哪个对象, 阻碍你看代码, 是实实在在的.
而且这个data_的类型是 sensor::TimedPointCloudData .
12
因为这儿, 基本是本次的终点了, 因为这是我需要搞清楚的问题, 雷达进去了, 保存在哪, 保存了, 在哪处理.
之后的, 是我后面继续要讲的拉.不过呢, 就顺便从这里继续看看.
说真, 从这里开始, 我又开始有比较多的不确定了. 顺便瞄一下:
void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { CHECK(local_trajectory_builder_) << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder."; std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result = local_trajectory_builder_->AddRangeData( sensor_id, timed_point_cloud_data); if (matching_result == nullptr) { // The range data has not been fully accumulated yet. return; } kLocalSlamMatchingResults->Increment(); std::unique_ptr<InsertionResult> insertion_result; if (matching_result->insertion_result != nullptr) { kLocalSlamInsertionResults->Increment(); auto node_id = pose_graph_->AddNode( matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->insertion_submaps); CHECK_EQ(node_id.trajectory_id, trajectory_id_); insertion_result = common::make_unique<InsertionResult>(InsertionResult{ node_id, matching_result->insertion_result->constant_data, std::vector<std::shared_ptr<const Submap>>( matching_result->insertion_result->insertion_submaps.begin(), matching_result->insertion_result->insertion_submaps.end())}); } if (local_slam_result_callback_) { local_slam_result_callback_( trajectory_id_, matching_result->time, matching_result->local_pose, std::move(matching_result->range_data_in_local), std::move(insertion_result)); } }
这个函数, 第一个调用 local_trajectory_builder_->AddRangeData()
这后面其实有一个给子图插入范围数据的链.
是以下这样的
GlobalTrajectoryBuilder::AddSensorData() ->
LocalTrajectoryBuilder2D::AddRangeData() ->
LocalTrajectoryBuilder2D::AddAccumulatedRangeData() ->
LocalTrajectoryBuilder2D::InsertIntoSubmap() ->
ActiveSubmaps2D::InsertRangeData() ->
Submap2D::InsertRangeData() ->
所以这里是为子图, 插入了范围数据的路. 但是具体是怎么样, 我还是不知道的.
LocalTrajectoryBuilder2D::AddRangeData() 这里, 返回的是有一个匹配结果, 如果匹配结果是有的, 才会进行下面的.
那么很可能LocalTrajectoryBuilder2D下面是会有匹配的东西的, 以后记得看看.
后面这个 local_slam_result_callback_ , 我看了, 每次匹配的一个回调, 是 GlobalTrajectoryBuilder 的构造函数来进行的赋值的
trajectory_builders_.push_back( common::make_unique<CollatedTrajectoryBuilder>( sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback)));
并且, 这个callback, 是函数传入的
int MapBuilder::AddTrajectoryBuilder( const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback)
调用这个 AddTrajectoryBuilder 的是
const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, ::std::placeholders::_1, ::std::placeholders::_2, ::std::placeholders::_3, ::std::placeholders::_4, ::std::placeholders::_5));
所以, 最终匹配结果的回调函数是:
void MapBuilderBridge::OnLocalSlamResult( const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr<const ::cartographer::mapping:: TrajectoryBuilderInterface::InsertionResult>) { std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data = std::make_shared<TrajectoryState::LocalSlamData>( TrajectoryState::LocalSlamData{time, local_pose, std::move(range_data_in_local)}); cartographer::common::MutexLocker lock(&mutex_); trajectory_state_data_[trajectory_id] = std::move(local_slam_data); }
emmm....不说了拉. 这些其实我都没看, 不过写的时候, 确实是看到这些以前没开始过, 那么就再记录一下.
这一章, 主要解决的, 就是雷达, 进来存储到哪儿. 然后在哪被处理了.
并且附加的, 是如何插入到了子图里面保存的.
但是里面子图怎么保存, 怎么匹配, 这些还没说.
后面第4, 我会看一下imu数据的回调处理
后面第5, 我会看一下odom的数据的回调处理.
这两个大概的过程是一致的.
