Ensure constant beam count for ROS2 SLAM Toolkit and Nav2 to work

[slgrobotics]

I am using LD14 device with the following launch description:

    ldlidar_node = Node(
        package='ldlidar_sl_ros2',
        executable='ldlidar_sl_ros2_node',
        name='ldlidar_publisher_ld14',
        output='screen',
        respawn=True,
        respawn_delay=10,
        parameters=[
          {'product_name': 'LDLiDAR_LD14'},
          {'laser_scan_topic_name': 'scan'},
          {'point_cloud_2d_topic_name': 'pointcloud2d'},
          {'frame_id': 'laser_frame'},
          {'port_name': '/dev/ttyUSBLDR'},
          {'serial_baudrate' : 115200},
          {'laser_scan_dir': True},
          {'enable_angle_crop_func': False},
          {'angle_crop_min': 135.0},
          {'angle_crop_max': 225.0}
        ]
    )

The "demo.cpp" code passes each LIDAR head rotation "beams vector" as it comes from the driver, with the beam count dependent on the head rotation speed. In my case, the beam count varies in the range 388...393, so that the "/scan" topic is also containing variable point count. ROS2 Jazzy SLAM Toolbox treats this as error - it expects all incoming messages to contain the same amount of points.

To fix that, I added code to measure an average of 20 first valid scans (first scan is skipped as it always comes truncated), and to resize the resulting "scan vector" to that average size (if it needs expanding in the process, the last element is used to fill the void). So, all messages come to ROS2 with the same point count.

The modification is tested on Raspberry Pi 5 under Ubuntu 24.04 and ROS2 Jazzy. SLAM Toolbox and Nav2 works fine now.

More detail about that robot is here: https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Dragger/README.md

[slgrobotics]

See also #2 for the #include <pthread.h> issue (fixed in this PR)

[DSJZS]

@slgrobotics, thank you for your contribution. This fix resolved the issue I was experiencing.

Previously, no matter how I configured slam_toolbox, I couldn't get proper mapping results—the output was either missing or very strange, and adjusting the slam_toolbox parameters didn't help.

After checking the logs and noticing the error: "LaserRangeScan contains 398 range readings, expected 399", I came to this repository and immediately found your solution.

Great work!