ROS2 Humble Gazebo仿真中导出3D激光雷达点云及RTAB-Map问题

一、点云保存为.pcd/.ply格式实现 #

1. 快速方案：用ROS2内置工具 #

无需编写代码，直接调用pcl_ros工具保存点云：

# 保存指定话题的点云为带时间戳的.pcd文件
ros2 run pcl_ros pointcloud_to_pcd --ros-args -p input:=/你的激光雷达点云话题 -p prefix:=./saved_cloud_

若需转换为ply格式，用PCL命令行工具：

pcl_convert_pcd_to_ply 输入文件.pcd 输出文件.ply

2. 代码集成RTAB-Map/SLAM Toolbox实现保存 #

RTAB-Map导入与调用

• CMakeLists.txt添加依赖：

find_package(rtabmap_ros REQUIRED)
find_package(pcl_conversions REQUIRED)

add_executable(cloud_saver src/cloud_saver.cpp)
ament_target_dependencies(cloud_saver
  rclcpp
  sensor_msgs
  rtabmap_ros
  pcl_conversions
)
install(TARGETS cloud_saver DESTINATION lib/${PROJECT_NAME})

• package.xml添加依赖：

<depend>rtabmap_ros</depend>
<depend>pcl_conversions</depend>

• 核心代码示例：

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <rtabmap_ros/save_map.h>

class CloudSaver : public rclcpp::Node
{
public:
  CloudSaver() : Node("cloud_saver")
  {
    save_map_client_ = this->create_client<rtabmap_ros::srv::SaveMap>("/rtabmap/save_map");
    cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/你的激光雷达点云话题", 10, std::bind(&CloudSaver::cloud_cb, this, std::placeholders::_1));
  }

private:
  void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
  {
    // 可根据需求触发保存（比如定时、按键指令）
    auto req = std::make_shared<rtabmap_ros::srv::SaveMap::Request>();
    req->path = "./saved_map.pcd"; // 支持.pcd/.ply格式
    req->format = "pcd"; // 可选"ply"
    
    if (save_map_client_->wait_for_service(std::chrono::seconds(3)))
    {
      auto future = save_map_client_->async_send_request(req);
      // 可添加结果回调处理保存状态
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
  rclcpp::Client<rtabmap_ros::srv::SaveMap>::SharedPtr save_map_client_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<CloudSaver>());
  rclcpp::shutdown();
  return 0;
}

SLAM Toolbox集成保存

SLAM Toolbox本身不直接提供点云保存接口，可订阅其输出的地图点云话题（如/slam_toolbox/map_cloud），再用上述pcl_ros工具或自定义代码保存。导入依赖只需在CMakeLists.txt和package.xml中添加slam_toolbox即可。

二、RTAB-Map点云在base_link/map/odom间闪烁的修复 #

1. 检查TF树完整性 #

执行以下命令生成TF树，确认变换关系无冲突：

ros2 run tf2_tools view_frames

需确保：

• 仅存在一个base_link到odom的变换发布者（避免Gazebo与其他节点重复发布）
• odom到map的变换由RTAB-Map唯一发布

2. 修正RTAB-Map启动参数 #

在启动文件中强制使用外部里程计，禁用内部预测：

<node name="rtabmap" pkg="rtabmap_ros" exec="rtabmap" output="screen">
  <param name="frame_id" value="base_link"/>
  <param name="odom_frame_id" value="odom"/>
  <param name="map_frame_id" value="map"/>
  <param name="subscribe_odom" value="true"/>
  <param name="use_odom_prediction" value="false"/>
  <remap from="scan_cloud" to="/你的激光雷达点云话题"/>
</node>

3. 同步点云与里程计数据 #

添加消息同步节点，确保两者时间戳匹配：

<node name="cloud_odom_sync" pkg="message_filters" exec="sync" output="screen">
  <remap from="input0" to="/你的激光雷达点云话题"/>
  <remap from="input1" to="/odom"/>
  <remap from="output" to="/synced_cloud"/>
  <param name="queue_size" value="10"/>
  <param name="approximate_sync" value="true"/>
</node>

之后让RTAB-Map订阅同步后的/synced_cloud话题。

4. 确认激光雷达TF变换正确性 #

检查机器人xacro文件，确保激光雷达的frame_id设置正确，且与base_link的静态变换无抖动：

<joint name="lidar_joint" type="fixed">
  <parent link="base_link"/>
  <child link="lidar_link"/>
  <origin xyz="0 0 0.3" rpy="0 0 0"/> <!-- 固定位置无偏差 -->
</joint>

<gazebo reference="lidar_link">
  <sensor type="gpu_lidar" name="lidar">
    <ros>
      <remapping>~/out:=/你的激光雷达点云话题</remapping>
      <parameter name="frame_id">lidar_link</parameter>
    </ros>
  </sensor>
</gazebo>

内容的提问来源于stack exchange，提问作者Searcher