C++?Cartographer源碼中關于傳感器的數據傳遞實現
上一節(jié)我們看了添加軌跡的相關處理，發(fā)現在此調用了LaunchSubscribers這個函數來訂閱相關傳感器數據. 這一節(jié)看看這些訂閱的傳感器數據的流動過程. 咱們進入Node::LaunchSubscribers看看
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id);可以看到, 傳入的參數只有當前軌跡id和配置參數. 咱們看看激光雷達和超聲雷達的內容
激光雷達,超聲雷達與點云數據處理與傳遞
存在3中雷達數據, 一種是單線雷達點云(LaserScan), 一種是多回聲波雷達點云(MultiEchoLaserScanMessage),一種是點云(PointCloud2)可以處理多線雷達,
// laser_scan 的訂閱與注冊回調函數, 多個laser_scan 的topic 共用同一個回調函數
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});
}
// multi_echo_laser_scans的訂閱與注冊回調函數
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this),
topic});
}以激光雷達為例, 進入HandleLaserScanMessage
// 調用SensorBridge的傳感器處理函數進行數據處理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根據配置,是否將傳感器數據跳過
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}注釋已經寫了一部分內容了,除此之外還調用了sensor_bridge的同名函數HandleLaserScanMessage, 看參數表示這個激光雷達的數據是某個軌跡id的某個sensorid的信息.
在進到這里看看. 在sensor_bridge.cc中實現
// 處理LaserScan數據, 先轉成點云,再傳入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);
}ToPointCloudWithIntensities把ros格式的LaserScan轉化成carto格式的PointCloudWithIntensities,
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time> 返回值是一個打包好的std::tuple, 內容是carto格式的點云和時間戳, 然后再由std::tie解壓成變量point_cloud和time, 傳入真正的數據處理函數- HandleLaserScan. 進到這個函數再看看
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;這一塊標會一幀雷達數據被分成多少部分處理. 這個分塊函數還是挺有意思的, 學習一下自己以后也可以巧妙的均勻切分數據了.
接下來就是檢查分割后點云是否有錯: 通過時間先后, 因為上一段分塊的點云的末尾的一個點的時間戳不可能比這一塊點云的第一個點的時間戳還早,否則就是錯的.
if (it != sensor_to_previous_subdivision_time_.end() &&
// 上一段點云的時間不應該大于等于這一段點云的時間
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
// 更新對應sensor_id的時間戳
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
// 檢查點云的時間
for (auto& point : subdivision) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);然后調用當前類的HandleRangefinder方法,處理分段后的點云.
HandleRangefinder() 函數將點云點云從雷達坐標系下轉到tracking_frame坐標系系下, 并轉成
TimedPointCloudData格式的點云, 然后才傳入到cartographer中.
值得注意的是,不論是激光雷達還是超聲雷達,最終都調用的是SensorBridge::HandleLaserScan.
咱們再看看最后一種激光傳感器類型-點云,不同于激光雷達和超聲雷達, 這個Cartographer定義的傳感器類型接受直接的點云數據, 所以免去了數據分割等數據處理. 他調用了SensorBridge::HandlePointCloud2Message這個方法, 這個方法同樣調用了HandleRangefinder. 所以HandleRangefinder這個函數實現了各種雷達與點云傳感器類型的統(tǒng)一.可以看到調用關系上點云少了一步.
里程計與IMU數據的走向
咱們看看Node::HandleImuMessage,
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock 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);
// extrapolators_使用里程計數據進行位姿預測
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}發(fā)現imu的數據走向有2個:
1. 傳入PoseExtrapolator,用于位姿預測與重力方向的確定,
2. 傳入SensorBridge,使用其傳感器處理函數進行imu數據處理
IMU數據通過sensor_bridge的ToImuData轉化為了位姿推測器的imudata, 然后傳遞給了位姿推測器的AddImuData, 用于位姿預測. 同樣, HandleImuMessage也采用了ToImuData轉化了imu的data, 然后調用trajectory_builder_的AddSensorData添加數據到軌跡中, 實現定位.
在ToImuData方法中:
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id));這個是通過tf得到傳感器相對于機器人坐標系的轉換關系, 由于imu的hz是很高的(1000hz左右), 所以機器人一般以IMU所在位子為機器人的坐標, 以減小計算量.
里程計和IMU數據是一樣的, 都是兩個數據走向, 參照IMU.
GPS與landmark數據走向
landmark從Node類中的回調函數進來,只有一個走向,直接傳入
SensorBridge::HandleLandmarkMessage()函數中,通過tf轉換到機器人坐標系下后,再從SensorBridge傳遞給cartographer.
GPS數據和landmark數據一樣, 只有一個走向, 和landmark不同的是, GPS數據需要進行坐標轉換,因為GPS得到的raw data是lla坐標, 需要轉化為ECEF坐標, 然后計算兩幀GPS數據之間的相對坐標轉換. 程序如下:
// 通過這個坐標變換 乘以 之后的gps數據,就相當于減去了一個固定的坐標,從而得到了gps數據間的相對坐標變換
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::FixedFramePoseData{
time, absl::optional<Rigid3d>(Rigid3d::Translation(
ecef_to_local_frame_.value() *
LatLongAltToEcef(msg->latitude, msg->longitude,
msg->altitude)))});總結
綜上所述, 所有的數據都要通過處理轉化為Cartographer能夠接受的數據結構, 然后通過調用trajectory_builder_的addsensordata方法, 把傳感器數據給到Cartographer算法部. 這一部分沒啥難點.
到此這篇關于C++ Cartographer源碼中關于傳感器的數據傳遞實現的文章就介紹到這了,更多相關C++ Cartographer數據傳遞內容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關文章希望大家以后多多支持腳本之家！
相關文章
這篇文章主要介紹了詳解C++中的自動存儲，幫助大家更好的理解和學習C++,感興趣的朋友可以了解下2020-09-09
這篇文章主要為大家詳細介紹了C語言實現順序表的實現代碼，具有一定的參考價值，感興趣的小伙伴們可以參考一下2017-09-09
這篇文章主要介紹了C語言 數據結構之連續(xù)存儲數組的算法的相關資料,需要的朋友可以參考下2017-01-01
C或C++報錯:ld returned 1 exit status報錯的原因及解
這篇文章主要介紹了C或C++報錯:ld returned 1 exit status報錯的原因及解決方法,本文給大家介紹的非常詳細，對大家的學習或工作具有一定的參考借鑒價值，需要的朋友可以參考下2023-02-02
這篇文章主要為大家詳細介紹了qt5之QFile讀寫文件功能，文中示例代碼介紹的非常詳細，具有一定的參考價值，感興趣的小伙伴們可以參考一下2022-02-02
這篇文章主要介紹了C++11 并發(fā)指南之std::mutex詳解，文中通過示例代碼介紹的非常詳細，對大家的學習或者工作具有一定的參考學習價值，需要的朋友們下面隨著小編來一起學習學習吧2020-02-02
這篇文章主要為大家詳細介紹了C語言實現猜數游戲，文中示例代碼介紹的非常詳細，具有一定的參考價值，感興趣的小伙伴們可以參考一下2020-12-12
這篇文章主要為大家詳細介紹了C語言代碼實現通訊錄管理系統(tǒng)，文中示例代碼介紹的非常詳細，具有一定的參考價值，感興趣的小伙伴們可以參考一下2022-06-06
數組是一組有序的數據的集合，數組中元素類型相同，由數組名和下標唯一地確定，數組中數據不僅數據類型相同，而且在計算機內存里連續(xù)存放，地址編號最低的存儲單元存放數組的起始元素，地址編號最高的存儲單元存放數組的最后一個元素2022-05-05
這篇文章介紹了C++超集C++/CLI模塊的基本語法，文中通過示例代碼介紹的非常詳細。對大家的學習或工作具有一定的參考借鑒價值，需要的朋友可以參考下2022-07-07