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

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

点云数据发送

  关于发送节点，只需要声明头文件：  

<code class="prism language-c has-numbering"><span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><sensor_msgs/PointCloud.h>
</span></span>
</code>

  定义消息发布者：  

<code class="prism language-c has-numbering">ros<span class="token punctuation">:</span><span class="token punctuation">:</span>Publisher cloud_pub <span class="token operator">=</span> n<span class="token punctuation">.</span>advertise<span class="token operator"><</span>sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span>PointCloud<span class="token operator">></span><span class="token punctuation">(</span><span class="token string">"cloud"</span><span class="token punctuation">,</span> <span class="token number">50</span><span class="token punctuation">)</span><span class="token punctuation">;
</span>
</code>

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

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

  有三点需要注意：  

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

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

<code class="prism language-c has-numbering">cloud<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">.</span>name <span class="token operator">=</span> <span class="token string">"rgb"</span><span class="token punctuation">;
</span>
</code>

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

<code class="prism language-c has-numbering">uint32_t rgb <span class="token operator">=</span> <span class="token punctuation">(</span>static_cast<span class="token operator"><</span>uint32_t<span class="token operator">></span> <span class="token punctuation">(</span><span class="token number">255</span><span class="token punctuation">)</span> <span class="token operator"><<</span> <span class="token number">16</span> <span class="token operator">|</span> static_cast<span class="token operator"><</span>uint32_t<span class="token operator">></span> <span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">)</span> <span class="token operator"><<</span> <span class="token number">8</span> <span class="token operator">|</span> static_cast<span class="token operator"><</span>uint32_t<span class="token operator">></span> <span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//rgb</span>
<span class="token keyword">float</span> rgb_float <span class="token operator">=</span><span class="token operator">*</span>reinterpret_cast<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">*</span><span class="token operator">></span> <span class="token punctuation">(</span><span class="token operator">&</span>rgb<span class="token punctuation">)</span><span class="token punctuation">;</span>
cloud<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> rgb_float <span class="token punctuation">;</span> <span class="token comment">//设置类型
</span>
</code>

点云数据接收

对于sensor_msgs/PointCloud

  同样需要包含头文件：  

<code class="prism language-c has-numbering"><span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><sensor_msgs/PointCloud.h>
</span></span>
</code>

  订阅消息节点：  

<code class="prism language-c has-numbering">ros<span class="token punctuation">:</span><span class="token punctuation">:</span>Subscriber sub <span class="token operator">=</span> n<span class="token punctuation">.</span>subscribe <span class="token punctuation">(</span><span class="token string">"cloud"</span><span class="token punctuation">,</span> <span class="token number">1</span><span class="token punctuation">,</span> getcloud<span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//订阅cloud节点
</span>
</code>

  定义回调函数：  

<code class="prism language-c has-numbering"><span class="token comment">//回调函数</span>
<span class="token keyword">void</span> getcloud <span class="token punctuation">(</span><span class="token keyword">const</span> sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span>PointCloudConstPtr<span class="token operator">&</span> clouddata<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
	<span class="token comment">//其中i为索引值，如下直接通过索引取得接收到的数据对象</span>
	clouddata<span class="token operator">-></span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">;</span>
	clouddata<span class="token operator">-></span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">;</span>
	clouddata<span class="token operator">-></span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z<span class="token punctuation">;</span>
	clouddata<span class="token operator">-></span>channels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">;</span>
	clouddata<span class="token operator">-></span>channels<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">[</span>i<span class="token punctuation">]</span>
<span class="token punctuation">}
</span>
</code>

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

对于sensor_msgs/PointCloud2

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

<code class="prism language-c has-numbering"><span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><sensor_msgs/PointCloud2.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><sensor_msgs/point_cloud_conversion.h>
</span></span>
</code>

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

<code class="prism language-c has-numbering"><span class="token keyword">void</span> <span class="token function">getcloud</span><span class="token punctuation">(</span><span class="token keyword">const</span> sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span>PointCloud2 <span class="token operator">&</span>msg<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
    sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span>PointCloud clouddata<span class="token punctuation">;</span>
	sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span><span class="token function">convertPointCloud2ToPointCloud</span><span class="token punctuation">(</span>msg<span class="token punctuation">,</span> clouddata<span class="token punctuation">)</span><span class="token punctuation">;</span>
	sensor_msgs<span class="token punctuation">:</span><span class="token punctuation">:</span>PointCloud cloud<span class="token punctuation">;</span>
    cloud<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> ros<span class="token punctuation">:</span><span class="token punctuation">:</span>Time<span class="token punctuation">:</span><span class="token punctuation">:</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    cloud<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"PERCEPTION2020"</span><span class="token punctuation">;</span> <span class="token comment">//帧id</span>
    cloud<span class="token punctuation">.</span>points<span class="token punctuation">.</span><span class="token function">resize</span><span class="token punctuation">(</span>clouddata<span class="token punctuation">.</span>points<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    cloud<span class="token punctuation">.</span>channels<span class="token punctuation">.</span><span class="token function">resize</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">;</span>             <span class="token comment">//设置增加通道数</span>
    cloud<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>name <span class="token operator">=</span> <span class="token string">"intensity"</span><span class="token punctuation">;</span> <span class="token comment">//增加反射强度信道，并设置其大小，使与点云数量相匹配</span>
    cloud<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">resize</span><span class="token punctuation">(</span>clouddata<span class="token punctuation">.</span>points<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> clouddata<span class="token punctuation">.</span>points<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span>
    <span class="token punctuation">{</span>
        cloud<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x <span class="token operator">=</span> clouddata<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">;</span>
        cloud<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y <span class="token operator">=</span> clouddata<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">;</span>
        cloud<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z <span class="token operator">=</span> clouddata<span class="token punctuation">.</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z<span class="token punctuation">;</span>
        cloud<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> clouddata<span class="token punctuation">.</span>channels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>values<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">;</span>    <span class="token comment">//设置反射强度</span>
    <span class="token punctuation">}</span>
    pub<span class="token operator">-></span><span class="token function">publish</span><span class="token punctuation">(</span>cloud<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}
</span>
</code>

  另外，sensor_msgs/PointCloud2格式可以直接获得pcd的点云帧数据。   查看bag包信息：  

<code class="prism language-bash has-numbering">rosbag info xxx.bag
</code>

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

<code class="prism language-bash has-numbering">rosrun pcl_ros bag_to_pcd xxx.bag /rfans_driver/rfans_points pcd
</code>

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