发布sensor_msgs::PointCloud2点云数据

       发布sensor_msgs::PointCloud2点云数据

方法一：

//设置消息头文件和初始化节点
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/Point.h>

//定义点的向量和消息发布器
std::vector<geometry_msgs::Point> circle;
ros::Publisher pub;


//将点的向量转换为PointCloud2消息
void convertAndPublish(const std::vector<geometry_msgs::Point>& points, ros::Publisher& publisher) {
    sensor_msgs::PointCloud2 cloud_msg;
    cloud_msg.header.stamp = ros::Time::now();
    cloud_msg.header.frame_id = "map"; // 根据需要设置frame_id

    // 设置点云消息的字段
    cloud_msg.height = 1;
    cloud_msg.width = points.size();
    cloud_msg.is_dense = false;

    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(points.size());

    // 使用迭代器填充点云消息的数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");

    for (const auto& point : points) {
        *iter_x = point.x;
        *iter_y = point.y;
        *iter_z = point.z;

        ++iter_x;
        ++iter_y;
        ++iter_z;
    }

    publisher.publish(cloud_msg);
}
//在主函数中初始化ROS节点、定义发布器并在循环中发布点云消息。
int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_publisher");
    ros::NodeHandle nh;

    // 初始化点的向量
    std::vector<geometry_msgs::Point> circle;
    // 添加一些点到向量中
    for (float angle = 0; angle < 2 * M_PI; angle += 0.1) {
        geometry_msgs::Point pt;
        pt.x = cos(angle);
        pt.y = sin(angle);
        pt.z = 0;
        circle.push_back(pt);
    }

    pub = nh.advertise<sensor_msgs::PointCloud2>("circle_points", 1);

    ros::Rate loop_rate(10); // 10 Hz

    while (ros::ok()) {
        convertAndPublish(circle, pub);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

这个代码片段展示了如何将一个std::vector<geometry_msgs::Point>转换为一个sensor_msgs::PointCloud2消息，并通过ROS发布它。你可以根据需要修改点的生成逻辑和消息的发布频率。

方法二.

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

typedef pcl::PointXYZ PointType; // 使用PCL的点类型

void publishPointCloud(const std::vector<PointType>& points, ros::Publisher& pointcloud_pub) {
    pcl::PointCloud<PointType> cloud;
    cloud.points = points; // 直接赋值
    cloud.width = points.size();
    cloud.height = 1;
    cloud.is_dense = true;

    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(cloud, cloud_msg); // 将PCL点云转换为ROS消息
    cloud_msg.header.frame_id = "your_frame_id"; // 替换为你的坐标系ID
    cloud_msg.header.stamp = ros::Time::now();

    pointcloud_pub.publish(cloud_msg);
}

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

    std::vector<PointType> circle;
    // 填充你的circle向量，例如：
    // circle.push_back(PointType(1.0f, 2.0f, 3.0f));

    ros::Publisher pointcloud_pub = nh.advertise<sensor_msgs::PointCloud2>("circle_cloud", 10);
    ros::Rate loop_rate(10); // 发布频率为10Hz

    while (ros::ok()) {
        publishPointCloud(circle, pointcloud_pub);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

在这个版本中，首先定义了PointType为pcl::PointXYZ，这是一种广泛使用的点类型，包含三维坐标。然后，在publishPointCloud函数中，直接将std::vector<PointType>赋值给PCL的点云对象，并使用pcl::toROSMsg函数将其转换为ROS的消息类型，大大简化了代码。注意，需要根据实际情况填充circle向量，并调整坐标系ID。