點云的地圖的發送邏輯中,我發現每次使用rostopic echo 時只會打印一次,然后就不會再打印了。并且rviz中也是始終都會顯示的,這里面其實就是用到了latched持久話機制,可以接受這最后一次發布的消息。
我們通過一個具體的項目來學習和認識這個過程
latched話題:
你創建的發布者使用了 latching(持久化)機制 (advertise<sensor_msgs::PointCloud2>(map_topic, 10, true) 中的 true 參數)。Latching 意味著最后一次發布的信息會被保存下來,新的訂閱者連接時會立即接收到這條消息。因此,每當一個新的訂閱者(如 rostopic echo)連接到這個話題時,它都會立即接收到那條被 latched 的消息。
源碼信息:
頭文件
#ifndef _MAP_LOADER_HPP_
#define _MAP_LOADER_HPP_#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <vector>
#include <pcl_ros/transforms.h>using namespace std;class MapLoader {
public:ros::Publisher pc_map_pub_;vector<string> file_list_;MapLoader(ros::NodeHandle &nh);private:sensor_msgs::PointCloud2 CreatePcd();
};#endif
map_load.cpp
#include <map_load/map_load.h>using namespace std;MapLoader::MapLoader(ros::NodeHandle &nh){std::string pcd_file, map_topic;nh.param<string>("pcd_path", pcd_file_path, "");nh.param<string>("map_topic", map_topic, "point_map");pc_map_pub = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);//latched話題:創建的發布者使用了 latching(持久化)機制 // (advertise<sensor_msgs::PointCloud2>(map_topic, 10, true) 中的 true 參數)// Latching 意味著最后一次發布的信息會被保存下來,新的訂閱者連接時會立即接收到這條消息。// 每當一個新的訂閱者(如 rostopic echo)連接到這個話題時,它都會立即接收到那條被 latched 的消息file_list_.push_back(pcd_file_path);auto pc_msg = CreatePcd();cout << "pc_msg.width" << "\n" << pc_msg.width << endl;if (pc_msg.width != 0) {pc_msg.header.frame_id = "map";pc_map_pub_.publish(pc_msg);}}sensor_msgs::PointCloud2 MapLoader::CreatePcd() {sensor_msgs::PointCloud2 pc_msg;pcl::PointCloud<pcl::PointXYZ> pcd, part;int pcd_width = 0;for (const string& path : file_list_) {if (pcd_width == 0) {if (pcl::io::loadPCDFile(path.c_str(), pcd) == -1) {cerr << "load failed " << path << endl;}}else {if (pcl::io::loadPCDFile(path.c_str(), part) == -1) {cerr << "load failed " << path << endl;}pcd.width += part.width;pcd.row_step += part.row_step;pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());}cerr << " load " << path << endl;if (!ros::ok()) break;}return pcd;
}int main(int argc, char** argv) {ros::init(argc, argv, "map_loader");ROS_INFO("\033[32m ---> \033[0m Map Loader Started.");ros::NodeHandle nh;ros::spin();return 0;
}
然后我們看launch文件中的傳參
<launch><!--- Sim Time --><param name="/use_sim_time" value="false" /><!--- Run Rviz--><!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find map_load)/rviz/map_show.rviz" /> --><!--- MapLoader --> <arg name="pcd_path" default="$(find map_load)/map/map_Statistical_filter.pcd"/><arg name="map_topic" default="point_map"/><node pkg="map_load" type="mapload" name="mapLoader" output="screen"><param name="pcd_path" value="$(arg pcd_path)"/><param name="map_topic" value="$(arg map_topic)"/></node></launch>
主要就是兩個參數,一個是pcd的路徑,一個是map發布的話題運行后:
此時可以查看一下話題的發布情況,可以看到
rostopic list
rostopic hz /mapLoader/point_map
所以就是沒有數據在這里發布,也就不會占用帶寬.我們卻是可以打印出來的:
rviz中也是可以顯示出對應的點云地圖的,這個方式非常適合那些不會變化的數據的內容.
我們從原理上深入分析一下:
ROS Publisher Latch 機制詳解
1. 核心含義
- ??功能定義??:
latch=true
啟用??持久化消息??機制,發布者會緩存最后一次發送的消息。 - ??核心作用??:
新訂閱者連接到話題時,??立即收到緩存的最后一條消息??,無需等待下一次發布。
2. 底層機制
機制 | 說明 |
---|---|
消息存儲 | ROS內部維護一個長度為1的隊列,僅保留最新消息 |
觸發條件 | 新訂閱者通過 ros::Subscriber 連接時觸發 |
生命周期 | 緩存消息保留至新消息發布或節點關閉 |
3. 適用場景
場景1:靜態數據發布(如地圖)
// 只需加載一次,新節點需立即獲取
pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>("map", 10, true);
場景2:低頻更新數據(如初始位姿)
// 新的GUI模塊啟動后需立刻顯示當前位置
pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("initial_pose", 1, true);
場景3:服務類數據(如全局路徑)
// 控制模塊可能稍后啟動,需確保其能獲取路徑
path_pub_ = nh.advertise<nav_msgs::Path>("global_plan", 1, true);
4. 禁用 Latch 的對比
// 默認行為:latch=false
pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>("map", 10);
新訂閱者體驗 立即收到最后消息 必須等待下次 publish() 調用
數據連續性 適合單次發布場景 需要持續發布維持數據流
5. 實際效果驗證
# 終端1:發布地圖(僅一次)
rosrun map_loader map_loader _pcd_path:=/home/user/map.pcd# 終端2:查看話題
rostopic echo /map # 立即顯示數據
# 禁用 Latch 時的表現
rostopic echo /map # 無輸出,直到再次調用 publish()
6注意事項
- List item
??內存管理??
大消息(如10萬+點云)會持續占用內存,需評估消息大小
- ??ROS 2 對應機制??
通過QoS策略實現:
// ROS 2 等效實現
auto qos = rclcpp::QoS(10).durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
pc_map_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("map", qos);
- 最佳實踐指南
ROS Publisher Latch 參數對比表
??對比維度?? | latch=true (持久化) | latch=false (非持久化,默認) |
---|---|---|
??新訂閱者連接行為?? | 立即收到最后一次發布的消息 | 必須等待下一次 publish() 調用 |
??消息存儲機制?? | ROS內部維護長度為1的隊列,保留最新消息 | 不存儲任何歷史消息 |
??適用數據類型?? | 靜態數據(地圖/參數)、低頻更新數據(路徑/位姿) | 高頻動態數據(激光/圖像/IMU) |
??內存占用?? | 存儲完整消息副本(需注意大消息內存消耗) | 無額外占用 |
??典型場景?? | 初始化配置、服務類結果、需即時同步的全局數據 | 實時數據流、連續狀態更新 |
??調試驗證方法?? | rostopic info /topic 顯示 LATCH: true | rostopic echo /topic 只在發布后顯示新數據 |
??隊列機制?? | 固定隊列長度=1(強制保留最新消息) | 隊列長度由 advertise() 的第二個參數定義 |
??ROS 2 等效實現?? | DurabilityPolicy::TRANSIENT_LOCAL | DurabilityPolicy::VOLATILE |
??典型代碼示例?? | pub = nh.advertise<MapMsg>("map", 10, true); | pub = nh.advertise<LaserScan>("scan", 10); |
??數據更新影響?? | 新消息會覆蓋緩存,后續訂閱者收到最新版本 | 舊訂閱者按隊列長度接收歷史消息,新訂閱者只收新消息 |