ROS node for VL53L0X range sensor

Published in

Robotics Weekends

5 min readJun 19, 2020

This time I’m going to create a ROS node which will publish sensor_msgs/Range message and use ToF distance sensor VL53L0X.

VL53L0X communicates by I2C bus, but to make it more universal I will use Arduino board to convert I2C signals to USB Serial.

Hardware part of the sensor is based on Arduino Pro Micro. Also I decided to make a 3D printed case for it. Due to a small flash and RAM amount (2.5KB) compiled sketch, which includes ROS library and VL53L0X driver, is bigger than allowed. To reduce size of the sketch, I moved all the ROS-related code to a separate ROS client node, which will run on the host computer.

We will need:

Computer with Linux installed. In my case it’s Raspberry Pi 4 with Ubuntu Linux 18.04;
ROS installed;
Arduino IDE installed;
Arduino Pro Micro. Actually any other model will be fine, but this one is small and cheap;
VL53L0X sensor.

Wiring the components

The wiring is simple — connect the sensor to the Arduino’s I2C bus. That’s it.

VL53L0X and Arduino Pro Micro wiring diagram

Assemby

I’ve printed a case with mounting holes. This will make sensor more convenient to use. STL files are on Thingiverse.

Example of 3D printed case and components placement schematics

VL53L0X Arduino library

Next we need a library to communicate with sensor on the Arduino side. There are two popular libraries: Adafruit_VL53L0X by Adafruit and VL53L0X by Pololu. Both libraries have pretty similar API, use Bosch sensor driver code and can be installed by Arduino IDE Library Manager. Library by Pololu requires less flash space and RAM, but still not so small to use it with the ROS library on Arduino Pro Micro. I chose Adafruit_VL53L0X library.

Adafruit_VL53L0X library in Arduino IDE Library Manager

Arduino sketch

Sketch logic is simple — read measurement data from VL53L0X sensor, convert to a transmission data format, send a message to serial port. Serial message format is also simple and lightweight. Every message, sent from Arduino, starts with 0xAD byte, followed by message type byte, and ends with \r\n (0x0D, 0x0A) sequence. There are 4 message types:

0xEF — initialization failed;
0xE0 — initialization was successful, no errors;
0x1D — range message, followed by two bytes of measured range;
0xEA — out of range.

Let’s see full Sketch code:

#include "Adafruit_VL53L0X.h"Adafruit_VL53L0X sensor = Adafruit_VL53L0X();
VL53L0X_RangingMeasurementData_t measure;
unsigned long range_timer;

void setup() {
    Serial.begin(57600);
    // wait until Serial port is opened
    while (!Serial) {}
    // if initialization failed - write message and freeze
    if (!sensor.begin()) {
        Serial.write(0xAD);
        Serial.write(0xEF);
        Serial.write(0x0D);
        Serial.write(0x0A);
        while (1);
    }
    // initialization successful
    Serial.write(0xAD);
    Serial.write(0xE0);
    Serial.write(0x0D);
    Serial.write(0x0A);
}

void loop() {
    if ((millis() - range_timer) > 50) {
        // message start
        Serial.write(0xAD);
        sensor.rangingTest(&measure, false);
        if (measure.RangeStatus == 4) {
            // out of range
            Serial.write(0xEA);
        } else {
            // measured distance
            Serial.write(0x1D);
            Serial.write(measure.RangeMilliMeter & 0xff);
            Serial.write(measure.RangeMilliMeter >> 8);
        }
        // message end
        Serial.write(0x0D);
        Serial.write(0x0A);
        range_timer = millis();
    }
}

Standard ROS Range message

sensor_msgs/Range message contains a set of fields to provide information about sensor and distance to detected obstacle:

Header header           # header contains timestamp and frame_id# Radiation type enums
uint8 ULTRASOUND=0
uint8 INFRARED=1uint8 radiation_type    # radiation type used by the sensor [enum]float32 field_of_view   # the size of the arc that the distance 
                        # reading is valid for [rad]float32 min_range       # minimum range value [m]
float32 max_range       # maximum range value [m]float32 range           # range data [m]

Some points about message header format. Header contains two important fields — time and frame_id. time field contains timestamp when the measurement was done. frame_id is a string which represents name of the frame this data associated with. Frame is used when you create a tf-tree — a spatial relations between robot’s components.

ROS client node

ROS client node connects to the sensor by serial port, reads data messages and converts them to the ROS Range message. To build correct Range message we need to specify sensor’s specs: emitter type, field of view, min and max range. Measured distance is stored in a range field. Angles are specified in radians and range data — in meters. Also we need to fill a message header — timestamp, for having time reference, and frame_id to make easier integration of our sensor into robot’s tf-tree. Also it makes sense to make frame_id configurable through the node’s attributes. Here is a piece of code, responsible for the Range message creation:

...
sensor_msgs::Range::Ptr range_scan(new sensor_msgs::Range);
range_scan->header.frame_id = frame_id;
range_scan->header.stamp = ros::Time::now();
...
// Now read range
boost::asio::read(serial_, boost::asio::buffer(&ir_range_, 2));
range_scan->radiation_type = sensor_msgs::Range::INFRARED;
range_scan->field_of_view = 0.44;  //25 degrees
range_scan->min_range = 0.03;
range_scan->max_range = 1.2;
range_scan->range = ir_range_ / 1000.0f;  // convert mm to m
...

Full source code of the Sketch and vl53l0x ROS node is on GitHub page:

https://github.com/Andrew-rw/vl53l0x_driver

For testing I’ve included a test.launch file, which runs vl53l0x_node and rviz, with prepared view:

<launch>
  <node pkg="vl53l0x_driver" type="vl53l0x_publisher" name="vl53l0x_node" output="screen">
     <param name="port" value="/dev/ttyACM0"/>
     <param name="frame_id" value="ir_range"/>
  </node>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find vl53l0x_driver)/rviz/demo.rviz" required="true" />
</launch>

It could happen that your port name differs from mine (for example /dev/ttyUSB0 or /dev/ttyACM1). In this case you have to change port param to the port name on your computer. Also make sure that your Linux user has access to read port (for example on Ubuntu Linux, check if the user is in dialout group).

The result will look similar to this one: