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 speedAs 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++).