mapping with Velodyne VLP-16

[Hi-Zed]

I have two turtlebot2 equipped with the same configuration except for the laser. One mounts an hokuyo while the other a Velodyne VLP-16. I obtained very satisfying results with the hokuyo using the default configuration for online synchronous mapping. However, when I try to run it with the velodyne nothing happens. The node starts correctly but there is no map.

The configuration of the robot is fine, since if I run gmapping it can create the map.

[SteveMacenski]

Please provide a bag file with scan, tf and tf static, as well as your yaml file.

Are you sure the topics and types line up correctly?

[Hi-Zed]

https://drive.google.com/open?id=1e2TgHA-UAygKAeXrSB4PvHB1b3v_surP

Here you can find a bag, I used it to create a map with gmapping and this is the result.

This is the yaml file I used to run slam_toolbox, it is basically the default one except for the max_laser_range

# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None

# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
mode: mapping #localization

debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 10.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps

# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1  
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true 
loop_match_minimum_chain_size: 10           
loop_match_maximum_variance_coarse: 3.0  
loop_match_minimum_response_coarse: 0.35    
loop_match_minimum_response_fine: 0.45

# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1 

# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03

# Scan Matcher Parameters
distance_variance_penalty: 0.5      
angle_variance_penalty: 1.0    

fine_search_angle_offset: 0.00349     
coarse_search_angle_offset: 0.349   
coarse_angle_resolution: 0.0349        
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

This is the configuration of the tf. map -> odom is present because slam_toolbox was running. frames.pdf

[SteveMacenski]

OK, I'll take a look at this as I have time this week. Might be a couple days.

Also what version of this are you running? I have 4 prod branches and 5 releases ;-)

Just a couple quick questions that might help you to find your issue

• you are publishing a LaserScan type message from the VLP
• I see you didnt set the scan topic parameter, you sure they're actually lining up?
• That bag file already has a odom->map transform being published, what's that from?
• If you turn debug_logging to true, what's the output? Are you seeing TF message filter failures from no available TF or something?

Do you see any errors?

[SteveMacenski]

I got a map -- ok I got an error repetitively:

LaserRangeScan contains 897 range readings, expected 899

Did you get that? (would have been nice to know :wink:).

That leads me to a couple questions:

• How are you getting the horizontal scan from the pointcloud? Another user with a VLP-16 didnt have this problem with pointcloud to laser scan nodelet
• Either my math is wrong for 360 lidars, or the driver that's giving you that info isn't filling in the message correctly, and I think in your case its the latter. Your sensor has 897 points coming out of that 360 lidar. 2*pi/897 the increment should be 0.0070124836 but your messages contain 0.00700000021607 (0.007 with floating point error). With the correct increment, it works fine.

header: 
  seq: 724
  stamp: 
    secs: 1576000244
    nsecs: 367226000
  frame_id: "velodyne"
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_increment: 0.00700000021607
time_increment: 0.0
scan_time: 0.0
range_min: 0.0
range_max: 200.0
ranges: "<array type: float32, length: 897>"
intensities: "<array type: float32, length: 897>"

Now, you're not the first person to have this issue with incorrectly made drivers, its becoming somewhat of a theme in the issues I get in this repo. The section of code needed to be removed to run with a wrong increment value is likes 216 - 222 in Karto.cpp. It could be possible to add a parameter here to toggle the validation check. If you feel that's what you'd rather do over changing your driver, I'd merge a PR of that and readme documentation about it. It is dangerous in general though if your driver is really wrong, it can create substantial distortion in the map. That's why we check and enforce the correct number so that we know the increment is correct. But you should probably just change the angle increment in your driver to be accurate.

[Hi-Zed]

Thank you for your help. Sorry for not reporting the error, but in my case it was appearing only once at the beginning, so I thought it was just a single malformed message. Especially because when enabling the debug mode I was getting multiple messages as the node was working. This is an example:

[DEBUG] [1576066747.471897984]: MessageFilter [target=odom ]: Message ready in frame velodyne at time 1576066747.448, count now 0
[DEBUG] [1576066747.555315651]: MessageFilter [target=odom ]: Added message in frame velodyne at time 1576066747.549, count now 1

Just for context, I am running ROS Melodic on Ubuntu 18.04, and I installed the package directly from apt.

For the scan topic I am running the default driver for the VLP-16 which provides both the pointcloud and the laser scan. I will try the nodelet to solve the problem.

[SteveMacenski]

