Writing your HC-SR04 driver (Python)

This tutorial will guide you through combining distance measurement and relative velocity measurement of the HC-SR04 sensor into a driver, which will be included in a ROS wrapper.

It is assumed that you have already read Distance measurement with ultrasonic sensor HC-SR04 (Python) and Speed measurement with ultrasonic sensor HC-SR04 (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. That means we can use it on every Raspberry Pi, also independent of ROS. Another important point of reusability is that I can connect multiple HC-SR04 sensors to one Raspberry Pi. To achieve this, we need to rewrite our previous code in an object-oriented way:

import sys
import RPi.GPIO as GPIO
import time

class UltrasonicHCSR04(object):

    def __init__(self, trigger, echo):
        self.trigger = trigger
        self.echo = echo

        self.timeout = 0.05

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)

        GPIO.setup(self.trigger, GPIO.OUT)
        GPIO.setup(self.echo, GPIO.IN)

    def distance(self):
        GPIO.output(self.trigger, True) # set trigger to HIGH

        # set trigger after 0.01 ms to LOW
        time.sleep(0.00001)
        GPIO.output(self.trigger, False)

        startTime = time.time()
        arrivalTime = time.time()

        timeout_start = time.time()

        # store startTime
        while GPIO.input(self.echo) == 0:
            startTime = time.time()

            if startTime - timeout_start > self.timeout:
                return -1

        # store arrivalTime
        while GPIO.input(self.echo) == 1:
            arrivalTime = time.time()

            if startTime - timeout_start > self.timeout:
                return -1

        if startTime != 0 and arrivalTime != 0:
            # calculate the difference between start and stop
            duration = arrivalTime - startTime

            # multiply with speed of sound (34300 cm/s)
            # and divide by 2 because there and back
            distance = (duration * 34300) / 2

            if distance >= 0:
                return distance
            else:
                return -1
        else:
            return -1

    def speed(self):
        start_time = time.time()

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

        end_time = time.time()

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

        return speed

As we can see, two things have changed:

  • I have added a limit consideration when determining the distance.
  • I have converted the speed from cm/s to m/s.

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 import UltrasonicHCSR04 # import our driver module

class UltrasonicHCSR04Wrapper(object):

    def __init__(self):
        self.min_range = 3
        self.max_range = 400
        self.fov = 0.26179938779915 # 15 degrees

        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 = UltrasonicHCSR04(18, 24)

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

Optimizations

What else do you need to optimize in this example? What do you need to adjust?

The sensor_msgs have information about the sensor, that is for example the Field of View. These should possibly be loaded via parameters. Also, depending on the sensor, the minimum and maximum range from which a sensor can measure can be adjusted to your sensor. Also here it is recommended to load parameters. And last but not least, you can of course select the pins for the trigger and for the echo differently. Here, too, it would be conceivable that they load parameters.

This can look like the following:

import rospy
from sensor_msgs.msg import Range
# import custom message type for Relative Velocity
from ultrasonic_hc_sr04 import UltrasonicHCSR04 # 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

        trigger_pin = rospy.get_param("~trigger_pin", 18)
        echo_pin = rospy.get_param("~echo_pin", 24)

        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 = UltrasonicHCSR04(trigger_pin, echo_pin)

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

In the next tutorial series, we'll look at how to implement the code using C++ instead of Python. Please read Distance measurement with ultrasonic sensor HC-SR04 (C++).

Wiki: Drivers/Tutorials/WritingYourHC-SR04DriverPython (last edited 2022-11-07 15:19:22 by Michdo93)