前言：基于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就可以看到点云随着超声数据变化：