Writing your Parallax PING))) driver (Python)

This tutorial will guide you through combining distance measurement and relative velocity measurement of the Parallax PING))) 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 Parallax PING))) (Python) and Speed measurement with ultrasonic sensor Parallax PING))) (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 Parallax PING))) 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 Parallax PING))) 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 UltrasonicParallaxPING(object):

    def __init__(self, signal):
        self.signal = signal

        self.timeout = 0.05

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

    def distance(self):
        arrivalTime = 0
        startTime = 0

        GPIO.setup(self.signal, GPIO.OUT) # Set to low
        GPIO.output(self.signal, False)

        time.sleep(0.01)

        GPIO.output(self.signal, True) # Set high

        time.sleep(0.00001)

        GPIO.output(self.signal, False) # Set low
        GPIO.setup(self.signal, GPIO.IN) # Set to input

        timeout_start = time.time()

        # Count microseconds that SIG was high
        while GPIO.input(self.signal) == 0:
            startTime = time.time()

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

        while GPIO.input(self.signal) == 1:
            arrivalTime = time.time()
        
            if startTime - timeout_start > self.timeout:
                return -1

        if startTime != 0 and arrivalTime != 0:
            pulse_duration = arrivalTime - startTime
            # The speed of sound is 340 m/s or 29 microseconds per centimeter.
            # The ping travels out and back, so to find the distance of the
            # object we take half of the distance travelled.
            # distance = duration / 29 / 2
            #distance = pulse_duration * 100 * 343.0 / 2
            distance = (pulse_duration * 34300) / 2

            #print('start = %s'%startTime,)
            #print('end = %s'%arrivalTime)
            if distance >= 0:
                return distance
            else:
                return -1
        else :
            #print('start = %s'%startTime,)
            #print('end = %s'%arrivalTime)
            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_parallax_ping import UltrasonicParallaxPING # import our driver module

class UltrasonicParallaxPINGWrapper(object):

    def __init__(self):
        self.min_range = 3
        self.max_range = 300
        self.fov = 0.34906585039887 # 20 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 = UltrasonicParallaxPING(17)

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

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

class UltrasonicParallaxPINGWrapper(object):

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

        signal_pin = rospy.get_param("~signal_pin", 17)

        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 = UltrasonicPARALLAXPING(signal_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 = UltrasonicParallaxPINGWrapper()

    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 Parallax PING))) (C++).