Using ultrasonic (sonar) navigation and obstacle avoidance in ROS

1. Download the code of sonar_layer

https://github.com/DLu/navigation_layers

In fact, only the range_sensor_layer needs to be placed in the workspace catkin_make. It is placed in src during the experiment, and it can be considered to be placed in the navigation folder for trying.

2. Insert the plugin into the ros system

Reference: http://www.cnblogs.com/W-chocolate/p/4328725.html

In fact, the configuration of the downloaded source code has been completed. When the compilation is passed, the range_sensor_layer has been inserted into the ros system. Here, you only need to source cartkin_ws and verify it: rospack plugins --attrib=plugin costmap_2d.

3. The use of sonar_layer in costmap_2d (navigation)

3.1. Configure sonar_layer parameters in costmap_common_params.yaml

sonar_layer:

ns ( string , default : "" ) : namespace, used as a prefix for all topics;

topics(Array of strings , default : ['/sonar'] ) : List the distance topics that can be subscribed;

no_readings_timeout ( double , default : 0.0 ) : If it is 0, this parameter has no effect, otherwise if the layer does not receive any data from the sensor within the time specified by this parameter, the layer will give a warning and be marked as no data flow;

clear_threshold( double, default : .2 ) : Cells with a probability lower than clear_threshold are marked as free space in the master costmap;

mark_threshold ( double , default : .8 ) : Cells with a higher probability than mark_threshold are marked as lethal obstacles in the master costmap;

clear_on_max_reading ( bool , default : false ) : Whether to clear the sonar maximum distance.

3.2. Insert sonar_layer in global_costmap_params.yaml

plugins：

- { name : sonar_layer , type : “ range_sensor_layer::RangeSensorLayer”}

In fact, you can also insert sonar_layer in local_costmap_params.yaml, the method is the same. Just choose the insertion location according to your specific needs.

4. Ultrasonic ranging module ROS driver

After the microcontroller reads the value of the ultrasonic ranging module, it sends it to the ROS host through the serial port. The ROS host needs to use the ROS driver to receive the data from the serial port and publish the ranging value to the ROS topic. The ROS driver written in python is used here. The main code of the node is as follows. Other configuration files about the ROS function package are common, and the space limit will not be posted.

#!/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")

The launch startup file of the corresponding node is also posted below.

<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>

references

[1] Zhang Hu, Robot SLAM Navigation Core Technology and Practice [M]. Machinery Industry Press, 2022.