There’s no “default” driver in ros for any sensor, can you provide a link? Is the scan increment parametrizable or dynamic reconfigurable? You really should do that first and foremost before trying anything else. All other options are going to have increased load.

[Hi-Zed]

This is the package I am using: http://wiki.ros.org/velodyne There is no other package directly available from apt that support the velodyne.

The increment is a parameter. However, changing it does not fix the problem, because a greater increment (e.g., 0.0070124836) just reduce the size of the array (from 897 to 895) giving basically the same error.

I did some digging in the code of https://github.com/ros-drivers/velodyne/blob/master/velodyne_laserscan/src/velodyne_laserscan.cpp Their implementation is reasonable:

const float RESOLUTION = std::abs(cfg_.resolution);
const size_t SIZE = 2.0 * M_PI / RESOLUTION;
sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
scan->header = msg->header;
scan->angle_increment = RESOLUTION;
scan->angle_min = -M_PI;
scan->angle_max = M_PI;
scan->range_min = 0.0;
scan->range_max = 200.0;
scan->time_increment = 0.0;
scan->ranges.resize(SIZE, INFINITY);

With a RESOLUTION value of 0.007 and the value of M_PI provided by math.h of 3.14159265358979323846 the SIZE is 897.59. It is then truncated to 897. However, Karto.cpp matches it against 899, which is unreachable even when rounding up.

By the way, if you consider pi as M_PI and take 897 as number of point, the resolution is 0.00700466589. Even with pi as 3.1415 the resolution for 897 point is 0.0070044593. So, I guess your math is wrong somewhere, when assuming that 2*pi/897 is 0.0070124836.

[SteveMacenski]

I mean regardless of any logic around this package, that package, any package, we can agree that 2*pi / 0.007 != 897, which means the velodyne drivers are wrong. Flooring a partial measurement (0.59!! not even 0.01 or something negligible) is poor engineering and clearly at minimum a partial cause of this issue, as well as messing up its own internal operations.

The fact that the resizing of the ranges is based on some parameter that can randomly change is also very, very wrong and can cause segmentation faults. I could go off on a much longer tangent about this, but suffice to say, this driver seems to have substantial issues.

The value is correct because while 360 / 897 is not exactly 0.0070124836 but a lidar isnt returning a measurement (or again, it shouldnt) at 0 degrees and 360 degrees, because those are the same points. Its [0, 360), so you have delta 1 point from the total number of points to get the increment. This is also what I do in my library, and what your Hokuyo, SICK, etc lidars do as well in their correctly formed drivers.

If you would like to enable a parameter for 360 lidars, or disabling the check, I'd merge your PR, but that may be masking issues. It seems to work fine, and maybe that small difference in angle doesn't really matter relative to the distance accuracy of the sensor, but its something to keep in the back of your mind if calibration is important. Another ticket I am working on has a 3D lidar with a similar thing and they removed the +1 from

    void Update()
    {
      m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
                                                                    GetMinimumAngle())
                                                                    / GetAngularResolution()) + 1);
    }

It may be that 3D lidars have a different convention in on inclusion or increment. I'd be OK with it being is_360_lidar boolean as a configuration, passed into LaserAssistant::makeLaser(const double& mountingYaw, const bool& is_360_lidar), and finally given to this Update() method as an alternative computation.

[SelvamArul]

@SteveMacenski I am using Velodyne VLP-16 laser as well. I had the same issue and removing +1 in Update () fixed the issue. I would like to submit a PR as you suggested but I need a small clarification. If the proposal to make a new boolean is_360_lidar a ROS param, can we simple use ros node handle to get the param inside LaserAssistant::makeLaser. This line inside the LaserAssistant::makeLaser function already used node handle to get ros param. Similary we can get is_360_lidar param.

When you proposed to modify the LaserAssistant::makeLaser to have const bool& is_360_lidar as second argument, did you have something else in mind?

[SteveMacenski]

A couple options

• have a param that is read in with all the other laser params (topic, etc) and then that param is given to the constructor of the lasermeta data, which then can be given each time the validation method is called
• look at the laser message itself, and if its -pi to pi, you give a bool for 360 or not to validate similarly, that way no param required
• change validate from returning false and change the stdout as a “WARN:” to print once.

All are reasonable options. Option 2 is my preference so theres no extra parameters and it “just works”.

[austin-InDro]

Hello,

I'm still having this exact same issue on ROS Melodic using Ubuntu 18.04 with a Velodyne 16. Was this merged to the Melodic branch?

Thanks, Austin