溫馨提示×
您好，登錄后才能下訂單哦！
點擊 登錄注冊 即表示同意《億速云用戶服務(wù)條款》
您好，登錄后才能下訂單哦！
本文小編為大家詳細介紹“C++Node類Cartographer開始軌跡的處理深度源碼分析”，內(nèi)容詳細，步驟清晰，細節(jié)處理妥當，希望這篇“C++Node類Cartographer開始軌跡的處理深度源碼分析”文章能幫助大家解決疑惑，下面跟著小編的思路慢慢深入，一起來學習新知識吧。
在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: 禁止編譯器自動生成默認函數(shù); =default: 要求編譯器生成一個默認函數(shù) // 禁止編譯器自動生成 默認拷貝構(gòu)造函數(shù)(復(fù)制構(gòu)造函數(shù)) Node(const Node&) = delete; // 禁止編譯器自動生成 默認賦值函數(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è)置,位姿推測器等部分.
/**
* @brief
* 聲明ROS的一些topic的發(fā)布器, 服務(wù)的發(fā)布器, 以及將時間驅(qū)動的函數(shù)與定時器進行綁定
*
* @param[in] node_options 配置文件的內(nèi)容
* @param[in] map_builder SLAM算法的具體實現(xiàn)
* @param[in] tf_buffer tf
* @param[in] collect_metrics 是否啟用metrics,默認不啟用
*/
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_);
// 默認不啟用
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, 默認不發(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 處理之后的點云的發(fā)布器
...
// Step: 4 進行定時器與函數(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)詳細說過了,map_builder是Cartographer算法部分,包含了前端和后端.
構(gòu)造函數(shù)通過使用初始化列表去初始化一些私有變量(node_options_, map_builder_bridge_), 然后使用MutexLock上鎖. 初始化列表和智能鎖比較基礎(chǔ)就不詳細介紹了.
總之,這個構(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中的核心部分. 它維護了傳感器列表, 添加了一條軌跡,新增了一個位姿估計器,傳感器數(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);
調(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ù)維護.
回到之前的代碼,不難看出, 一個軌跡的傳感器的列表有一下命名規(guī)則:
如果只有一個傳感器, 那訂閱的topic就是topic
如果是多個傳感器, 那訂閱的topic就是topic_1,topic_2, 依次類推(多個超聲雷達)
3d slam必須有imu, 2d可有可無, imu的topic的個數(shù)只能有一個
里程計可有可無, 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)系的實現(xiàn)方法相當復(fù)雜, 將在另一節(jié)詳細說
// 調(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ǔ)不細說啦).另一個就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內(nèi)容. 返回值很簡單,就是新建軌跡的編號. Cartographer允許有多個軌跡同時維護,而且后面我們會發(fā)現(xiàn), Cartographer定位其實就是把建好的地圖和定位作為兩個不同的軌跡實現(xiàn)的. 這個函數(shù)是整個Cartographer的功能實現(xiàn), 方法很復(fù)雜, 將會在以后MapBuilder部分詳細說.
這個函數(shù)功能是通過IMU和里程計(輪編碼器)去預(yù)估下一次可能的位姿,給定位一個初始值
/**
* @brief 新增一個位姿估計器
*
* @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() 用于通過在容器中插入新元素來擴展map容器
// 元素是直接構(gòu)建的（既不復(fù)制也不移動）.僅當鍵不存在時才進行插入
// c++11: std::forward_as_tuple tuple的完美轉(zhuǎn)發(fā)
// 該 tuple 在以右值為參數(shù)時擁有右值引用數(shù)據(jù)成員, 否則擁有左值引用數(shù)據(jù)成員
// c++11: std::piecewise_construct 分次生成tuple的標志常量
// 在map::emplace()中使用forward_as_tuple時必須要加piecewise_construct,不加就報錯
// 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));
}這里面有個重點變量:
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
看PoseExtrapolator這個類, 發(fā)現(xiàn)功能是使用IMU和/或里程計數(shù)據(jù)（如果有）來改善預(yù)測估計速度與運動. 因為機器人運動,角速度和線速度都不可能變化特別大, 所以可以用上一次的速度去預(yù)測下一次的位姿,然后用預(yù)測的位姿為初始值去進行優(yōu)化, 這樣的好處是可以以較小的迭代次數(shù)獲得更好的結(jié)果,而且不容易陷入局部最小值.
這一部分很簡單, 通過使用配置文件, 去給某條軌跡的各個傳感器得到的值進行采樣,
/**
* @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)作用只有控制各個傳感器的采樣頻率.
LaunchSubscribers(options, trajectory_id);
這個函數(shù)就挺有意思的, 有值得學的的編程技巧. 同樣, 傳入的參數(shù)只有配置文件和軌跡ID.
咱們進入到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ù)進行注冊
*
* @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表達式
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}這個地方有點晦澀, 不過特別優(yōu)美, 也是值得學習的部分, 通過SubscribeWithHandler這個函數(shù), 實現(xiàn)了所有傳感器的訂閱,回調(diào)函數(shù)注冊,以及訂閱器的維護,簡潔明了.
首先, 定義了一個模板, 用來實現(xiàn)同一個函數(shù)適應(yīng)不同的傳感器類型. 返回值是ros的Subscriber類. 對于第一個參數(shù), 這是一個函數(shù)指針,也就是函數(shù)的地址. 第二個和第三個就是軌跡id和message的topic, 第四個是ros的nodehandle, 掌控ros節(jié)點的開啟與關(guān)閉,讓我們能使用ros的Subscribe, 不重要. 最后一個參數(shù)就是當前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, 連同訂閱話題名稱一起壓入某條軌跡的訂閱維護器.
看看這個回調(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,和當前模板的傳感器類常量指針.
在看看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表達式:
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg)
{(node->*handler)(trajectory_id, topic, msg);}這個lambda捕獲了外部的node類本身,也就是this, 軌跡id, topic名字, 傳入了當前message的類型, 執(zhí)行了該回調(diào)函數(shù)(HandleLaserScanMessage), 注意啊,現(xiàn)在node->*handler就是HandleLaserScanMessage了, 因為在傳進SubscribeWithHandler的時候就確定了這個函數(shù)指針是啥. 所以再來看看HandleLaserScanMessage
// 調(diào)用SensorBridge的傳感器處理函數(shù)進行數(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,和當前模板的傳感器類常量指針. 然后把這些數(shù)據(jù)MapBuilder的HandleLaserScanMessage進行處理.
讀到這里，這篇“C++Node類Cartographer開始軌跡的處理深度源碼分析”文章已經(jīng)介紹完畢，想要掌握這篇文章的知識點還需要大家自己動手實踐使用過才能領(lǐng)會，如果想了解更多相關(guān)內(nèi)容的文章，歡迎關(guān)注億速云行業(yè)資訊頻道。
免責聲明：本站發(fā)布的內(nèi)容（圖片、視頻和文字）以原創(chuàng)、轉(zhuǎn)載和分享為主，文章觀點不代表本網(wǎng)站立場，如果涉及侵權(quán)請聯(lián)系站長郵箱：is@yisu.com進行舉報，并提供相關(guān)證據(jù)，一經(jīng)查實，將立刻刪除涉嫌侵權(quán)內(nèi)容。
億速云公眾號
手機網(wǎng)站二維碼