1.下载sonar_layer的代码
https://github.com/DLu/navigation_layers
实际只需要其中的range_sensor_layer放到工作空间catkin_make。实验时放置于src中，可以考虑放置于navigation文件夹中进行尝试。
2.将plugin插入ros系统
参考：http://www.cnblogs.com/W-chocolate/p/4328725.html
实际上下载的源码配置已完善，编译通过时range_sensor_layer已插入ros系统，在此只需要source一下cartkin_ws，验证一下即可：rospack plugins --attrib=plugin costmap_2d。
3.sonar_layer在costmap_2d（navigation）中的使用
3.1.在costmap_common_params.yaml中配置sonar_layer的参数
sonar_layer:
ns ( string , default : ”” ) : 命名空间，用作所有topic的前缀；
topics(Array of strings , default : [‘/sonar’] ) : 列举可以订阅的距离topic；
no_readings_timeout ( double , default : 0.0 ) : 如果是0，该参数不起作用，否则如果层在该参数指定时间内没有收到传感器任何数据，层会给出warning并被标记为没有数据流；
clear_threshold( double, default : .2 ) : 概率比clear_threshold低的cell在master costmap中被标记为free空间；
mark_threshold ( double , default : .8 ) : 概率比mark_threshold高的cell在master costmap中被标记为lethal obstacles；
clear_on_max_reading ( bool , default : false ) : 是否将超出sonar最大距离清除。
3.2.在global_costmap_params.yaml中插入sonar_layer
plugins：
- { name : sonar_layer , type : “ range_sensor_layer::RangeSensorLayer”}
其实，也可以在local_costmap_params.yaml中插入sonar_layer，方法是一样的。根据具体需求选择插入的位置就行了。
4.超声波测距模块ROS驱动程序
单片机读取到超声波测距模块的值后，通过串口发送给ROS主机，ROS主机需要用ROS驱动程序来接收串口的数据，并将测距值发布到ROS的话题上。这里使用python写的ROS驱动程序，节点的主代码如下，关于ROS功能包的其他配置文件是通用的，篇幅限制就不贴出了。
#!/usr/bin/env python
import rospy
import serial
import binascii
from sensor_msgs.msg import Range
class Sonar():
def __init__(self):
rospy.init_node('sonar_publisher')
self.rate = rospy.get_param('~rate', 10.0)
self.device_port = rospy.get_param('~device_port',"/dev/ttyUSB0")
self.frame_id = rospy.get_param('~sonar_frame_id', '/sonar')
self.field_of_view = rospy.get_param('~field_of_view', 0.1)
self.min_range = rospy.get_param('~min_range', 0.02)
self.max_range = rospy.get_param('~max_range', 4.50)
self.sonar_pub = rospy.Publisher('/sonar', Range, queue_size=50)
self.ser = serial.Serial(self.device_port, 9600, timeout=1)
def handleSonar(self):
sonar = Range()
sonar.header.frame_id = self.frame_id
sonar.radiation_type = Range.ULTRASOUND
sonar.field_of_view = 0.1
sonar.min_range = self.min_range
sonar.max_range = self.max_range
self.ser.write(binascii.a2b_hex('55')) ##modify as your UART command
line = self.ser.readline()
try:
high = ord(line[0])
low = ord(line[1])
distance = (high*256. + low)/1000.
except:
distance = 11.
# print distance,'m'
# print ":".join("{:02x}".format(ord(c)) for c in line)
sonar.header.stamp = rospy.Time.now()
sonar.range = distance
self.sonar_pub.publish(sonar)
def spin(self):
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.handleSonar()
r.sleep()
if __name__ == '__main__':
scanner = Sonar()
rospy.loginfo("=== run")
scanner.spin()
rospy.loginfo("=== end")
对应的节点的launch启动文件也贴在下面吧。
<launch>
<node pkg="ultrasonic_sonar" type="sonar.py" name="sonar" output="screen">
<param name="rate" value="10.0"/>
<param name="device_port" value="/dev/ttyUSB1"/>
<param name="sonar_frame_id" value="/sonar"/>
</node>
</launch>