您好,登錄后才能下訂單哦!
本篇內(nèi)容主要講解“C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向分析”,感興趣的朋友不妨來看看。本文介紹的方法操作簡單快捷,實用性強(qiáng)。下面就讓小編來帶大家學(xué)習(xí)“C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向分析”吧!
詳細(xì)了解了MapBuilder類, 發(fā)現(xiàn)其構(gòu)造函數(shù), 以及AddTrajectory中有使用到SensorBridge這個類還有sensor_collator_這個變量, 并且似乎是用這個類進(jìn)行傳感器數(shù)據(jù)的傳遞的. 當(dāng)然啦, 如果想建立一個完整的軌跡(Trajectory)和SLAM功能, 我們肯定需要有傳感器的數(shù)據(jù)灌入的.
在MapBuilder的入口類-MapBuilderBridge中, 可以看到一個變量sensor_bridges_
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
sensor_bridges_存儲了一系列SensorBridge類的實例, 并且在MapBuilderBridge::AddTrajectory
的第二步使用, 作用是為當(dāng)前軌跡添加一個SensorBridge.
所以我們重點看看SensorBridge這個類. 我們以最重要的sensor類-LaserScan為例子
我們都知道, ros中軟實時的程序數(shù)據(jù)的入口都是從subscriber的回調(diào)函數(shù)開始. 之前已經(jīng)講過了Node的LaunchSubscriber函數(shù), 用來專門啟動所有傳感器的訂閱. 咱們就看看Node類的HandleLaserScanMessage:
// 調(diào)用SensorBridge的傳感器處理函數(shù)進(jìn)行數(shù)據(jù)處理 void Node::HandleLaserScanMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { absl::MutexLock lock(&mutex_); // 根據(jù)配置,是否將傳感器數(shù)據(jù)跳過 if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { return; } map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleLaserScanMessage(sensor_id, msg); }
我們可以看到, 他最終是調(diào)用了MapBuilderBridge類的sensor_bridge的成員函數(shù)-HandleLaserScanMessage來處理傳入的傳感器的數(shù)據(jù).
咱們再去SensorBridge中看看, 以下是主要代碼部分
// 處理LaserScan數(shù)據(jù), 先轉(zhuǎn)成點云,再傳入trajectory_builder_ void SensorBridge::HandleLaserScanMessage( const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { carto::sensor::PointCloudWithIntensities point_cloud; carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); } void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { if (sensor_to_tracking != nullptr) { trajectory_builder_->AddSensorData( sensor_id, carto::sensor::TimedPointCloudData{ time, sensor_to_tracking->translation().cast<float>(), carto::sensor::TransformTimedPointCloud( ranges, sensor_to_tracking->cast<float>())} ); // 強(qiáng)度始終為空 } }
前幾節(jié)也講過這部分SensorBridge::HandleLaserScanMessage調(diào)用了SensorBridge::HandleRangefinder, 把carto::sensor::TimedPointCloudData這個數(shù)據(jù)類型和sensor_id通過trajectory_builder_的AddSensorData把點云類型傳遞給CollatedTrajectoryBuilder的AddSensorData. 為啥是CollatedTrajectoryBuilder的AddSensorData呢?這塊我也想了很久. 咱們先去sensor_bridge.h中看
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
發(fā)現(xiàn)是TrajectoryBuilderInterface, 這明顯是個父類啊,沒啥意義. 咱們回到SensorBridge的構(gòu)造函數(shù)
/** * @brief 構(gòu)造函數(shù), 并且初始化TfBridge * * @param[in] num_subdivisions_per_laser_scan 一幀數(shù)據(jù)分成幾次發(fā)送 * @param[in] tracking_frame 數(shù)據(jù)都轉(zhuǎn)換到tracking_frame * @param[in] lookup_transform_timeout_sec 查找tf的超時時間 * @param[in] tf_buffer tf_buffer * @param[in] trajectory_builder 軌跡構(gòu)建器 */ SensorBridge::SensorBridge( const int num_subdivisions_per_laser_scan, const std::string& tracking_frame, const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer, carto::mapping::TrajectoryBuilderInterface* const trajectory_builder) : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), trajectory_builder_(trajectory_builder) {}
發(fā)現(xiàn)這個trajectory_builder_是SensorBridge構(gòu)造函數(shù)的最后一個參數(shù), 那么這個SensorBridge是在哪構(gòu)造的呢? 是在map_builder_bridge.cc中, MapBuilderBridge::AddTrajectory的第二步sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>. 我們看到最后一個參數(shù)是
map_builder_->GetTrajectoryBuilder(trajectory_id)
這個map_builder_是MapBuilderBridge構(gòu)造函數(shù)map_builder_(std::move(map_builder)), 而這個map_builder也是父類定義,沒啥參考價值
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder
再向前回溯, 看MapBuilderBridge是咋構(gòu)造的, 發(fā)現(xiàn)是Node的構(gòu)造函數(shù)就構(gòu)造了map_builder_bridge_
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
又要往前回溯, 在node_main.cc中
auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options); Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);
發(fā)現(xiàn)map_builder是cartographer::mapping::CreateMapBuilder給的. 咱們再進(jìn)CreateMapBuilder, 發(fā)現(xiàn)其只是一個工廠函數(shù)
std::unique_ptr<MapBuilderInterface> CreateMapBuilder( const proto::MapBuilderOptions& options) { return absl::make_unique<MapBuilder>(options); }
而這個工廠函數(shù)實例化了MapBuilder這個類, 所以map_builder_->GetTrajectoryBuilder(trajectory_id)調(diào)用的是MapBuilder的GetTrajectoryBuilder. (有一種峰回路轉(zhuǎn)的感覺), 返回trajectory_builders_的軌跡id為trajectory_id的指針,即:
mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder( int trajectory_id) const override { return trajectory_builders_.at(trajectory_id).get(); }
而trajectory_builders_在map_builder.cc中被壓入absl::make_unique<CollatedTrajectoryBuilder>, 如下
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 將3D前端與3D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph4D*>(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter)));
所以最終SensorBridge的trajectory_builder_實際上是CollatedTrajectoryBuilder的地址, 所以SensorBridge的trajectory_builder_的AddSensorData實際上是把數(shù)據(jù)添加到了CollatedTrajectoryBuilder里面, 而不是GlobalTrajectoryBuilder或者LocalTrajectoryBuilder(這三個TrajectoryBuilder都繼承于TrajectoryBuilderInterface)
這塊地方難就難在子類可以用父類代替, 搞不清到底是調(diào)用的哪個子類的成員函數(shù).
既然上面調(diào)用的是CollatedTrajectoryBuilder, 那咱們看看CollatedTrajectoryBuilder這個類的AddSensorData
void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)); } void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) { sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); }
再看看sensor::MakeDispatchable, 在dispatchable.h文件中
// 根據(jù)傳入的data的數(shù)據(jù)類型,自動推斷DataType, 實現(xiàn)一個函數(shù)處理不同類型的傳感器數(shù)據(jù) template <typename DataType> std::unique_ptr<Dispatchable<DataType>> MakeDispatchable( const std::string &sensor_id, const DataType &data) { return absl::make_unique<Dispatchable<DataType>>(sensor_id, data); }
這個函數(shù)通過模板, 實現(xiàn)了一個函數(shù)處理多個類型, 也就是說可以用一個函數(shù)去分發(fā)上到激光雷達(dá),下到IMU的數(shù)據(jù), 值得學(xué)習(xí).
CollatedTrajectoryBuilder::AddData又調(diào)用sensor_collator_->AddSensorData, 用std::move(data), 把data移動給AddSensorData, 給某個Trajectory加入傳感器數(shù)據(jù). 而這個sensor_collator_定義如下:
sensor::CollatorInterface* const sensor_collator_;
又是用父類代替子類, 在CollatedTrajectoryBuilder的構(gòu)造函數(shù)中實現(xiàn)實例化, 這個sensor_collator_實際上是sensor::Collator, 原因是在map_builder.cc中的MapBuilder構(gòu)造函數(shù)中有如下一段程序
// 在 cartographer/configuration_files/map_builder.lua 中設(shè)置 // param: MAP_BUILDER.collate_by_trajectory 默認(rèn)為false if (options.collate_by_trajectory()) { sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>(); } else { // sensor_collator_初始化, 實際使用這個 sensor_collator_ = absl::make_unique<sensor::Collator>(); }
一般collate_by_trajectory設(shè)置為false, 所以是absl::make_unique<sensor::Collator>, 即sensor的Collator
// sensor::Collator的初始化 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)); //傳遞給GlobalTrajectoryBuilder類相應(yīng)的函數(shù) });
咱們進(jìn)到collator這個類中看看, 發(fā)現(xiàn)這個類繼承于CollatorInterface, 再看看collator這個類的AddSensorData
// 向數(shù)據(jù)隊列中添加 傳感器數(shù)據(jù) 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)); }
作用是 向隊列中添加傳感器數(shù)據(jù), 啥是隊列?以后將在線程池部分詳細(xì)說說. 現(xiàn)在簡單看看
queue_是Cartographer的任務(wù)隊列, 用于線程池多任務(wù)序列的儲存與處理.
// Queue keys are a pair of trajectory ID and sensor identifier. OrderedMultiQueue queue_;
也就是說Collator::AddSensorData負(fù)責(zé)把data放在任務(wù)隊列中等待處理并賦一個key, 并不負(fù)責(zé)處理數(shù)據(jù), 所以咱們再往前看看, 看一下OrderedMultiQueue這個類的關(guān)于添加數(shù)據(jù)的成員函數(shù)-Add
OrderedMultiQueue這個類定義在ordered_multi_queue.cc中, 添加數(shù)據(jù)是在Add成員函數(shù)實現(xiàn)的:
// 向數(shù)據(jù)隊列中添加數(shù)據(jù) void OrderedMultiQueue::Add(const QueueKey& queue_key, std::unique_ptr<Data> data) { auto it = queues_.find(queue_key); // 如果queue_key不在queues_中, 就忽略data if (it == queues_.end()) { LOG_EVERY_N(WARNING, 1000) << "Ignored data for queue: '" << queue_key << "'"; return; } // 向數(shù)據(jù)隊列中添加數(shù)據(jù) it->second.queue.Push(std::move(data)); // 傳感器數(shù)據(jù)的分發(fā)處理 Dispatch(); }
可以發(fā)現(xiàn)Add就是生產(chǎn)者, 用于生成并傳遞可用數(shù)據(jù).
Dispatch()這個成員函數(shù)負(fù)責(zé)數(shù)據(jù)分發(fā), 將處于數(shù)據(jù)隊列中的數(shù)據(jù)根據(jù)時間依次傳入回調(diào)函數(shù). 這個后面再看, 咱們先看看it->second.queue.Push(std::move(data));這個部分.
it這個變量就是queues_最后一個數(shù)據(jù),可以理解為最新的一個數(shù)據(jù), 而OrderedMultiQueue的queue_和上一小節(jié)提到的Collator是不同的, 在OrderedMultiQueue中的queue_是定義為一個std::map
std::map<QueueKey, Queue> queues_; // 多個數(shù)據(jù)隊列
所以it->second就是Queue, 而Queue是個定義在OrderedMultiQueue的結(jié)構(gòu)體
struct Queue { common::BlockingQueue<std::unique_ptr<Data>> queue; // 存儲數(shù)據(jù)的隊列 Callback callback; // 本數(shù)據(jù)隊列對應(yīng)的回調(diào)函數(shù) bool finished = false; // 這個queue是否finished };
Push 也就是把data壓入Queue這個結(jié)構(gòu)體中,然后生成map形成個對列. 而這個Push不是push_back, 這個Push是Cartographer自己定義的一種壓棧方法. 定義在blocking_queue.h中,如下...
// Pushes a value onto the queue. Blocks if the queue is full. // 將值壓入隊列. 如果隊列已滿, 則阻塞 void Push(T t) { // 首先定義判斷函數(shù) const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return QueueNotFullCondition(); }; // absl::Mutex的更多信息可看: https://www.jianshu.com/p/d2834abd6796 // absl官網(wǎng): https://abseil.io/about/ // 如果數(shù)據(jù)滿了, 就進(jìn)行等待 absl::MutexLock lock(&mutex_); mutex_.Await(absl::Condition(&predicate)); // 將數(shù)據(jù)加入隊列, 移動而非拷貝 deque_.push_back(std::move(t)); }
發(fā)現(xiàn)Push作用相當(dāng)于阻塞者, 使用了mutex_.Await和鎖用來阻塞數(shù)據(jù)傳入. 看看QueueNotFullCondition這個函數(shù)就一目了然了. 當(dāng)隊列為無限大或者小于queue_size_的時候返回true.
// Returns true iff the queue is not full. // 如果隊列未滿, 則返回true bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_; }
除了阻塞作用, 最大的所用就是把數(shù)據(jù)壓入deque_, 咱們再看看這個deque_
template <typename T> ... std::deque<T> deque_ GUARDED_BY(mutex_);
發(fā)現(xiàn)它是std::deque這個基礎(chǔ)類型,類型決定于模板T, GUARDED_BY(mutex_)表示這個數(shù)據(jù)在使用的時候必須要上鎖, 否則就會報錯.
所以Push的作用就是有阻塞作用的push_back, 負(fù)責(zé)把data壓入OrderedMultiQueue的queue_, 并且在隊列滿的時候阻塞.
咱們再回到OrderedMultiQueue類的Add函數(shù)的Dispatch中, 看看Dispatch函數(shù)有關(guān)數(shù)據(jù)分發(fā)的部分
void OrderedMultiQueue::Dispatch() { while (true) { const Data* next_data = nullptr; Queue* next_queue = nullptr; QueueKey next_queue_key; // 遍歷所有的數(shù)據(jù)隊列, 找到所有數(shù)據(jù)隊列的第一個數(shù)據(jù)中時間最老的一個數(shù)據(jù) for (auto it = queues_.begin(); it != queues_.end();) { const auto* data = it->second.queue.Peek<Data>(); } // end for // 正常情況, 數(shù)據(jù)時間都超過common_start_time if (next_data->GetTime() >= common_start_time) { last_dispatched_time_ = next_data->GetTime(); // 將數(shù)據(jù)傳入 callback() 函數(shù)進(jìn)行處理,并將這個數(shù)據(jù)從數(shù)據(jù)隊列中刪除 next_queue->callback(next_queue->queue.Pop()); } } }
Peek是取出隊列最前面的一個數(shù)據(jù). callback定義在頭文件中, 這個是std::function封裝的一個函數(shù)
using Callback = std::function<void(std::unique_ptr<Data>)>;
而Callback這個函數(shù)到底是啥呢? 這個要看OrderedMultiQueue::AddQueue這個成員函數(shù)
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) { CHECK_EQ(queues_.count(queue_key), 0); queues_[queue_key].callback = std::move(callback); }
我們看到這個函數(shù)把參數(shù)傳入的callback傳入queues_的callback. 那么是誰調(diào)用的OrderedMultiQueue的AddQueue這個函數(shù)呢? 是Collator的AddTrajectory調(diào)用的!
我們在回溯到Collator這個類看看Collator的AddTrajectory成員函數(shù)
/** * @brief 添加軌跡以生成排序的傳感器輸出, 每個topic設(shè)置一個回調(diào)函數(shù) * * @param[in] trajectory_id 新生成的軌跡的id * @param[in] expected_sensor_ids 需要排序的topic名字的集合 * @param[in] callback 2個參數(shù)的回調(diào)函數(shù), 實際是CollatedTrajectoryBuilder::HandleCollatedSensorData()函數(shù) */ void Collator::AddTrajectory( const int trajectory_id, const absl::flat_hash_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, // void(std::unique_ptr<Data> data) 帶了個默認(rèn)參數(shù)sensor_id [callback, sensor_id](std::unique_ptr<Data> data) { callback(sensor_id, std::move(data)); }); queue_keys_[trajectory_id].push_back(queue_key); } }
我們看到它調(diào)用了queue_的AddQueue, 而queue_就是OrderedMultiQueue的實例化 ,所以這里的AddQueue是OrderedMultiQueue的AddQueue. Callback又是個lambda函數(shù)
[callback, sensor_id](std::unique_ptr<Data> data) { callback(sensor_id, std::move(data)); }
這個lambda函數(shù)調(diào)用的是傳入的callback函數(shù), 而這個Collator::AddTrajectory是誰調(diào)用的呢?實際上是CollatedTrajectoryBuilder. 在CollatedTrajectoryBuilder的構(gòu)造函數(shù)中就實現(xiàn)了Collator這個類的初始化, 并且調(diào)用了AddTrajectory這個函數(shù)
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, 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_->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)); //傳遞給GlobalTrajectoryBuilder類相應(yīng)的函數(shù) }); }
所以傳入的參數(shù)是是HandleCollatedSensorData這個函數(shù).
這個函數(shù)才是真正的消費(fèi)者. 看一下HandleCollatedSensorData傳入sensor data的部分:
void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { // 將排序好的數(shù)據(jù)送入 GlobalTrajectoryBuilder中的AddSensorData()函數(shù)中進(jìn)行使用 data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); }
這個函數(shù)的作用是處理按照時間順序分發(fā)的傳感器數(shù)據(jù), 在進(jìn)去到Data的AddToTrajectoryBuilder里看看
Data這個類又是個基類, 里面有個純虛函數(shù)
virtual void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;
這個基類只有一個子類: Dispatchable. 進(jìn)到這個子類中去看看AddToTrajectoryBuilder.
// 調(diào)用傳入的trajectory_builder的AddSensorData() void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override { trajectory_builder->AddSensorData(sensor_id_, data_); }
所以這里的trajectory_builder指的就是CollatedTrajectoryBuilder::HandleCollatedSensorData中調(diào)用的wrapped_trajectory_builder_. 而這個wrapped_trajectory_builder_是啥呢?這又要回溯到CollatedTrajectoryBuilder的初始構(gòu)造中去, 在map_builder.cc中實現(xiàn)
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 將2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder2D( //全局軌跡構(gòu)建器 //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法, //繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個父類 std::move(local_trajectory_builder), //前端構(gòu)建器 trajectory_id, // static_cast<PoseGraph3D*>(pose_graph_.get()), //后端位姿圖 local_slam_result_callback, pose_graph_odometry_motion_filter)));
我們看到wrapped_trajectory_builder_實際上是CreateGlobalTrajectoryBuilder2D, 這個在上回也說到就是GlobalTrajectoryBuilder這個類CreateGlobalTrajectoryBuilder2D, 返回Cartographer的前端和后端.
到這里, 整個Cartographer的傳感器數(shù)據(jù)傳遞過程也就明了了.
從GlobalTrajectoryBuilder2D開始, 數(shù)據(jù)才真正走到SLAM的前端與后端部分.
到此,相信大家對“C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向分析”有了更深的了解,不妨來實際操作一番吧!這里是億速云網(wǎng)站,更多相關(guān)內(nèi)容可以進(jìn)入相關(guān)頻道進(jìn)行查詢,關(guān)注我們,繼續(xù)學(xué)習(xí)!
免責(zé)聲明:本站發(fā)布的內(nèi)容(圖片、視頻和文字)以原創(chuàng)、轉(zhuǎn)載和分享為主,文章觀點不代表本網(wǎng)站立場,如果涉及侵權(quán)請聯(lián)系站長郵箱:is@yisu.com進(jìn)行舉報,并提供相關(guān)證據(jù),一經(jīng)查實,將立刻刪除涉嫌侵權(quán)內(nèi)容。