Map timestamp always 0.0 when localizing

[marco-monforte]

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • binaries
• ROS Version
  • ROS2 Humble

Hi, I'm trying to use slam_toolbox to create a map of my simulated environment and then localize asynchronously.

I was able to create and serialize the map without any issues but then, when I launch the file for localization, for some reason the map is not taken by Rviz. It is published but with timestamp always 0.0. Rviz doesn't show it and doesn't see the parameters of the map *.yaml (of course).

What puzzles me is that if I launch another Rviz instance and add a new Map, it works.

This is my config file:

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_link
    scan_topic: /top_laser_controller/out
    mode: 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: /ros_ws/src/ros2-valkyrie/valkyrie_localization/maps/alto_office/alto_office_serialized
    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: 0.1
    resolution: 0.05
    max_laser_range: 12.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 10.0
    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.2 #// always refresh!
    minimum_travel_heading: 0.1 #// always refresh!
    scan_buffer_size: 100
    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

Any idea where I might be wrong?

[SteveMacenski]

https://github.com/SteveMacenski/slam_toolbox/blob/ros2/src/slam_toolbox_common.cpp#L645C27-L645C38

It appears that we timestamp it with the last laser reading, so if none have come in yet for the first after deserialization, then I can see how 0 happens. Any suggestions? :-)

[marco-monforte]

There must be something broken in the TF tree.

The /map, /odom, and /top_laser_controller/out are publishing correctly. Also, I get a TF with ros2 run tf2_ros tf2_echo odom base_link. What I don't get is the TF from ros2 run tf2_ros tf2_echo map odom.

The lidar messages are not consumed, since I get [slam_toolbox]: Message Filter dropping message: frame 'top_laser_frame' at time XX.XXX for reason 'discarding message because the queue is full'. If I've understood correctly, this is what's causing the map to be initially timestamped with 0, but I can't understand what's the origin of the missing TF :thinking:

EDIT: I'm sorry but I'm new to ROS2 and slam_toolbox, so it is all a bit confusing to me

[marco-monforte]

Ok, I've found the error. The timestamp of the odometry was wrong. Once fixed, slam_toolbox worked properly :rocket: