Writing your VL53L1X driver (Python)

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

It is assumed that you have already read Distance measurement with ToF sensor VL53L1X (Python) and Speed measurement with ToF sensor VL53L1X (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

Finally, we build a wrapper around an existing driver code so that we can query the speed and distance with a single line later. Our driver looks like this:

import os
import sys
import time
import RPi.GPIO as GPIO
import VL53L1X

class ToFVL53L1X(object):

    def __init__(self, address, xshut):
        self.address = address

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

        self.xshut = xshut
        GPIO.setup(self.xshut, GPIO.IN)

        # 1 = Short Range, 2 = Medium Range, 3 = Long Range 
        self.range = 1

        if(self.address == 0x29):
            self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address = 0x29)
        else:
            self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address = self.address)
            self.tof.open()

    def start_sensor(self, pin):
        # The XSHUT pin is set HIGH to activate the sensor.
        GPIO.setup(pin, GPIO.OUT)
        GPIO.output(pin, True)
        time.sleep(0.2)

        self.tof.open()
        self.tof.start_ranging(self.range)

    def stop_sensor(self, pin):
        self.tof.stop_ranging()
        GPIO.output(pin, False)

    def set_range(self, range):
        if range == "short":
            self.range = 1
        elif range == "medium":
            self.range = 2
        else:
            self.range = 3

        self.tof.stop_ranging()
        self.tof.start_ranging(self.range)

    def get_range(self):
        if self.range == 1:
            currentRange = "short"
        elif self.range == 2:
            currentRange = "medium"
        else:
            currentRange = "long"

        return currentRange

    def get_distance(self):
        distance = 0.0
        distance = self.tof.get_distance() * 0.1 # mm to cm conversion

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

    def get_speed(self):
        start_distance = self.get_distance() * 0.01     # to m conversion

        time.sleep(1)

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

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

        return speed

We need the XSHUT pin so that a sensor can also be activated.

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

class ToFVL53L1XWrapper(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", 27) * 0.0174532925199433 # degree to radian conversion

        i2c_address = rospy.get_param("~i2c_address", 0x29)
        xshut = rospy.get_param("~xshut", 17)

        self.tofPub = rospy.Publisher('/tof/distance', Range, queue_size=10)
        self.tofVelocityPub = rospy.Publisher('/tof/relative_velocity', Range, queue_size=10)

        # loading the driver
        self.tof_sensor = ToFVL53L1X(i2c_address, xshut)

        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.tof_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.INFRARED
        r.field_of_view = self.fov
        r.min_range = self.min_range
        r.max_range = self.max_range

        r.range = distance

        self.tofPub.publish(r)

    def publish_current_velocity(self):
        relative_velocity = self.tof_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.INFRARED
        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.tofVelocityPub.publish(rv)

    def stop(self):
        self.tofPub.unregister()
        self.tofVelocityPub.unregister()

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

    tof_wrapper = ToFVL53L1XWrapper()

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

    rospy.spin()

Now we have a nice ROS driver that we can use for our ToF sensors. However, we haven't even talked about the fact that you can only assign an I2C address once. For this we look at Using multiple VL53L1X sensors at the same time (Python).