Using slam_toolbox for localization

[Agri-Mechatronic]

• Operating System:
  • Ubuntu 22.04
• Installation type:
• ROS Version
  • ROS2 Humble

Hello,

I am using turtlebot3 simulation to simulate localization in pre-saved and serialized map. I am experiencing issues with slam_toolbox in localization mode while using it in a simulation environment. Initially, the localization works correctly, but after a few seconds, the robot's pose becomes inconsistent, and it seems to jump to random position on the map. The following points describe the problem in detail:

I have ensured that there are no inf values in the laser scan ranges.
However, I still see the following warning repeatedly in the logs:

Info: clipped range threshold to be within minimum and maximum range!
I wonder if this could be affecting the scan matching process and leading to incorrect localization.

TF Tree and Topic Rates:

The TF tree looks correct, and the parent-child relationships are properly defined.
Topic rates are as follows:
/filtered_scan: ~10 Hz
/odom: ~29 Hz
/tf: ~49 Hz
Despite these seemingly correct rates, the localization eventually fails.

Attempted Solutions:

I have followed the discussions in related GitHub issues (#720, #611, #576, #465, #422, #415) but have not found a solution that resolves my problem.
I tried using debug logs and tools like swri_console to understand the issue better, but nothing conclusive was found.

Launch Command:

ros2 launch slam_toolbox localization_launch.py params_file:=/path/to/mapper_params_localization.yaml

mapper_params_localization.yaml:

slam_toolbox:
  ros__parameters:
    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
    use_sim_time: true
    mode: "localization"
    map_file_name: "/home/user/ros2_ws/maps/map_for_localization"
    scan_topic: /filtered_scan
    base_frame: base_footprint
    odom_frame: odom
    map_frame: map
    transform_publish_period: 0.1
    map_update_interval: 0.5
    transform_tolerance: 0.1

    # if you'd like to start localizing on bringup in a map and pose
    #map_file_name: test_steve
    map_start_pose: [0.0, 0.0, 0.0]

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.05 #if 0 never publishes odometry
    map_update_interval: 0.0
    resolution: 0.05
    min_laser_range: 0.15 #for rastering images
    max_laser_range: 3.5 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 100.0
    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.2
    minimum_travel_heading: 0.2
    scan_buffer_size: 5
    scan_buffer_maximum_scan_distance: 8.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    do_loop_closing: true 
    loop_match_minimum_chain_size: 3
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45
    
    scan_queue_size: 50
    scan_qos_history: "keep_last"
    scan_qos_reliability: "reliable"
    scan_qos_durability: "volatile"
    num_threads: 4

    # 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
    loop_search_maximum_distance: 3.0

    # 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

Thanks everyone...

[SteveMacenski]

This is Nav2, not slam toolbox, please file with the correct repository.

[Agri-Mechatronic]

Sorry but I could not understand, isn't this slam_toolbox issues page?

[SteveMacenski]

Oh sorry, that's my bad misreading my notifications tray 😆

Info: clipped range threshold to be within minimum and maximum range!

It occurs here, which is called from here. What I can ascertain is that max_laser_range parameter (3.5) was clipped to be within the min/max ranges - so most likely your scan that's being published from the TB3 is > 3.5 range.

[Agri-Mechatronic]

Thank you for your response;

Could this be the underlying cause of the localization issue I'm experiencing? When I start the localization process, the robot initially localizes itself correctly for a few seconds. However, shortly after, I see the message "[localization_slam_toolbox_node-1] Registering sensor: [Custom Described Lidar]" in the logs, and the robot's position on the map changes unexpectedly.

Below is the output with additional debug logs that I added for further insight:

[localization_slam_toolbox_node-1] Mapper <- Module
[localization_slam_toolbox_node-1] Mapper <- m_pSequentialScanMatcher
[localization_slam_toolbox_node-1] Mapper <- m_pGraph
[localization_slam_toolbox_node-1] MapperGraph <- Graph; Graph <- m_Edges; Graph <- m_Vertices
[localization_slam_toolbox_node-1] MapperGraph <- m_pMapper; MapperGraph <- m_pLoopScanMatcher; MapperGraph <- m_pTraversal
[localization_slam_toolbox_node-1] Mapper <- m_pMapperSensorManager
[localization_slam_toolbox_node-1] MapperSensorManager <- m_ScanManagers; MapperSensorManager <- m_Scans
[localization_slam_toolbox_node-1] Mapper <- m_Listeners
[localization_slam_toolbox_node-1] **Finished serializing Mapper**
[localization_slam_toolbox_node-1] Load From File
[localization_slam_toolbox_node-1] **Serializing Dataset**
[localization_slam_toolbox_node-1] Dataset <- m_SensorNameLookup
[localization_slam_toolbox_node-1] Dataset <- m_Data
[localization_slam_toolbox_node-1] Dataset <- m_Lasers
[localization_slam_toolbox_node-1] Dataset <- m_pDatasetInfo
[localization_slam_toolbox_node-1] **Finished serializing Dataset**
[localization_slam_toolbox_node-1] Registering sensor: [Custom Described Lidar]
[localization_slam_toolbox_node-1] WARNING: Logging before InitGoogleLogging() is written to STDERR
[localization_slam_toolbox_node-1] W0120 00:37:52.146977 60612 preprocessor.cc:62] Specified options.num_threads: 50 exceeds maximum available from the threading model Ceres was compiled with: 12.  Bounding to maximum number available.
[localization_slam_toolbox_node-1] [INFO] [1737329872.239306558] [slam_toolbox]: Laser scan received with 360 ranges
[localization_slam_toolbox_node-1] [INFO] [1737329872.239358746] [slam_toolbox]: First range value: 3.400000
[localization_slam_toolbox_node-1] [INFO] [1737329872.239370358] [slam_toolbox]: Constructor called in laserCallback
[localization_slam_toolbox_node-1] [WARN] [1737329872.239541118] [slam_toolbox]: minimum laser range setting (0.1 m) exceeds the capabilities of the used Lidar (0.1 m)
[localization_slam_toolbox_node-1] [INFO] [1737329872.239623543] [slam_toolbox]: Constructor called in addScan
[localization_slam_toolbox_node-1] Info: clipped range threshold to be within minimum and maximum range!
[localization_slam_toolbox_node-1] Registering sensor: [Custom Described Lidar]
[localization_slam_toolbox_node-1] [INFO] [1737329876.041543293] [slam_toolbox]: Laser scan received with 360 ranges
[localization_slam_toolbox_node-1] [INFO] [1737329876.041585242] [slam_toolbox]: First range value: 3.400000
[localization_slam_toolbox_node-1] [INFO] [1737329876.041595040] [slam_toolbox]: Constructor called in laserCallback
[localization_slam_toolbox_node-1] [INFO] [1737329876.042225084] [slam_toolbox]: Laser scan received with 360 ranges
[localization_slam_toolbox_node-1] [INFO] [1737329876.042249149] [slam_toolbox]: First range value: 3.400000
[localization_slam_toolbox_node-1] [INFO] [1737329876.042261462] [slam_toolbox]: Constructor called in laserCallback
[localization_slam_toolbox_node-1] [INFO] [1737329876.042496353] [slam_toolbox]: Laser scan received with 360 ranges
[localization_slam_toolbox_node-1] [INFO] [1737329876.042508787] [slam_toolbox]: First range value: 3.400000

