Writing your HC-SR04 serial driver (Python)

This tutorial will guide you through writing a serial driver using the HC-SR04 on the Arduino Nano, which will be included in a ROS wrapper.

It is assumed that you have already read Distance measurement with ultrasonic sensor HC-SR04 using the Arduino with a serial connection (Python) and Speed measurement with ultrasonic sensor HC-SR04 using the Arduino with a serial connection (Python).

Besides the basics in ROS from the Tutorials, it is also already assumed that you have read the chapters What is a ROS Wrapper?, Write a ROS Wrapper (Python) and Package and Test Your Driver.

Programming our Driver code

Why do we need a driver? Of course, we can simplistically say that we need a driver so that we can operate the sensor. That is also correct. We could simplified yes access the sensor also within our ROS code and save and the concept of a driver. One advantage of programming our driver is that we create a reusability through it. This driver is a HC-SR04 Python driver for the Raspberry Pi which will serial Read the sensor informations from an Arduino. That means we can use it on every Raspberry Pi with every serial connected Arduino, also independent of ROS. Another important point of reusability is that I can connect multiple HC-SR04 sensors to one Raspberry Pi, but using several Arduinos, because I can only transfer data serially once per Arduino at the same time. To achieve this, we need to rewrite our previous code in an object-oriented way:

import serial_float
import struct
import time

class UltrasonicHCSR04Serial(object):

    def __init__(self, serial_port: str, baudrate: int):
        self.serial = serial_float.Serial(serial_port, baudrate)

    def distance(self):
        bites = self.serial.read(4)
        distance = struct.unpack("f", bites)[0]

        return distance

    def speed(self):
        start_distance = self.distance() * 0.01     # to m conversion

        time.sleep(1)

        end_distance = self.distance() * 0.01       # to m conversion

        speed = (end_distance - start_distance) / 1.0   # m/s

        return speed

Programming our Wrapper code

The next step is to build our ROS wrapper for this driver. This looks like the following for example:

import rospy
from sensor_msgs.msg import Range
# import custom message type for Relative Velocity
from ultrasonic_hc_sr04_serial import UltrasonicHCSR04Serial # import our driver module

class UltrasonicHCSR04Wrapper(object):

    def __init__(self):
        self.min_range = rospy.get_param("~minimum_range", 3)
        self.max_range = rospy.get_param("~maximum_range", 400)
        self.fov = rospy.get_param("~field_of_view", 15) * 0.0174532925199433 # degree to radian conversion

        serial_port = rospy.get_param("~serial_port", "/dev/ttyUSB0")
        baudrate = rospy.get_param("~baudrate", 115200)

        self.ultrasonicPub = rospy.Publisher('/ultrasonic/distance', Range, queue_size=10)
        self.ultrasonicVelocityPub = rospy.Publisher('/ultrasonic/relative_velocity', Range, queue_size=10)

        # loading the driver
        self.ultrasonic_sensor = UltrasonicHCSR04Serial(serial_port, baudrate)

        self.rate = rospy.Rate(10) # 10hz
        rospy.Timer(self.rate, self.publish_current_distance)
        rospy.Timer(self.rate, self.publish_current_velocity)

    def publish_current_distance(self):
        distance = self.ultrasonic_sensor.distance()

        message_str = "Distance: %s cm" % distance
        rospy.loginfo(message_str)

        #for distance in ranges:
        r = Range()

        r.header.stamp = rospy.Time.now()
        r.header.frame_id = "/base_link"
        r.radiation_type = Range.ULTRASOUND
        r.field_of_view = self.fov # 15 degrees
        r.min_range = self.min_range
        r.max_range = self.max_range

        r.range = distance

        self.ultrasonicPub.publish(r)

    def publish_current_velocity(self):
        relative_velocity = self.ultrasonic_sensor.speed()

        message_str = " Speed: %s m/s" % relative_velocity
        rospy.loginfo(message_str)

        rv = Range() # using range instead of a custom message type

        rv.header.stamp = rospy.Time.now()
        rv.header.frame_id = "/base_link"
        rv.radiation_type = Range.ULTRASOUND
        rv.field_of_view = self.fov
        rv.min_range = self.min_range
        rv.max_range = self.max_range

        rv.rage = relative_velocity # using range instead of a custom message type with a field relative velocity

        self.ultrasonicVelocityPub.publish(rv)

    def stop(self):
        self.ultrasonicPub.unregister()
        self.ultrasonicVelocityPub.unregister()

# Main function.
if __name__ == '__main__':
    # Initialize the node and name it.
    rospy.init_node("ultrasonic_driver", anonymous=False)

    ultrasonic_wrapper = UltrasonicHCSR04Wrapper()

    rospy.on_shutdown(ultrasonic_wrapper.stop)
    rospy.loginfo("Ultrasonic driver is now started.")

    rospy.spin()