溫馨提示×
您好，登錄后才能下訂單哦！
點擊 登錄注冊 即表示同意《億速云用戶服務條款》
您好，登錄后才能下訂單哦！
本文小編為大家詳細介紹“C++Node類Cartographer開始軌跡的處理深度源碼分析”，內容詳細，步驟清晰，細節處理妥當，希望這篇“C++Node類Cartographer開始軌跡的處理深度源碼分析”文章能幫助大家解決疑惑，下面跟著小編的思路慢慢深入，一起來學習新知識吧。
在Node.h中,包含了以下幾個部分(程序太大,就不貼了):
構造,析構,拷貝構造,賦值函數的構建
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: 禁止編譯器自動生成默認函數; =default: 要求編譯器生成一個默認函數 // 禁止編譯器自動生成 默認拷貝構造函數(復制構造函數) Node(const Node&) = delete; // 禁止編譯器自動生成 默認賦值函數 Node& operator=(const Node&) = delete;
軌跡有關部分
// 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);
傳感器數據部分
// 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);
其他部分.
比如傳感器采樣設置,位姿推測器等部分.
/**
* @brief
* 聲明ROS的一些topic的發布器, 服務的發布器, 以及將時間驅動的函數與定時器進行綁定
*
* @param[in] node_options 配置文件的內容
* @param[in] map_builder SLAM算法的具體實現
* @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_上鎖, 防止在初始化時數據被更改
absl::MutexLock lock(&mutex_);
// 默認不啟用
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
// Step: 1 聲明需要發布的topic
// 發布SubmapList
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
// 發布軌跡
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
// 發布landmark_pose
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
// 發布約束
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
// 發布tracked_pose, 默認不發布
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 聲明發布對應名字的ROS服務, 并將服務的發布器放入到vector容器中
...
// Step: 3 處理之后的點云的發布器
...
// Step: 4 進行定時器與函數的綁定, 定時發布數據
...
// 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構造函數最重要的兩個傳入變量是node_options和map_builder. 這兩個在上一節中已經詳細說過了,map_builder是Cartographer算法部分,包含了前端和后端.
構造函數通過使用初始化列表去初始化一些私有變量(node_options_, map_builder_bridge_), 然后使用MutexLock上鎖. 初始化列表和智能鎖比較基礎就不詳細介紹了.
總之,這個構造函數使用node_options初始化了map_builder_bridge, 而map_builder_bridge又調用Cartographer算法部分的map_builder(前,后端), 同時還確定了要發布的topic,和可視化所需的topic.
現在介紹一下Node::AddTrajectory。這塊函數是這一節的重中之重了,是node.cc中的核心部分. 它維護了傳感器列表, 添加了一條軌跡,新增了一個位姿估計器,傳感器數據采樣器,還有訂閱所需的topic和注冊對應的回調函數. 為了確保topic沒有重復,他還保存了注冊topic的鍵值對,以供查詢是否重復.
添加軌跡的函數是AddTrajectory
/** * @brief 添加一個新的軌跡 * * @param[in] options 軌跡的參數配置 * @return int 新生成的軌跡的id */ int Node::AddTrajectory(const TrajectoryOptions& options);
調用了Node的函數ComputeExpectedSensorIds, 作用是根據配置文件,去返回一個傳感器列表:
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的名字
......
};它規定了一個傳感器的類型與一個對應的topic的名字. 傳感器的類型是一個限域枚舉(枚舉類). 總之作用就是聯系topic與topic對應的傳感器類型, 以便后續維護.
回到之前的代碼,不難看出, 一個軌跡的傳感器的列表有一下命名規則:
如果只有一個傳感器, 那訂閱的topic就是topic
如果是多個傳感器, 那訂閱的topic就是topic_1,topic_2, 依次類推(多個超聲雷達)
3d slam必須有imu, 2d可有可無, imu的topic的個數只能有一個
里程計可有可無, topic的個數只能有一個
gps可有可無, topic的個數只能有一個
Landmark可有可無, topic的個數只能有一個
接下來就是最重要的函數, AddTrajectory, Cartographer的核心, 傳感器數據和Cartographer算法庫連接處的大門, 調用了ros部分的map_builder_bridge和算法部分的map_builder產生聯系. 聯系的實現方法相當復雜, 將在另一節詳細說
// 調用map_builder_bridge的AddTrajectory, 添加一個軌跡 const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
這一行調用了Map_builder_bridge_的AddTrajectory添加一條軌跡. 傳入的參數有一個std::set<...Sensor_id>類型的變量,std::set是一個容器,可以簡單理解為鍵值對,而鍵就是值,值就是鍵.(比較基礎不細說啦).另一個就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內容. 返回值很簡單,就是新建軌跡的編號. Cartographer允許有多個軌跡同時維護,而且后面我們會發現, Cartographer定位其實就是把建好的地圖和定位作為兩個不同的軌跡實現的. 這個函數是整個Cartographer的功能實現, 方法很復雜, 將會在以后MapBuilder部分詳細說.
這個函數功能是通過IMU和里程計(輪編碼器)去預估下一次可能的位姿,給定位一個初始值
/**
* @brief 新增一個位姿估計器
*
* @param[in] trajectory_id 軌跡id
* @param[in] options 參數配置
*/
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
// 新生成的軌跡的id 不應該在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容器
// 元素是直接構建的（既不復制也不移動）.僅當鍵不存在時才進行插入
// c++11: std::forward_as_tuple tuple的完美轉發
// 該 tuple 在以右值為參數時擁有右值引用數據成員, 否則擁有左值引用數據成員
// c++11: std::piecewise_construct 分次生成tuple的標志常量
// 在map::emplace()中使用forward_as_tuple時必須要加piecewise_construct,不加就報錯
// https://www.cnblogs.com/guxuanqing/p/11396511.html
// 以1ms, 以及重力常數10, 作為參數構造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這個類, 發現功能是使用IMU和/或里程計數據（如果有）來改善預測估計速度與運動. 因為機器人運動,角速度和線速度都不可能變化特別大, 所以可以用上一次的速度去預測下一次的位姿,然后用預測的位姿為初始值去進行優化, 這樣的好處是可以以較小的迭代次數獲得更好的結果,而且不容易陷入局部最小值.
這一部分很簡單, 通過使用配置文件, 去給某條軌跡的各個傳感器得到的值進行采樣,
/**
* @brief 新生成一個傳感器數據采樣器
*
* @param[in] trajectory_id 軌跡id
* @param[in] options 參數配置
*/
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, 發現作用只有控制各個傳感器的采樣頻率.
LaunchSubscribers(options, trajectory_id);
這個函數就挺有意思的, 有值得學的的編程技巧. 同樣, 傳入的參數只有配置文件和軌跡ID.
咱們進入到node.cc里看這個函數本身, 發現它是首先通過配置options判斷了是否用了某個傳感器, 然后把SubscribeWithHandler壓入subscribers_里. 現在, 這里有兩個疑問, subscribers_是啥, SubscribeWithHandler又是啥. 咱們先看subscribers_
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
subscribers_是一個無序表, 鍵是軌跡id, 值是一個數組, 里面放的都是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是一個結構體, 里面是ros的訂閱器和對應的topic名字.
所以subscribers_表示某條軌跡的ros訂閱器以及訂閱topic的名字.
然后就是SubscribeWithHandler
/**
* @brief 在node_handle中訂閱topic,并與傳入的回調函數進行注冊
*
* @tparam MessageType 模板參數,消息的數據類型
* @param[in] handler 函數指針, 接受傳入的函數的地址
* @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構造回調函數,被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);
}));
}這個地方有點晦澀, 不過特別優美, 也是值得學習的部分, 通過SubscribeWithHandler這個函數, 實現了所有傳感器的訂閱,回調函數注冊,以及訂閱器的維護,簡潔明了.
首先, 定義了一個模板, 用來實現同一個函數適應不同的傳感器類型. 返回值是ros的Subscriber類. 對于第一個參數, 這是一個函數指針,也就是函數的地址. 第二個和第三個就是軌跡id和message的topic, 第四個是ros的nodehandle, 掌控ros節點的開啟與關閉,讓我們能使用ros的Subscribe, 不重要. 最后一個參數就是當前node類本身,讓我們可以在后續使用node類相關的方法和變量.
咱們以LaserScan為例看看
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});整體看來,就是通過SubscribeWithHandler這個模板函數,把LaserScan類型的message, 回調函數, 軌跡id, 訂閱話題名稱等打包成ros的subscriber, 連同訂閱話題名稱一起壓入某條軌跡的訂閱維護器.
看看這個回調函數咋傳的: 參數就是回調函數指針(地址), 而回調函數指針的定義在這里定義:
void (Node::*handler)(int, const std::string&, const typename MessageType::ConstPtr&)
可以看到,這個函數是無返回值,函數指針名叫handler的, 傳入參數類型為int, string,和當前模板的傳感器類常量指針.
在看看SubscribeWithHandler. 第一個參數就是接收到LaserScan這個類型的message之后執行的回調函數的地址,讓ros的Subscribe有回調函數調用, 后面的參數在上面已經提到, 就不多說了. 把SubscribeWithHandler的定義和使用對照著看, 發現返回值是一個ros的subscribe, 寫法上和我們自己寫最基礎的ros訂閱是一樣的, topic+數字+回調函數. 回調函數用的是boost::function, 作用和std::function差不多, 用來構造函數. 函數本身又是一個lambda表達式:
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg)
{(node->*handler)(trajectory_id, topic, msg);}這個lambda捕獲了外部的node類本身,也就是this, 軌跡id, topic名字, 傳入了當前message的類型, 執行了該回調函數(HandleLaserScanMessage), 注意啊,現在node->*handler就是HandleLaserScanMessage了, 因為在傳進SubscribeWithHandler的時候就確定了這個函數指針是啥. 所以再來看看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);
}和定義的函數指針一樣傳入參數類型為int, string,和當前模板的傳感器類常量指針. 然后把這些數據MapBuilder的HandleLaserScanMessage進行處理.
讀到這里，這篇“C++Node類Cartographer開始軌跡的處理深度源碼分析”文章已經介紹完畢，想要掌握這篇文章的知識點還需要大家自己動手實踐使用過才能領會，如果想了解更多相關內容的文章，歡迎關注億速云行業資訊頻道。
免責聲明：本站發布的內容（圖片、視頻和文字）以原創、轉載和分享為主，文章觀點不代表本網站立場，如果涉及侵權請聯系站長郵箱：is@yisu.com進行舉報，并提供相關證據，一經查實，將立刻刪除涉嫌侵權內容。
