PCL学习之：将超声数据按照PCL点云方式发布出去

前言：基于2D激光雷达的机器人，想让它跑自动导航，众所周知有2个比较明显的缺陷，1，那就是普通的激光雷达无法对玻璃或是镜面物体有反映; 2，机器人避障时只能对某一个平面的物体有反映，超过或者低于这个平面就不行，类似桌面等悬空的物体就无法检测。

基于这个缺陷，大部分的做法是使用廉价的超声雷达来辅助激光雷达，达到改善这个缺陷的目地。而且超声波模块很便宜，大部分在5～15元之间。测距误差在<3cm，用于机器人效果上还是不错的。之前的做法是，直接读取超声然后判断障碍物是否在安全距离，在安全距离以内就急停，这种做法有一个缺陷，那就是无法让机器人对障碍物进行绕行，所有把超声的数据安装PCL点云的方式发布出去，然后塞到局部动态地图里面，这样就可以达到使得机器人可以向激光雷达一样，做绕行的动作。废话少说，直接上代码：

class TestPcl():
    def __init__(self):
        rospy.init_node('talk_sonar_pcl', anonymous=True)
        pub_cloud = rospy.Publisher("sonar_pcl", PointCloud2)
        self.sonar_pub = rospy.Publisher('sonar', Range, queue_size=5)
        rospy.Subscriber("stm_pub_ultrasonic1", Int16, self.sonarCallback)
        self.sonar_value = 0
        while not rospy.is_shutdown():
            now = rospy.Time.now()
            sonar_range = Range()
            sonar_range.header.stamp = now
            sonar_range.header.frame_id = "/sonar"
            sonar_range.radiation_type = Range.ULTRASOUND
            sonar_range.field_of_view = 0.3
            sonar_range.min_range = 0.04
            sonar_range.max_range = 0.8
            sonar_range.range = self.sonar_value/1000.0
            self.sonar_pub.publish(sonar_range)
            pcloud = PointCloud2()
            # make point cloud
            cloud = [[1,5,1],[1.1,5,1],[1.2,5,1]]
            cloud[0][1] = sonar_range.range
            cloud[1][1] = sonar_range.range
            cloud[2][1] = sonar_range.range
            pcloud.header.frame_id="/base_footprint"
            pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
            pub_cloud.publish(pcloud)
            rospy.loginfo(pcloud)
            rospy.sleep(1.0)

def sonarCallback(self,seq):
        self.sonar_value = seq.data

代码很简单，我就不解释了。
运行rviz，添加pointcloud2，topic主题为sonar_pcl就可以看到点云随着超声数据变化：