在ROS中使用超声波（sonar）导航避障

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>

参考文献

[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.