ROS系统学习11—点云数据的发送和接收

264
0
2021年2月2日 09时04分

在ROS节点中解析并发送点云数据是非常基础的需求,下面我们将做简单的介绍。

 

点云数据发送

 

关于发送节点,只需要声明头文件:

 

#include <sensor_msgs/PointCloud.h>

 

定义消息发布者:

 

ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

 

然后在循环中将消息发布出去即可:

 

//将数据通过消息发送出去
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";//帧id
cloud.points.resize(LINE*CIRCLEPT);
cloud.channels.resize(2);//设置增加通道数
cloud.channels[0].name = "intensity";//增加反射强度信道,并设置其大小,使与点云数量相匹配
cloud.channels[0].values.resize(LINE*CIRCLEPT);
cloud.channels[1].name = "distance";//增加反射强度信道,并设置其大小,使与点云数量相匹配
cloud.channels[1].values.resize(LINE*CIRCLEPT);
int i=0;
for (int l = 0; l < LINE; l++)
	for (int c = 0; c < CIRCLEPT; c++)
	{
		cloud.points[i].x = mdecoder.mpointcloud[l][c].x;
		cloud.points[i].y = mdecoder.mpointcloud[l][c].y;
		cloud.points[i].z = mdecoder.mpointcloud[l][c].z;
		cloud.channels[0].values[i] = mdecoder.mpointcloud[l][c].r;//设置反射强度
		cloud.channels[1].values[i] = mdecoder.mpointcloud[l][c].d;
		i++;
	}
cloud_pub.publish(cloud);

 

有三点需要注意:

 

  1. frame_id与rviz的Fixed Frame相对应,设置成什么在rviz中就应该做出相应设置。
  2. 结构体中只有x,y,z三个值,如果需要发布的信息中还有反射强度和距离等值,需要利用channels来进行。写法参考上面代码,按照上面写法,可以在rviz上看到反射强度的区别。
  3. 如果需要发送有序点云,则在点云结构体赋值的时候按顺序进行即可,无需其他操作。

 

PS: 如果想要发布指定颜色的点云,可以将附加通道命名为“rgb”:

 

cloud.channels[2].name = "rgb";

 

然后通过如下的操作设置rgb的值即可:

 

uint32_t rgb = (static_cast<uint32_t> (255) << 16 | static_cast<uint32_t> (0) << 8 | static_cast<uint32_t> (0));//rgb
float rgb_float =*reinterpret_cast<float*> (&rgb);
cloud.channels[2].values[i] = rgb_float ; //设置类型

点云数据接收

 

对于sensor_msgs/PointCloud

 

同样需要包含头文件:

 

#include <sensor_msgs/PointCloud.h>

 

订阅消息节点:

 

ros::Subscriber sub = n.subscribe ("cloud", 1, getcloud);//订阅cloud节点

 

定义回调函数:

 

//回调函数
void getcloud (const sensor_msgs::PointCloudConstPtr& clouddata)
{
	//其中i为索引值,如下直接通过索引取得接收到的数据对象
	clouddata->points[i].x;
	clouddata->points[i].y;
	clouddata->points[i].z;
	clouddata->channels[0].values[i];
	clouddata->channels[1].values[i]
}

 

如果发送过来的点云是按顺序赋值的,则取的时候也按照同样的顺序获取即可获得有序点云。

 

对于sensor_msgs/PointCloud2

 

有部分雷达驱动给的节点是”sensor_msgs/PointCloud2″格式的,读取的时候则需要先做转换。

 

包含头文件

 

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

 

之后将PointCloud2先转为PointCloud再以该形式进行处理即可。代码如下

 

void getcloud(const sensor_msgs::PointCloud2 &msg)
{
    sensor_msgs::PointCloud clouddata;
	sensor_msgs::convertPointCloud2ToPointCloud(msg, clouddata);
	sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "PERCEPTION2020"; //帧id
    cloud.points.resize(clouddata.points.size());
    cloud.channels.resize(1);             //设置增加通道数
    cloud.channels[0].name = "intensity"; //增加反射强度信道,并设置其大小,使与点云数量相匹配
    cloud.channels[0].values.resize(clouddata.points.size());

    for (int i = 0; i < clouddata.points.size(); i++)
    {
        cloud.points[i].x = clouddata.points[i].x;
        cloud.points[i].y = clouddata.points[i].y;
        cloud.points[i].z = clouddata.points[i].z;
        cloud.channels[0].values[i] = clouddata.channels[0].values[i];    //设置反射强度
    }
    pub->publish(cloud);
}

 

另外,sensor_msgs/PointCloud2格式可以直接获得pcd的点云帧数据。

 

查看bag包信息:

 

rosbag info xxx.bag

 

将bag包中的点云转为pcd格式

 

rosrun pcl_ros bag_to_pcd xxx.bag /rfans_driver/rfans_points pcd

 

最后一个参数为输出文件的文件夹名称,没有则会被创建。倒数第二个参数为话题名称。
不过用这个命令转出来的PCD文件是二进制形式的,而且Windows的PCLViewer无法读取,原因暂时不明。

发表评论

后才能评论