Slam_toolbox: map is not creating

[Maxxbolotov]

Hello everyone.
Ros2 Humble
I’m using livox mid360 for Slam and FAST_LIO to get PointClouds. After I covert pointclouds to laserscan using pointcloud_to_laserscan package (target frame - body)

When I start Slam_toolbox to create a 2d map I face the issue that I only see an initial map, which is not updating. I guess that there is something wrong with my tf relations but not sure what to do to resolve.

P. S. I’m using default config file for online_async_mapping

[rtetete]

Ok let’s go step by step:

• First post here an image of Rviz showing the laser scan that you got after converting the pointcloud. Include the full tf tree of the robot. The fixed frame must be /odom
• Which are the names of your topics and frames you are using for slam_roolbox? Post them here.
• At which rate is the laser been published?
• Post here your configurations for slam_toolbox

[Maxxbolotov]

Providing the information as you asked:

1. Here is a picture of what topic /scan is showing after executing pointcloud_to_laserscan. Fixed frame named body as it’s target frame name in pointcloud_to_laserscan config (code of config below)
   
   

Node(
            package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
            remappings=[('cloud_in', '/cloud_registered_body'),
                        ('scan', '/scan')],
            parameters=[{
                'target_frame': 'body',
                'transform_tolerance': 0.01,
                'min_height': -1.0,
                'max_height': 1.5,
                'angle_min': -3.141592654,  # -M_PI/2
                'angle_max': 3.141592654,  # M_PI/2
                'angle_increment': 0.003141592,  # M_PI/360.0
                'scan_time': 0.03333,
                'range_min': 0.5,
                'range_max': 40.0,
                'use_inf': True,
                'inf_epsilon': 1.0
            }],
            name='pointcloud_to_laserscan'
        )

2. Fixed frame named odom show nothing (i guess its obvious as my target frame of laserscan named body. Do i need to replace it with odom?)
   (will post picture of it in the next post as i can’t post more than one picture in a post as a new member)

3. My slam_toolbox config (also containing names of frames of slam_toolbox)

slam_toolbox:
  ros__parameters:

    # 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_line
    scan_topic: /scan
    use_map_saver: true
    mode: mapping #localization

    # if you'd like to immediately start continuing a map at a given pose
    # or at the dock, but they are mutually exclusive, if pose is given
    # will use pose
    #map_file_name: test_steve
    # map_start_pose: [0.0, 0.0, 0.0]
    #map_start_at_dock: true

    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: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # 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.0
    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

4. Livox mid360 has its own odometry posting under the topic /Odometry

5. Rate of scan publishes is 10 Hz

6. Initial map showing after three transforms that i execute into the separate terminals
   ros2 run tf2_ros static_transform_publisher 0.0 0 0.0 0 0 0 1 body Odometry

ros2 run tf2_ros static_transform_publisher 0.0 0 0.0 0 0 0 1 Odometry map

ros2 run tf2_ros static_transform_publisher 0.0 0 0.0 0 0 0 1 map odom

but i don’t think these are correct transforms

[Maxxbolotov]

Pic for my second point:

[Maxxbolotov]

Sorry, forgot to use reply to tag you with my answer.

[rtellez]

Hi I can see that your problem is that you don’t have proper odometry.

You did a trick in the rviz image, you did not post it from fixed frame /odom frame but from body frame. I presume you did that because you cannot see the laser if you select /odom. That is what I wanted to see because that signals you have an error publishing your odometry. That is what can be seen in your second image.

Also I don’t know why you are publishing those static transforms. They make no sense. I presume you were trying to get it working doing tricks. But that is not how you have to do it. You have to provide the proper data to slam_toolbox not do tricks so slam_toolbox thinks it has the proper data.

The transform from /odom frame to body frame need to be published by the program that computes the odometry
The transform from /odom frame to /map frame will be published by slam_toolbox if you provide to it with the proper data.

So, the static transforms should not be published.
Also, you need to have a node that computes the odometry from the movement of the robot and publishes two things:

1. the odometry in the /odom topic
2. the tf transform from /odom to body. That is not a static transform!!! That needs to be properly computed continuously from the movement of the robot.

Check this course to better understand how odometry works (I think you lack that understanding):

[Maxxbolotov]

I’m sorry but it so complicated…
My odometry computes by FAST_LIO and publishes in the parent frame camera_init (made by fast_lio, child frame of it is body)
The odometry itself (I mean those red arrows that change its position while I’m moving lidar) publishes in a topic /Odometry
According to this and my previous info what and where i should change to make it work?

If i change odom frame in config of slam_toolbox from odom to camera_init - nothing happen.

[Maxxbolotov]

Providing more info of the problem:

1. Once i execute ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 odom camera_init RVIZ starts to show the map of environ. Providing the pic:
   
   

I name base_frame in slam_toolbox cfg as camera_init but the same is happening if i name base_frame as base_footprint and TF odom to base_footprint.
Problem is the map is not updating even when i move lidar. But /map topic says that it receiving messages.
2) TF tree i’ll provide in the next post as far as i can’t pin more than one picture in a post.
I guess the problem is there is no publishing between odom and camera_init (or base_footprint)

Edit: the TF map → created itself after I created odom → camera_init

[Maxxbolotov]

TF tree of the point 2

[Maxxbolotov]

Okay, I moved further. As far as my odometry frame is camera_init I set camera_init as odom_frame and body as base_frame in my params for slam_toolbox. Map has started to create but it’s overlapping. I guess it’s so because map is fixed to my odom frame. So while I move my lidar map is moving with it. Need to fix this.