最主要的數據的流入是范圍數據, 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的數據的回調處理.
這兩個大概的過程是一致的.