adityakamath / tof_imager_ros

ROS 2 package for VL53L5CX and VL53L7CX ToF (8x8 Array) Imagers

tof_imager_ros

ROS 2 package for the VL53L5CX and VL53L7CX ToF (8x8 Array) Imager. The same implementation works for both sensors without any additional configuration.

Note: This implementation is a bit over-engineered, as I have been experimenting with ROS 2 managed/lifecycle nodes using Python.

Implementation details

• tof_imager_publisher: This executable uses the vl53l5cx_python library to access sensor data (for both sensors) over I2C. Distance measurements from the sensor are converted to Pointcloud2 messages which are published periodically using a timer to the /pointcloud topic. The sensor works in 8x8 and 4x4 modes, with the resolution of 8x8 as default. This implementation is designed as a lifecycle component and can be run individually as well.

• tof_imager_launch.py: This is the launch file that launches tof_imager_publisher as a lifecycle node, loads its parameters, and then configures and activates it. The lifecycle node is first initialized, and then set to 'configure' from the launch file. When the 'inactive' state is reached, the registered event handler activates the node.

Parameters

• frame_id: Parent frame for the Pointcloud2 message (Default: tof_frame)
• resolution: 4 for 4x4 OR 8 for 8x8 (Default: 8)
• mode: 1 for Continuous mode OR 3 for Autonomous mode (Default: 1)
• ranging_freq: Ranging frequency, limited to 15Hz for 8x8 and 60Hz for 4x4 (Default: 15)
• timer_period: Timer period in seconds (Default: 0.1)

More info about the mode and ranging frequency parameters can be found in the Datasheets for VL53L5CX and VL53L7CX, and Guides for VL53L5CX and VL53L7CX.

How to use

• Connect the VL53L5CX or VL53L7CX to the I2C GPIO pins of a Raspberry Pi device:
  • 3.3V to any 3.3V pin
  • SDA to BCM 2
  • SCL to BCM 3
  • GND to any ground pin
• Enable and update I2C on the Raspberry Pi Note: These steps are only for Ubuntu. For Raspbian, follow the steps here
  • Enable I2C:

    sudo apt install -y i2c-tools python3-pip
    sudo pip3 install smbus2
    

  • Update baud rate by editing the I2C setting in /boot/firmware/config.txt with the baud rate (1MHz in this case):

    dtparam=i2c_arm=on,i2c_arm_baudrate=1000000
    

• Install the vl53l5cx library (this library works for both sensors):

  $ git clone https://github.com/Abstract-Horizon/vl53l5cx_python
  $ cd vl53l5cx_python
  $ sudo python3 setup.py install
  

• Clone this repository in a ROS 2 workspace. Check the sensor_params.yaml file in the config directory, and make any necessary changes.
• Build the package and run the launch file: ros2 launch tof_imager_ros tof_imager_launch.py

Results

This package was tested using both VL53L5CX ToF Imager and VL53L7CX and a Raspberry Pi 4 (4GB) running ROS 2 Humble.

The following results were achieved for the two devices:

VL53L5CX:

• With default I2C baud rates:
  • resolution=8, ranging_freq=15Hz, Both modes: ~6.1Hz
• With I2C baud rate=1MHz:
  • resolution=8, ranging_frequency=15Hz:
    • Continuous mode, timer_period=0.1: ~15Hz
    • Autonomous mode, timer_period=0.1: ~10Hz
    • Autonomous mode, timer_period=0.01: ~15Hz
  • resolution=4, ranging_frequency=60Hz:
    • Both modes, timer_period=0.01: ~56.5Hz
    • Both modes, timer_period=0.025: ~40Hz

VL53L7CX:

• With I2C baud rate=1MHz:
  • resolution=8, ranging_frequency=15Hz:
    • Continuous mode: ~7Hz
    • Autonomous mode: ~6Hz
  • resolution=4, ranging_frequency=60Hz, Both modes: ~18Hz:

tof_imager_ros's Issues

Support for vl53l7cx

and rename the repo vl53lxcx_ros

Pacakge not found in ROS2

Hello, thank you for distributing this good materials.
But I had a problem with my ROS2 humble package couldn't launch the vl53l5cx_ros package and tof_imager_launch.py.

• OS: Ubuntu 23.10 LTS
• ROS2 version: ROS2 Humble
• Working structure: Local PC(WSL) --> Raspberrypi 5(conneted by SSH, ubuntu 23.10)

In the description,

• Build the package and run the launch file: ros2 launch vl53l5cx_ros tof_imager_launch.py
  I had cloned tof_sensor_python repository to ROS2 workspace, and completed build.
  • command by colcon build

But my ROS2 work space and all packages could't find and build the package that vl53l5cx_ros when I launched by ros2 launch vl53l5cx_ros tof_imager_launch.py.

Is there any way to build that package? or other way to launch?

Failed to make transition 'TRANSITION_CONFIGURE' for LifecycleNode '/tof_imager'

          Thanks for reply, just one more question!

the senseHAT problem was solved.
but when I launched tof_imager_launch.py, this error message is shown.

[INFO] [launch]: All log files can be found below /home/peav/.ros/log/2024-06-07-10-24-43-564223-bot1-7452
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [tof_imager_publisher-1]: process started with pid [7463]
[tof_imager_publisher-1] [INFO] [1717723485.170278519] [tof_imager]: Initialized
[ERROR] [launch_ros.actions.lifecycle_node]: Failed to make transition 'TRANSITION_CONFIGURE' for LifecycleNode '/tof_imager'

• Is there any hardware problem or something else to fail the initialization of configure state?
• We're almost there to get the solution!

Originally posted by @SewoongPark in #2 (comment)