Using the pololu VL53L5CX (can be powered via 5v directly, connect VIN, SCL, SDA, & ground to the jetson
Clone this repo into a ROS 2 overlay workspace alongside px4_msgs:
colcon build --packages-select px4_msgs jetson_obs_distance
Source the overlay:
. install/setup.bash
There are a minimum 2 nodes in this package that need to be run in order to get obstacle distance map messages in PX4.
The jetson_obs_distance node runs the VL53L5CX driver and publishes to a laserscan message in ROS 2.
px4_obstacle_dist subscribes to the laserscan from one or more jetson_obs_distance instances and publishes them to a unified obstacle map in PX4.
vl53_set_address is required for setting the i2c address of multiple sensors in an array, but is optional if running only one sensor.
The default I2C bus index depends on the Jetson i2c port (and the jetson board used). Usually it is either bus 0, bus 1, or bus 7. For example on bus 0:
ros2 run jetson_obs_distance jetson_obs_distance --ros-args -p i2c_bus:=0
In order to change the i2c address of the sensor, connect each XSHUT to a different (but consecutive) GPIO on the jetson (E.g SPI1 gpio 12). Then run the vl53_set_address node with the param num_sensors set to the number of VL53L5CX devices attached to the bus. In the example of using gpio 12 (which is SPI1_MOSI on the nano) it will iterate num_sensors (gpio 12, 13, 14, etc).
gpio-12 (SPI1_MOSI |sysfs ) out hi
gpio-13 (SPI1_MISO |sysfs ) out hi
gpio-14 (SPI1_SCK |sysfs ) out hi
gpio-15 (SPI1_CS0 |sysfs ) out hi
Built and tested on ROS 2 Foxy. Built package in ROS 2 humble without issues but not tested.
Note that these sensors are for indoor use only! Outdoors the ambient light can be too strong for the VL53L5CX sensor.