0
$\begingroup$

ver: lubuntu20.04, ros1-noetic, raspberry Pi 4

I am currently thinking of doing SLAM using a ToF sensor called vl53l1x. The SLAM I plan to use is Hector-SLAM.

However, I am confused about converting the data to the sensor_msgs::LaserScan type required for Hector-SLAM.

/*
 * STM VL53L1X ToF rangefinder driver for ROS
 *
 * Author: Oleg Kalachev <[email protected]>
 *
 * Distributed under BSD 3-Clause License (available at https://opensource.org/licenses/BSD-3-Clause).
 *
 * Documentation used:
 * VL53L1X datasheet - https://www.st.com/resource/en/datasheet/vl53l1x.pdf
 * VL53L1X API user manual - https://www.st.com/content/ccc/resource/technical/document/user_manual/group0/98/0d/38/38/5d/84/49/1f/DM00474730/files/DM00474730.pdf/jcr:content/translations/en.DM00474730.pdf
 *
 */

#include <string>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/Range.h>
#include <vl53l1x/MeasurementData.h>

#include "vl53l1_api.h"
#include "i2c.h"

#define xSTR(x) #x
#define STR(x) xSTR(x)

#define CHECK_STATUS(func) { \
    VL53L1_Error status = func; \
    if (status != VL53L1_ERROR_NONE) { \
        ROS_WARN("VL53L1X: Error %d on %s", status, STR(func)); \
    } \
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vl53l1x");
    ros::NodeHandle nh, nh_priv("~");

    sensor_msgs::Range range;
    vl53l1x::MeasurementData data;
    range.radiation_type = sensor_msgs::Range::INFRARED;
    ros::Publisher range_pub = nh_priv.advertise<sensor_msgs::Range>("range", 20);
    ros::Publisher data_pub = nh_priv.advertise<vl53l1x::MeasurementData>("data", 20);

    // Read parameters
    int mode, i2c_bus, i2c_address;
    double poll_rate, timing_budget, offset;
    bool ignore_range_status;
    std::vector<int> pass_statuses { VL53L1_RANGESTATUS_RANGE_VALID,
                                     VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL,
                                     VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE };

    nh_priv.param("mode", mode, 3);
    nh_priv.param("i2c_bus", i2c_bus, 1);
    nh_priv.param("i2c_address", i2c_address, 0x29);
    nh_priv.param("poll_rate", poll_rate, 100.0);
    nh_priv.param("ignore_range_status", ignore_range_status, false);
    nh_priv.param("timing_budget", timing_budget, 0.1);
    nh_priv.param("offset", offset, 0.0);
    nh_priv.param<std::string>("frame_id", range.header.frame_id, "");
    nh_priv.param("field_of_view", range.field_of_view, 0.471239f); // 27 deg, source: datasheet
    nh_priv.param("min_range", range.min_range, 0.0f);
    nh_priv.param("max_range", range.max_range, 4.0f);
    nh_priv.getParam("pass_statuses", pass_statuses);

    if (timing_budget < 0.02 || timing_budget > 1) {
        ROS_FATAL("Error: timing_budget should be within 0.02 and 1 s (%g is set)", timing_budget);
        ros::shutdown();
    }

    // The minimum inter-measurement period must be longer than the timing budget + 4 ms (*)
    double inter_measurement_period = timing_budget + 0.004;

    // Setup I2C bus
    i2c_setup(i2c_bus, i2c_address);

    // Init sensor
    VL53L1_Dev_t dev;
    VL53L1_Error dev_error;
    VL53L1_software_reset(&dev);
    VL53L1_WaitDeviceBooted(&dev);
    VL53L1_DataInit(&dev);
    VL53L1_StaticInit(&dev);
    VL53L1_SetPresetMode(&dev, VL53L1_PRESETMODE_AUTONOMOUS);

    // Print device info
    VL53L1_DeviceInfo_t device_info;
    CHECK_STATUS(VL53L1_GetDeviceInfo(&dev, &device_info));
    ROS_INFO("VL53L1X: Device name: %." STR(VL53L1_DEVINFO_STRLEN) "s", device_info.Name);
    ROS_INFO("VL53L1X: Device type: %." STR(VL53L1_DEVINFO_STRLEN) "s", device_info.Type);
    ROS_INFO("VL53L1X: Product ID: %." STR(VL53L1_DEVINFO_STRLEN) "s", device_info.ProductId);
    ROS_INFO("VL53L1X: Type: %u Version: %u.%u", device_info.ProductType,
              device_info.ProductRevisionMajor, device_info.ProductRevisionMinor);

    // Setup sensor
    CHECK_STATUS(VL53L1_SetDistanceMode(&dev, mode));
    CHECK_STATUS(VL53L1_SetMeasurementTimingBudgetMicroSeconds(&dev, round(timing_budget * 1e6)));

    double min_signal;
    if (nh_priv.getParam("min_signal", min_signal)) {
        CHECK_STATUS(VL53L1_SetLimitCheckValue(&dev, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, min_signal * 65536));
    }

    double max_sigma;
    if (nh_priv.getParam("max_sigma", max_sigma)) {
        CHECK_STATUS(VL53L1_SetLimitCheckValue(&dev, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, max_sigma * 1000 * 65536));
    }

    // Start sensor
    for (int i = 0; i < 100; i++) {
        CHECK_STATUS(VL53L1_SetInterMeasurementPeriodMilliSeconds(&dev, round(inter_measurement_period * 1e3)));
        dev_error = VL53L1_StartMeasurement(&dev);
        if (dev_error == VL53L1_ERROR_INVALID_PARAMS) {
            inter_measurement_period += 0.001; // Increase inter_measurement_period to satisfy condition (*)
        } else break;
    }

    // Check for errors after start
    if (dev_error != VL53L1_ERROR_NONE) {
        ROS_FATAL("VL53L1X: Can't start measurement: error %d", dev_error);
        ros::shutdown();
    }

    ROS_INFO("VL53L1X: ranging");

    VL53L1_RangingMeasurementData_t measurement_data;

    // Main loop
    ros::Rate r(poll_rate);
    while (ros::ok()) {
        r.sleep();
        range.header.stamp = ros::Time::now();

        // Check the data is ready
        uint8_t data_ready = 0;
        VL53L1_GetMeasurementDataReady(&dev, &data_ready);
        if (!data_ready) {
            continue;
        }

        // Read measurement
        VL53L1_GetRangingMeasurementData(&dev, &measurement_data);
        VL53L1_ClearInterruptAndStartMeasurement(&dev);

        // Publish measurement data
        data.header.stamp = range.header.stamp;
        data.signal = measurement_data.SignalRateRtnMegaCps / 65536.0;
        data.ambient = measurement_data.AmbientRateRtnMegaCps / 65536.0;
        data.effective_spad = measurement_data.EffectiveSpadRtnCount / 256;
        data.sigma = measurement_data.SigmaMilliMeter / 65536.0 / 1000.0;
        data.status = measurement_data.RangeStatus;
        data_pub.publish(data);

        // Check measurement for validness
        if (!ignore_range_status &&
            std::find(pass_statuses.begin(), pass_statuses.end(), measurement_data.RangeStatus) == pass_statuses.end()) {
            char range_status[VL53L1_MAX_STRING_LENGTH];
            VL53L1_get_range_status_string(measurement_data.RangeStatus, range_status);
            ROS_DEBUG("Range measurement status is not valid: %s", range_status);
            ros::spinOnce();
            continue;
        }

        // Publish measurement
        range.range = measurement_data.RangeMilliMeter / 1000.0 + offset;
        range_pub.publish(range);

        ros::spinOnce();
    }

    // Release
    ROS_INFO("VL53L1X: stop ranging");
    VL53L1_StopMeasurement(&dev);
    i2c_release();
}

