C++Node類Cartographer開始軌跡的處理深度詳解
上一節(jié)我們看了一下node_main.cc的內(nèi)容,發(fā)現(xiàn)其中最重要的部分就是Node類,這個類吃下了map_builder類,且里面實(shí)現(xiàn)了傳感器數(shù)據(jù)的處理與分發(fā)(sensor_bridge),還有整個Cartographer算法的調(diào)用(map_builder與map_builder_bridge). 這一節(jié)我們重點(diǎn)看一下Node類的初始化和開始軌跡部分到底做了些什么.
Node.h頭文件
在Node.h中,包含了以下幾個部分(程序太大,就不貼了):
構(gòu)造,析構(gòu),拷貝構(gòu)造,賦值函數(shù)的構(gòu)建
Node(const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* tf_buffer, bool collect_metrics);
~Node();
// c++11: =delete: 禁止編譯器自動生成默認(rèn)函數(shù); =default: 要求編譯器生成一個默認(rèn)函數(shù)
// 禁止編譯器自動生成 默認(rèn)拷貝構(gòu)造函數(shù)(復(fù)制構(gòu)造函數(shù))
Node(const Node&) = delete;
// 禁止編譯器自動生成 默認(rèn)賦值函數(shù)
Node& operator=(const Node&) = delete;軌跡有關(guān)部分
// Finishes all yet active trajectories. void FinishAllTrajectories(); // Finishes a single given trajectory. Returns false if the trajectory did not // exist or was already finished. bool FinishTrajectory(int trajectory_id); // Runs final optimization. All trajectories have to be finished when calling. void RunFinalOptimization(); // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
傳感器數(shù)據(jù)部分
// The following functions handle adding sensor data to a trajectory.
void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg);
void HandleLandmarkMessage(
int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage(
int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);其他部分.
比如傳感器采樣設(shè)置,位姿推測器等部分.
Node類的構(gòu)造函數(shù)
/**
* @brief
* 聲明ROS的一些topic的發(fā)布器, 服務(wù)的發(fā)布器, 以及將時間驅(qū)動的函數(shù)與定時器進(jìn)行綁定
*
* @param[in] node_options 配置文件的內(nèi)容
* @param[in] map_builder SLAM算法的具體實(shí)現(xiàn)
* @param[in] tf_buffer tf
* @param[in] collect_metrics 是否啟用metrics,默認(rèn)不啟用
*/
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
// 將mutex_上鎖, 防止在初始化時數(shù)據(jù)被更改
absl::MutexLock lock(&mutex_);
// 默認(rèn)不啟用
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
// Step: 1 聲明需要發(fā)布的topic
// 發(fā)布SubmapList
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
// 發(fā)布軌跡
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
// 發(fā)布landmark_pose
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
// 發(fā)布約束
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
// 發(fā)布tracked_pose, 默認(rèn)不發(fā)布
if (node_options_.publish_tracked_pose) {
tracked_pose_publisher_ =
node_handle_.advertise<::geometry_msgs::PoseStamped>(
kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
}
// lx add
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
point_cloud_map_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);
}
// Step: 2 聲明發(fā)布對應(yīng)名字的ROS服務(wù), 并將服務(wù)的發(fā)布器放入到vector容器中
...
// Step: 3 處理之后的點(diǎn)云的發(fā)布器
...
// Step: 4 進(jìn)行定時器與函數(shù)的綁定, 定時發(fā)布數(shù)據(jù)
...
// lx add
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kPointCloudMapPublishPeriodSec), // 10s
&Node::PublishPointCloudMap, this));
}
}Node構(gòu)造函數(shù)最重要的兩個傳入變量是node_options和map_builder. 這兩個在上一節(jié)中已經(jīng)詳細(xì)說過了,map_builder是Cartographer算法部分,包含了前端和后端.
構(gòu)造函數(shù)通過使用初始化列表去初始化一些私有變量(node_options_, map_builder_bridge_), 然后使用MutexLock上鎖. 初始化列表和智能鎖比較基礎(chǔ)就不詳細(xì)介紹了.
總之,這個構(gòu)造函數(shù)使用node_options初始化了map_builder_bridge, 而map_builder_bridge又調(diào)用Cartographer算法部分的map_builder(前,后端), 同時還確定了要發(fā)布的topic,和可視化所需的topic.
開始一條軌跡
現(xiàn)在介紹一下Node::AddTrajectory。這塊函數(shù)是這一節(jié)的重中之重了,是node.cc中的核心部分. 它維護(hù)了傳感器列表, 添加了一條軌跡,新增了一個位姿估計(jì)器,傳感器數(shù)據(jù)采樣器,還有訂閱所需的topic和注冊對應(yīng)的回調(diào)函數(shù). 為了確保topic沒有重復(fù),他還保存了注冊topic的鍵值對,以供查詢是否重復(fù).
添加一條軌跡
添加軌跡的函數(shù)是AddTrajectory
/** * @brief 添加一個新的軌跡 * * @param[in] options 軌跡的參數(shù)配置 * @return int 新生成的軌跡的id */ int Node::AddTrajectory(const TrajectoryOptions& options);
添加傳感器維護(hù)功能:
調(diào)用了Node的函數(shù)ComputeExpectedSensorIds, 作用是根據(jù)配置文件,去返回一個傳感器列表:
std::set<SensorId> expected_topics;
在看傳感器列表之前咱們先看一下Cartographer中傳感器類型的定義:
enum class SensorType {
RANGE = 0,
IMU,
ODOMETRY,
FIXED_FRAME_POSE,
LANDMARK,
LOCAL_SLAM_RESULT
};
struct SensorId {
SensorType type; // 傳感器的種類
std::string id; // topic的名字
......
};它規(guī)定了一個傳感器的類型與一個對應(yīng)的topic的名字. 傳感器的類型是一個限域枚舉(枚舉類). 總之作用就是聯(lián)系topic與topic對應(yīng)的傳感器類型, 以便后續(xù)維護(hù).
回到之前的代碼,不難看出, 一個軌跡的傳感器的列表有一下命名規(guī)則:
如果只有一個傳感器, 那訂閱的topic就是topic
如果是多個傳感器, 那訂閱的topic就是topic_1,topic_2, 依次類推(多個超聲雷達(dá))
3d slam必須有imu, 2d可有可無, imu的topic的個數(shù)只能有一個
里程計(jì)可有可無, topic的個數(shù)只能有一個
gps可有可無, topic的個數(shù)只能有一個
Landmark可有可無, topic的個數(shù)只能有一個
添加一個軌跡
接下來就是最重要的函數(shù), AddTrajectory, Cartographer的核心, 傳感器數(shù)據(jù)和Cartographer算法庫連接處的大門, 調(diào)用了ros部分的map_builder_bridge和算法部分的map_builder產(chǎn)生聯(lián)系. 聯(lián)系的實(shí)現(xiàn)方法相當(dāng)復(fù)雜, 將在另一節(jié)詳細(xì)說
// 調(diào)用map_builder_bridge的AddTrajectory, 添加一個軌跡
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);這一行調(diào)用了Map_builder_bridge_的AddTrajectory添加一條軌跡. 傳入的參數(shù)有一個std::set<...Sensor_id>類型的變量,std::set是一個容器,可以簡單理解為鍵值對,而鍵就是值,值就是鍵.(比較基礎(chǔ)不細(xì)說啦).另一個就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內(nèi)容. 返回值很簡單,就是新建軌跡的編號. Cartographer允許有多個軌跡同時維護(hù),而且后面我們會發(fā)現(xiàn), Cartographer定位其實(shí)就是把建好的地圖和定位作為兩個不同的軌跡實(shí)現(xiàn)的. 這個函數(shù)是整個Cartographer的功能實(shí)現(xiàn), 方法很復(fù)雜, 將會在以后MapBuilder部分詳細(xì)說.
新增位姿估計(jì)器
這個函數(shù)功能是通過IMU和里程計(jì)(輪編碼器)去預(yù)估下一次可能的位姿,給定位一個初始值
/**
* @brief 新增一個位姿估計(jì)器
*
* @param[in] trajectory_id 軌跡id
* @param[in] options 參數(shù)配置
*/
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
// 新生成的軌跡的id 不應(yīng)該在extrapolators_中
CHECK(extrapolators_.count(trajectory_id) == 0);
// imu_gravity_time_constant在2d, 3d中都是10
const double gravity_time_constant =
node_options_.map_builder_options.use_trajectory_builder_3d()
? options.trajectory_builder_options.trajectory_builder_3d_options()
.imu_gravity_time_constant()
: options.trajectory_builder_options.trajectory_builder_2d_options()
.imu_gravity_time_constant();
// c++11: map::emplace() 用于通過在容器中插入新元素來擴(kuò)展map容器
// 元素是直接構(gòu)建的（既不復(fù)制也不移動）.僅當(dāng)鍵不存在時才進(jìn)行插入
// c++11: std::forward_as_tuple tuple的完美轉(zhuǎn)發(fā)
// 該 tuple 在以右值為參數(shù)時擁有右值引用數(shù)據(jù)成員, 否則擁有左值引用數(shù)據(jù)成員
// c++11: std::piecewise_construct 分次生成tuple的標(biāo)志常量
// 在map::emplace()中使用forward_as_tuple時必須要加piecewise_construct,不加就報(bào)錯
// https://www.cnblogs.com/guxuanqing/p/11396511.html
// 以1ms, 以及重力常數(shù)10, 作為參數(shù)構(gòu)造PoseExtrapolator
extrapolators_.emplace(
std::piecewise_construct,
std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
}這里面有個重點(diǎn)變量:
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
看PoseExtrapolator這個類, 發(fā)現(xiàn)功能是使用IMU和/或里程計(jì)數(shù)據(jù)（如果有）來改善預(yù)測估計(jì)速度與運(yùn)動. 因?yàn)闄C(jī)器人運(yùn)動,角速度和線速度都不可能變化特別大, 所以可以用上一次的速度去預(yù)測下一次的位姿,然后用預(yù)測的位姿為初始值去進(jìn)行優(yōu)化, 這樣的好處是可以以較小的迭代次數(shù)獲得更好的結(jié)果,而且不容易陷入局部最小值.
數(shù)據(jù)采樣器
這一部分很簡單, 通過使用配置文件, 去給某條軌跡的各個傳感器得到的值進(jìn)行采樣,
/**
* @brief 新生成一個傳感器數(shù)據(jù)采樣器
*
* @param[in] trajectory_id 軌跡id
* @param[in] options 參數(shù)配置
*/
void Node::AddSensorSamplers(const int trajectory_id,
const TrajectoryOptions& options) {
CHECK(sensor_samplers_.count(trajectory_id) == 0);
sensor_samplers_.emplace(
std::piecewise_construct,
std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
options.rangefinder_sampling_ratio,
options.odometry_sampling_ratio,
options.fixed_frame_pose_sampling_ratio,
options.imu_sampling_ratio,
options.landmarks_sampling_ratio));
}看看sensor_samplers_, 定義如下
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
看看TrajectorySensorSamplers, 發(fā)現(xiàn)作用只有控制各個傳感器的采樣頻率.
訂閱話題與注冊回調(diào)函數(shù)
LaunchSubscribers(options, trajectory_id);
這個函數(shù)就挺有意思的, 有值得學(xué)的的編程技巧. 同樣, 傳入的參數(shù)只有配置文件和軌跡ID.
咱們進(jìn)入到node.cc里看這個函數(shù)本身, 發(fā)現(xiàn)它是首先通過配置options判斷了是否用了某個傳感器, 然后把SubscribeWithHandler壓入subscribers_里. 現(xiàn)在, 這里有兩個疑問, subscribers_是啥, SubscribeWithHandler又是啥. 咱們先看subscribers_
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
subscribers_是一個無序表, 鍵是軌跡id, 值是一個數(shù)組, 里面放的都是Subscriber:
struct Subscriber {
::ros::Subscriber subscriber;
// ::ros::Subscriber::getTopic() does not necessarily return the same
// std::string
// it was given in its constructor. Since we rely on the topic name as the
// unique identifier of a subscriber, we remember it ourselves.
std::string topic;
};Subscriber是一個結(jié)構(gòu)體, 里面是ros的訂閱器和對應(yīng)的topic名字.
所以subscribers_表示某條軌跡的ros訂閱器以及訂閱topic的名字.
然后就是SubscribeWithHandler
/**
* @brief 在node_handle中訂閱topic,并與傳入的回調(diào)函數(shù)進(jìn)行注冊
*
* @tparam MessageType 模板參數(shù),消息的數(shù)據(jù)類型
* @param[in] handler 函數(shù)指針, 接受傳入的函數(shù)的地址
* @param[in] trajectory_id 軌跡id
* @param[in] topic 訂閱的topic名字
* @param[in] node_handle ros的node_handle
* @param[in] node node類的指針
* @return ::ros::Subscriber 訂閱者
*/
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
void (Node::*handler)(int, const std::string&,
const typename MessageType::ConstPtr&),
const int trajectory_id, const std::string& topic,
::ros::NodeHandle* const node_handle, Node* const node) {
return node_handle->subscribe<MessageType>(
topic, kInfiniteSubscriberQueueSize, // kInfiniteSubscriberQueueSize = 0
// 使用boost::function構(gòu)造回調(diào)函數(shù),被subscribe注冊
boost::function<void(const typename MessageType::ConstPtr&)>(
// c++11: lambda表達(dá)式
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}這個地方有點(diǎn)晦澀, 不過特別優(yōu)美, 也是值得學(xué)習(xí)的部分, 通過SubscribeWithHandler這個函數(shù), 實(shí)現(xiàn)了所有傳感器的訂閱,回調(diào)函數(shù)注冊,以及訂閱器的維護(hù),簡潔明了.
首先, 定義了一個模板, 用來實(shí)現(xiàn)同一個函數(shù)適應(yīng)不同的傳感器類型. 返回值是ros的Subscriber類. 對于第一個參數(shù), 這是一個函數(shù)指針,也就是函數(shù)的地址. 第二個和第三個就是軌跡id和message的topic, 第四個是ros的nodehandle, 掌控ros節(jié)點(diǎn)的開啟與關(guān)閉,讓我們能使用ros的Subscribe, 不重要. 最后一個參數(shù)就是當(dāng)前node類本身,讓我們可以在后續(xù)使用node類相關(guān)的方法和變量.
咱們以LaserScan為例看看
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});整體看來,就是通過SubscribeWithHandler這個模板函數(shù),把LaserScan類型的message, 回調(diào)函數(shù), 軌跡id, 訂閱話題名稱等打包成ros的subscriber, 連同訂閱話題名稱一起壓入某條軌跡的訂閱維護(hù)器.
看看這個回調(diào)函數(shù)咋傳的: 參數(shù)就是回調(diào)函數(shù)指針(地址), 而回調(diào)函數(shù)指針的定義在這里定義:
void (Node::*handler)(int, const std::string&,
const typename MessageType::ConstPtr&)可以看到,這個函數(shù)是無返回值,函數(shù)指針名叫handler的, 傳入?yún)?shù)類型為int, string,和當(dāng)前模板的傳感器類常量指針.
在看看SubscribeWithHandler. 第一個參數(shù)就是接收到LaserScan這個類型的message之后執(zhí)行的回調(diào)函數(shù)的地址,讓ros的Subscribe有回調(diào)函數(shù)調(diào)用, 后面的參數(shù)在上面已經(jīng)提到, 就不多說了. 把SubscribeWithHandler的定義和使用對照著看, 發(fā)現(xiàn)返回值是一個ros的subscribe, 寫法上和我們自己寫最基礎(chǔ)的ros訂閱是一樣的, topic+數(shù)字+回調(diào)函數(shù). 回調(diào)函數(shù)用的是boost::function, 作用和std::function差不多, 用來構(gòu)造函數(shù). 函數(shù)本身又是一個lambda表達(dá)式:
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg)
{(node->*handler)(trajectory_id, topic, msg);}這個lambda捕獲了外部的node類本身,也就是this, 軌跡id, topic名字, 傳入了當(dāng)前message的類型, 執(zhí)行了該回調(diào)函數(shù)(HandleLaserScanMessage), 注意啊,現(xiàn)在node->*handler就是HandleLaserScanMessage了, 因?yàn)樵趥鬟M(jìn)SubscribeWithHandler的時候就確定了這個函數(shù)指針是啥. 所以再來看看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);
}和定義的函數(shù)指針一樣傳入?yún)?shù)類型為int, string,和當(dāng)前模板的傳感器類常量指針. 然后把這些數(shù)據(jù)MapBuilder的HandleLaserScanMessage進(jìn)行處理. MapBuilder這一部分內(nèi)容較多,還是要單獨(dú)講.
到此這篇關(guān)于C++Node類Cartographer開始軌跡的處理深度詳解的文章就介紹到這了,更多相關(guān)C++ Cartographer開始軌跡內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！
相關(guān)文章
深入C++浮點(diǎn)數(shù)無效值定義與判定的解決辦法
本篇文章是對C++中浮點(diǎn)數(shù)無效值定義與判定進(jìn)行了介紹，需要的朋友參考下2013-05-05
Java C++算法題解leetcode1592重新排列單詞間的空格
這篇文章主要為大家介紹了Java C++算法題解leetcode1592重新排列單詞間的空格示例，有需要的朋友可以借鑒參考下，希望能夠有所幫助，祝大家多多進(jìn)步，早日升職加薪2022-09-09
詳解C/C++實(shí)現(xiàn)各種字符轉(zhuǎn)換方法合集
這篇文章主要為大家詳細(xì)介紹了C/C++中實(shí)現(xiàn)各種字符轉(zhuǎn)換的方法，文中的示例代碼講解詳細(xì)，對我們學(xué)習(xí)C++具有一定借鑒價值，需要的可以參考一下2022-09-09
C++連接mysql數(shù)據(jù)庫(改進(jìn)版)
C++是大家都非常熟悉的,也是大家平時辦公中經(jīng)常會用到的,下面這篇文章主要給大家介紹了關(guān)于C++連接mysql數(shù)據(jù)庫的相關(guān)資料,文中通過代碼介紹的非常詳細(xì),需要的朋友可以參考下2023-12-12
這篇文章主要為大家詳細(xì)介紹了C語言實(shí)現(xiàn)代碼雨效果，文中示例代碼介紹的非常詳細(xì)，具有一定的參考價值，感興趣的小伙伴們可以參考一下2022-05-05
使用C++和代理IP實(shí)現(xiàn)天氣預(yù)報(bào)的采集
在當(dāng)今的互聯(lián)網(wǎng)時代,網(wǎng)絡(luò)信息的獲取變得日益重要,天氣預(yù)報(bào)信息作為日常生活的重要參考,其獲取方式也隨著技術(shù)的發(fā)展而不斷變化,在本文中,我們將探討如何使用C++和代理IP來采集天氣預(yù)報(bào)信息,文中通過代碼講解的非常詳細(xì),需要的朋友可以參考下2023-12-12
C++實(shí)現(xiàn)快捷店會員管理系統(tǒng)
這篇文章主要為大家詳細(xì)介紹了C++實(shí)現(xiàn)快捷店會員管理系統(tǒng)，文中示例代碼介紹的非常詳細(xì)，具有一定的參考價值，感興趣的小伙伴們可以參考一下2022-03-03
這篇文章主要介紹了C++讀寫INI配置文件的類,實(shí)例分析了C++操作ini配置文件的相關(guān)技巧,需要的朋友可以參考下2015-06-06
在宏定義中，“宏名稱”和“宏字符串”是通過“空格”來區(qū)分的,某些朋友不要混淆了，接下來請祥看本文2012-12-12
c語言實(shí)現(xiàn)MD5算法的項(xiàng)目實(shí)踐
MD5用于確保信息傳輸完整一致，是計(jì)算機(jī)廣泛使用的雜湊算法之一，本文主要介紹了c語言實(shí)現(xiàn)MD5算法，具有一定的參考價值，感興趣的可以了解一下2023-08-08