No map topic and visualization

[ShameerMasroor]

Hello!

I am trying to integrate your work with a project that I am working on.

My project involves using a drone affixed with a 3D Lidar to map a forest and localize itself.

My drone publishes the following topics, the relevant ones are /scan and /imu/out, and the namespace is simple_drone.

/scan is published as a topic with a message type of PointCloud2, and the IMU topic is of type Imu.

I tried to remap the parameters in the lidar_slam.launch.py file to the ones for my drone, as shown in the following code:

import os

import launch
import launch_ros.actions

from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

main_param_dir = launch.substitutions.LaunchConfiguration(
    'main_param_dir',
    default=os.path.join(
        get_package_share_directory('lidarslam'),
        'param',
        'lidarslam.yaml'))

rviz_param_dir = launch.substitutions.LaunchConfiguration(
    'rviz_param_dir',
    default=os.path.join(
        get_package_share_directory('lidarslam'),
        'rviz',
        'mapping.rviz'))

mapping = launch_ros.actions.Node(
    package='scanmatcher',
    executable='scanmatcher_node',
    parameters=[main_param_dir],
    remappings=[('/input_cloud','/simple_drone/scan')],
    output='screen'
    )

tf = launch_ros.actions.Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0','0','0','0','0','0','1','simple_drone/base_link','simple_drone/laser_frame']
    )


graphbasedslam = launch_ros.actions.Node(
    package='graph_based_slam',
    executable='graph_based_slam_node',
    parameters=[main_param_dir],
    output='screen'
    )

rviz = launch_ros.actions.Node(
    package='rviz2',
    executable='rviz2',
    arguments=['-d', rviz_param_dir]
    )


return launch.LaunchDescription([
    launch.actions.DeclareLaunchArgument(
        'main_param_dir',
        default_value=main_param_dir,
        description='Full path to main parameter file to load'),
    mapping,
    tf,
    graphbasedslam,
    rviz,
        ])`

What I feel that the issue lies in is with the transform frames. Do I need to make a transformation between the map and the base_link of the drone? Apologies as I am a beginner with ROS at the moment.

Terminal output on running the lidarslam launch file

Secondly, RVIZ shows absolutely no output. I see that some /map and /modified map is being published, but PointCloud2 is not visualizing it.

It seems the map frame is missing

The following shows the point cloud created by the lidar:

I am unsure as to what the issue is. Any help would be greatly appreciated.

[rsasaki0109]

The scan matcher hasn't been running since the node initialization, so it might simply be that no scans are being fed into the scan matcher. I suggest checking the connections with commands like ros2 topic info -v, ros2 node info, or using rqt_graph to ensure there are no issues.
Another possibility could be modifying the frame_id in lidarslam.yaml.

[ShameerMasroor]

Do I need to define some additional transform for the map to base_link or will the scanmatcher do that itself?

[rsasaki0109]

Probably it does that itself... Sorry, I haven't worked with tf recently, so my memory is a bit vague.
https://github.com/rsasaki0109/lidarslam_ros2/blob/develop/scanmatcher/src/scanmatcher_component.cpp#L416

[ShameerMasroor]

Hello!

The issue was resolved. The drone base_link transform frame had a "/" at the start of its name. The tf system was throwing errors but they were not being visualized.

Also, the lidarslam.yaml file had to be updated to the new transform frame name.

The package now works. I now need to tune the parameters of the graph_based_slam node so that it performs better optimization. The current setting seems to over-optimize it, optimizing curves in the trajectory as straight lines.

It works nonetheless.

[rsasaki0109]

The low resolution of the green path compared to the yellow path should not be an issue. I believe the map is properly created.

ref
#101 (comment)

[ahsan155]

@ShameerMasroor can you show what your tf looks like and what changes you made in lidarslam.launch.py and lidarslam.yaml file. this is what I have in lidarslam.launch.py

mapping = launch_ros.actions.Node(
     package='scanmatcher',
     executable='scanmatcher_node',
     parameters=[main_param_dir],
     remappings=[('/input_cloud','/ouster/points')],
     output='screen'
     )

 tf = launch_ros.actions.Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     arguments=['0','0','0','0','0','0','1','os_sensor','os_lidar']
     ) 

and lidarslam.yaml

scan_matcher:
  ros__parameters:
    global_frame_id: "map"
    robot_frame_id: "os_sensor"
    registration_method: "NDT"

my tf is
map -> os_sensor -> os_lidar

I have the same issue. There is no visualization and map topic doesn't output anything.

[ShameerMasroor]

@ShameerMasroor can you show what your tf looks like and what changes you made in lidarslam.launch.py and lidarslam.yaml file.

TF Tree:

  mapping = launch_ros.actions.Node(
          package='scanmatcher',
          executable='scanmatcher_node',
          parameters=[main_param_dir],
          remappings=[('input_cloud', 'simple_drone/scan')], 
          output='screen'
          )


    tf = launch_ros.actions.Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', '1', 'simple_drone/base_link', 'simple_drone/laser_frame']
        )

My yaml file:

scan_matcher:
  ros__parameters:
    global_frame_id: "map"
    robot_frame_id: "simple_drone/base_link"
    registration_method: "NDT"
    ndt_resolution: 2.0
    ndt_num_threads: 4
    gicp_corr_dist_threshold: 5.0
    trans_for_mapupdate: 1.5
    vg_size_for_input: 0.5
    vg_size_for_map: 0.3
    use_min_max_filter: true
    scan_min_range: 0.1
...

My rqt_graph looks like this:

my tf is map -> os_sensor -> os_lidar

You need to ensure that there is a transform from the robot base link to the sensor frame. I am not sure if os_sensor is a base_link or simply the sensor frame.
Can you please share your transform tree?
You can use: ros2 run tf2_tools view_frames
Also share the topic list (ros2 topic list)

[ShameerMasroor]

@rsasaki0109 I additionally wish to know if your work is an implementation of some paper. It would be great if you could tell me as that paper can teach me much more about how this algorithm is working.

Thank you

[ahsan155]

@ShameerMasroor Thanks for the response. It actually works with the tf configuration I posted above. It works when I publish lidar data with command ros2 bag play file_name -r 2.5. It did not work when I published lidar from a mcap file with a python script.

[rsasaki0109]

@ShameerMasroor
There isn’t a specific paper for this. To explain briefly, LiDAR odometry is performed using NDT scan matching between the current scan and an accumulated mini-map of scans. Loop detection is achieved by matching submaps created by sampling the trajectory of LiDAR odometry at regular intervals.

I believe it is using the orthodox algorithm of that time. Useful references might include Autoware's NDT Mapping, HDL Graph SLAM, BLAM, and LEGO LOAM.