I have created one node for limiting datas from lidar sensor and it outputs datas like:

header:
  stamp:
    sec: 19
    nanosec: 511000000
  frame_id: base_scan
angle_min: 0.0
angle_max: 6.28000020980835
angle_increment: 0.01749303564429283
time_increment: 0.0
scan_time: 0.0
range_min: 0.10000000149011612
range_max: 3.4000000953674316
ranges:
- 3.4000000953674316
- 3.4000000953674316
- 3.4000000953674316
- 3.4000000953674316
- 3.4000000953674316
- 3.4000000953674316
- 3.4000000953674316
- 3.1055243015289307
- 3.0426688194274902
- 3.0501654148101807
- 2.038742780685425
- 1.9977275133132935
- 1.985163688659668

And from the yaml file of localization, we can see min and max values:

    use_sim_time: true
    mode: "localization"
    map_file_name: "/home/user/ros2_ws/maps/map_for_localization"
    scan_topic: /filtered_scan
    base_frame: base_footprint
    odom_frame: odom
    map_frame: map
    transform_publish_period: 0.1
    map_update_interval: 0.5
    transform_tolerance: 0.1

    # if you'd like to start localizing on bringup in a map and pose
    #map_file_name: test_steve
    map_start_pose: [0.0, 0.0, 0.0]

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.05 #if 0 never publishes odometry
    map_update_interval: 0.0
    resolution: 0.05
    min_laser_range: 0.1 #for rastering images
    max_laser_range: 3.4 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 100.0
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps

But I can't still solve the problem with localization. I want turtlebot3 to localize itself in the pre-saved map without needing to set initial position from rviz or something. Weird part here is that localization works for couple of seconds and then breaks.

One solution would be to echo the /pose topic when localization starts but no luck. It publishes data to /pose topic after localization breaks (after couple of seconds), so data is incorrect.

Thank you again...

[0RBalaji]

I see you are using:
ros2 launch slam_toolbox localization_launch.py params_file:=/path/to/mapper_params_localization.yaml
It needs nav2_params.yaml configuration file, not the mapper_params file.
Check your launch file and see if the node executables match with the config node names

[Agri-Mechatronic]

Thanks for the reply. This is my localization_launch.py file:

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_params_file = LaunchConfiguration('slam_params_file')

    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation/Gazebo clock')
    declare_slam_params_file_cmd = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(get_package_share_directory("slam_toolbox"),
                                   'config', 'mapper_params_localization.yaml'),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node')

    start_localization_slam_toolbox_node = Node(
        parameters=[
          slam_params_file,
          {'use_sim_time': use_sim_time}
        ],
        package='slam_toolbox',
        executable='localization_slam_toolbox_node',
        name='slam_toolbox',
        output='screen')

    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time_argument)
    ld.add_action(declare_slam_params_file_cmd)
    ld.add_action(start_localization_slam_toolbox_node)

    return ld

This is my nav2_params.yaml file that I use for navigation:

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node

bt_navigator_navigate_through_poses_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator_navigate_to_pose_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

map_server:
  ros__parameters:
    use_sim_time: True
    # Overridden in launch by the "map" launch configuration or provided default value.
    # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
    yaml_filename: ""

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    use_sim_time: True
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0