Writing your HC-SR04 I2C driver (Python)

This tutorial will guide you through writing a I2C 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 I2C connection (Python) and Speed measurement with ultrasonic sensor HC-SR04 using the Arduino with a I2C 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 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 (I2C) read the sensor informations from an Arduino. That means we can use it on every Raspberry Pi with every serial (I2C) 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 struct
import smbus2
import time

class UltrasonicHCSR04I2C(object):

    def __init__(self, bus: int, i2c_address):
        self.bus = smbus2.SMBus(bus)
        self.address = i2c_address

    def distance(self):
        b = []
        for byt in range(4):
            b.append(self.bus.read_byte(self.address))

        distance = struct.unpack("f", bytearray(b))[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_i2c import UltrasonicHCSR04I2C # 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

        bus = rospy.get_param("~bus", 1)
        i2c_address = rospy.get_param("~i2c_address", 0x04)

        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 = UltrasonicHCSR04I2C(bus, i2c_address)

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