ROS 发送带颜色的点云

最新推荐文章于 2024-08-01 14:45:17 发布

TiRan_Yang

最新推荐文章于 2024-08-01 14:45:17 发布

阅读量2k

点赞数 2

分类专栏： C++ ROS 文章标签： ROS

本文链接：https://blog.csdn.net/lovelyaiq/article/details/118530388

版权

C++ 同时被 2 个专栏收录

12 篇文章 0 订阅

订阅专栏

ROS

4 篇文章 1 订阅

订阅专栏

ROS 发送带颜色的点云

方式一：PointXYZRGB

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

//定义需要的变量
ros::Publisher pubCloud;
sensor_msgs::PointCloud2 colored_msg;  //等待发送的点云消息
pubCloud = n.advertise<sensor_msgs::PointCloud2>("/colored_cloud", 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr  colored_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);

//取出需要发送的点云
for (int i = 0; i <  raw_pcl_ptr->points.size(); i++)
 {
      pcl::PointXYZRGB  p;
      p.x=raw_pcl_ptr->points[i].x;
      p.y=raw_pcl_ptr->points[i].y;
      p.z=raw_pcl_ptr->points[i].z;
      //点云的颜色，根据需要将点云设置为不同的颜色
      p.r =  255;
      p.g =  0;
      p.b =  0; 
      colored_pcl_ptr->points.push_back(p);
      }
    colored_pcl_ptr->width = 1;
    colored_pcl_ptr->height = raw_pcl_ptr->points.size();
    pcl::toROSMsg( *colored_pcl_ptr,  colored_msg);  //将点云转化为消息才能发布
    colored_msg.header.frame_id = "ColorPoint";
    pubCloud.publish( colored_msg); //发布调整之后的点云数据，主题为/adjustd_cloud

方式2：sensor_msgs::PointCloud

ros::Publisher m_allLidarPointsPub;

m_allLidarPointsPub = nh.advertise<sensor_msgs::PointCloud>(GlobalData.m_topicNamesMap["allLidarPoints"], 2);

size_t lidarSize = lidarPoints.size ();

sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = m_rosFrameId;
cloud.points.resize(lidarSize);

//we'll also add an intensity channel to the cloud
//两个channel，可以显示两个不同信息
cloud.channels.resize(2);
cloud.channels[0].name = "intensities";
cloud.channels[0].values.resize(lidarSize);
cloud.channels[1].name = "labels";
cloud.channels[1].values.resize(lidarSize);
int cnt = 0;
for (size_t i = 0; i < lidarSize; ++i)
{
    if (PLabel::isValidToUse(lidarPoints[i]))
    {
        cloud.points[cnt].x = lidarPoints[i].x;
        cloud.points[cnt].y = lidarPoints[i].y;
        cloud.points[cnt].z = lidarPoints[i].z;
        cloud.channels[0].values[cnt] = lidarPoints[i].intensity;
        //颜色通道
        cloud.channels[1].values[cnt] = 0;
        cnt++;
    }
    else if (PLabel::isVehicleSelf(lidarPoints[i]))
    {
        cloud.points[cnt].x = lidarPoints[i].x;
        cloud.points[cnt].y = lidarPoints[i].y;
        cloud.points[cnt].z = lidarPoints[i].z;
        cloud.channels[0].values[cnt] = lidarPoints[i].intensity;
        cloud.channels[1].values[cnt] = 1;
        cnt++;
    }

}
m_allLidarPointsPub.publish(cloud);