Kislate's Note

Appearance

cpp
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>

作用：

  • 引入 ROS 基础功能、点云消息类型、tf2 坐标变换相关的头文件。
cpp
ros::Publisher pub;

作用：

  • 定义一个全局发布器，用于后面发布转换后的点云。
cpp
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &input)
{
  static tf2_ros::Buffer tf_buffer;
  static tf2_ros::TransformListener tf_listener(tf_buffer);

作用：

  • 定义点云回调函数，每收到一次点云消息就会调用。
  • tf_buffer 和 tf_listener 用于查找和监听 tf 坐标变换（静态变量保证只初始化一次）。
cpp
  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    // 获取从 lidar_link 到 world 的变换
    transform_stamped = tf_buffer.lookupTransform("world", "camera_init", ros::Time(0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }

作用：

  • 尝试查找 camera_init 到 world 的坐标变换（即点云原始坐标系到目标坐标系）。
  • 如果找不到变换，则输出警告并返回，不做处理。
cpp
  // 创建一个用于存储转换后点云数据的对象
  sensor_msgs::PointCloud2 output;

  // 使用 tf2 将点云数据转换到 world 坐标系
  tf2::doTransform(*input, output, transform_stamped);

  // 设置输出点云的坐标系
  output.header.frame_id = "world";

  // 发布转换后的点云
  pub.publish(output);
}

作用：

  • 新建一个点云对象 output 用于存储转换结果。
  • 用 tf2::doTransform 把输入点云从 camera_init 坐标系变换到 world 坐标系。
  • 设置输出点云的 frame_id 为 world，确保下游节点识别。
  • 发布转换后的点云到新的话题。
cpp
int main(int argc, char **argv)
{
  ros::init(argc, argv, "pointcloud_transformer");
  ros::NodeHandle nh;

作用：

  • 初始化 ROS 节点，节点名为 pointcloud_transformer。
  • 创建节点句柄 nh，用于后续通信。
cpp
  // 订阅激光雷达的点云数据
  ros::Subscriber sub = nh.subscribe("/cloud_registered", 10, pointCloudCallback);

  // 创建发布器，用于发布转换后的点云数据
  pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_world", 10);

作用：

  • 订阅原始点云话题 /cloud_registered，每收到一帧点云就调用回调函数。
  • 创建发布器，发布转换后的点云到 /cloud_registered_world。
cpp
  ros::Rate r(100.0);

  while (ros::ok())
  {
    // 调用一次回调函数
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}

作用：

  • 设置循环频率为 100Hz。
  • 主循环中不断处理回调（ros::spinOnce()），保持节点运行。
  • 程序正常退出时返回 0。

总结 ​

  • 核心逻辑：订阅原始点云 → 查找 tf 变换 → 转换点云到 world → 发布新点云
  • 每一行代码都围绕这个流程展开，保证点云数据在世界坐标系下统一，方便后续建图和规划。

源码 ​

cpp
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>

ros::Publisher pub;

void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &input)
{
  static tf2_ros::Buffer tf_buffer;
  static tf2_ros::TransformListener tf_listener(tf_buffer);

  geometry_msgs::TransformStamped transform_stamped;
  try
  {
    // 获取从 lidar_link 到 world 的变换
    transform_stamped = tf_buffer.lookupTransform("world", "camera_init", ros::Time(0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }

  // 创建一个用于存储转换后点云数据的对象
  sensor_msgs::PointCloud2 output;

  // 使用 tf2 将点云数据转换到 world 坐标系
  tf2::doTransform(*input, output, transform_stamped);

  // 设置输出点云的坐标系
  output.header.frame_id = "world";

  // 发布转换后的点云
  pub.publish(output);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pointcloud_transformer");
  ros::NodeHandle nh;

  // 订阅激光雷达的点云数据
  ros::Subscriber sub = nh.subscribe("/cloud_registered", 10, pointCloudCallback);

  // 创建发布器，用于发布转换后的点云数据
  pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_world", 10);

  ros::Rate r(100.0);

  while (ros::ok())
  {
    // 调用一次回调函数
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}