The ToF sensor publish program is available from github.

Also, the program that receives the data is

#include <ros/ros.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>

ros::Publisher laser_scan_pub;

void chatterCallback(const sensor_msgs::Range& range_msg)
{
    // Create a LaserScan message
    sensor_msgs::LaserScan laser_scan_msg;
    laser_scan_msg.header = range_msg.header;

    // Set the LaserScan-specific parameters
    laser_scan_msg.angle_min = -M_PI / 4.0;     // Start angle (usually 0 radians)
    laser_scan_msg.angle_max = M_PI / 4.0;     // End angle (usually 0 radians)
    laser_scan_msg.angle_increment = M_PI / 180.0;  // Angle increment (usually 0 radians)
    laser_scan_msg.time_increment = 0.0;    // Time between measurements (usually 0 seconds)
    laser_scan_msg.scan_time = 0.1;         // Time taken for one scan (usually 0 seconds)
    laser_scan_msg.range_min = range_msg.min_range;
    laser_scan_msg.range_max = range_msg.max_range;

    // Calculate ranges array
    laser_scan_msg.ranges.push_back(range_msg.range);

    // Publish LaserScan message
    laser_scan_pub.publish(laser_scan_msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sensor_data");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/vl53l1x/range", 10, chatterCallback);
    laser_scan_pub = nh.advertise<sensor_msgs::LaserScan>("/laserscan", 10);
    ros::spin();

    return 0;
}

rostopic echo /laserscan This command confirms that the Topic is being received

header: 
  seq: 202
  stamp: 
    secs: 1697194120
    nsecs: 356778758
  frame_id: ''
angle_min: -0.7853981852531433
angle_max: 0.7853981852531433
angle_increment: 0.01745329238474369
time_increment: 0.0
scan_time: 0.10000000149011612
range_min: 0.0
range_max: 4.0
ranges: [0.8009999990463257]
intensities: []

However, when I tried to visualize the /laserscan Topic with rviz, the point cloud data was not displayed.

The publish side, /vl53l1x/range, could be visualized with rviz

I couldn't improve it on my own Anything is fine, so please lend me your help.

$\endgroup$

1 Answer 1

1
$\begingroup$

The "frame_id" in the header of your laserscan is not populated - RVIZ/TF has no idea where to put the data. You need to copy this data over from the range message.

Once that is fixed though, your data will likely not show up where you expect it, since your laserscan angle_min is a large negative value - this should probably be set to 0 assuming that the TOF sensor is aligned with the frame you set for frame_id.

$\endgroup$
1
  • $\begingroup$ thank you! If frame_id = "map", sensor_msgs::Range type was obtained. For the sensor_msgs::LaserScan type, if I increased the amount of sensor data published at once, I was able to display it with rviz. $\endgroup$
    – S.Tomo
    Commented Oct 16, 2023 at 10:20

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.