"Discrete jumps" from odom frame while using slam toolbox with gazebo. #802
Description
Hi, I have an issue when using slam toolbox in a simulated environment, what is happening is that whenever I start this launch file:
import os
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
def generate_launch_description():
use_sim_time = LaunchConfiguration("use_sim_time")
gazebo = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_description"),
"launch",
"gazebo_V1.launch.py"
),
launch_arguments={
"use_sim_time": use_sim_time
}.items()
)
controller = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_controller"),
"launch",
"controller_V1.launch.py"
),
launch_arguments={"use_simple_controller": "False",
"use_sim_time": use_sim_time,
"use_python": "False"}.items()
)
ekf_localization_filter = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_localization"),
"launch",
"local_localization.launch.py"
),
launch_arguments={"use_sim_time": use_sim_time}.items()
)
# SLAM Toolbox
slam_toolbox = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[
os.path.join(
get_package_share_directory('skid_steer_slam'),
'config',
'mapper_params_online_async.yaml'
),
{"use_sim_time": use_sim_time}
],
remappings=[
('/scan', '/ldlidar_node/scan')
]
)
nav2_launch = IncludeLaunchDescription(
os.path.join(
get_package_share_directory('skid_steer_navigation'),
'launch',
'nav2_bringup.launch.py'
),
)
# Twist Mux
twist_mux_config = os.path.join(
get_package_share_directory('skid_steer_controller'),
'config',
'twist_mux.yaml'
)
twist_mux = Node(
package='twist_mux',
executable='twist_mux',
name='twist_mux',
parameters=[twist_mux_config, {"use_sim_time": use_sim_time}],
remappings=[
('/cmd_vel_out', '/skid_steer_controller_V1/cmd_vel_unstamped')
],
output='screen'
)
return LaunchDescription([
DeclareLaunchArgument("use_sim_time", default_value='true'),
gazebo,
controller,
ekf_localization_filter,
twist_mux,
# nav2_launch,
slam_toolbox
])the odom frame and my robot's base_link tree does these little jumps (using teleop twist keyboard and nav2 plugins for autonomous movement both with twist_mux):
Screencast.from.08-17-2025.08.11.24.PM.webm
However if I only initialize the ekf, my controller, and the gazebo simulation node it seems to work fine:
import os
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
def generate_launch_description():
use_sim_time = LaunchConfiguration("use_sim_time")
gazebo = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_description"),
"launch",
"gazebo_V1.launch.py"
),
launch_arguments={
"use_sim_time": use_sim_time
}.items()
)
controller = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_controller"),
"launch",
"controller_V1.launch.py"
),
launch_arguments={"use_simple_controller": "False",
"use_sim_time": use_sim_time,
"use_python": "False"}.items()
)
ekf_localization_filter = IncludeLaunchDescription(
os.path.join(
get_package_share_directory("skid_steer_localization"),
"launch",
"local_localization.launch.py"
),
launch_arguments={"use_sim_time": use_sim_time}.items()
)
# SLAM Toolbox
slam_toolbox = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[
os.path.join(
get_package_share_directory('skid_steer_slam'),
'config',
'mapper_params_online_async.yaml'
),
{"use_sim_time": use_sim_time}
],
remappings=[
('/scan', '/ldlidar_node/scan')
]
)
nav2_launch = IncludeLaunchDescription(
os.path.join(
get_package_share_directory('skid_steer_navigation'),
'launch',
'nav2_bringup.launch.py'
),
)
# Twist Mux
twist_mux_config = os.path.join(
get_package_share_directory('skid_steer_controller'),
'config',
'twist_mux.yaml'
)
twist_mux = Node(
package='twist_mux',
executable='twist_mux',
name='twist_mux',
parameters=[twist_mux_config, {"use_sim_time": use_sim_time}],
remappings=[
('/cmd_vel_out', '/skid_steer_controller_V1/cmd_vel_unstamped')
],
output='screen'
)
return LaunchDescription([
DeclareLaunchArgument("use_sim_time", default_value='true'),
gazebo,
controller,
ekf_localization_filter,
twist_mux,
# nav2_launch,
# slam_toolbox
])Screencast.from.08-17-2025.08.58.43.PM.webm
So my question is if this behavior is expected or am I doing something wrong. I must say that the real-life implementation of this robot works actually fine so Im confusing if these jumps could affect any behavior for other packages and how to solve it.
This is my system:
- ROS2 Humble desktop (Deb packages install)
- Ubuntu 22
- Using gazebo Ignition
- Tf tree:
- Slam toolbox launch file:
import os
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
use_sim_time = LaunchConfiguration("use_sim_time")
slam_config = LaunchConfiguration("slam_config")
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="true"
)
slam_config_arg = DeclareLaunchArgument(
"slam_config",
default_value=os.path.join(
get_package_share_directory("skid_steer_slam"),
"config",
"mapper_params_online_async.yaml"
),
description="Full path to slam yaml file to load"
)
slam_toolbox = Node(
package="slam_toolbox",
executable="async_slam_toolbox_node",
name="slam_toolbox",
output="screen",
parameters=[
slam_config,
{"use_sim_time": use_sim_time},
],
)
return LaunchDescription([
use_sim_time_arg,
slam_config_arg,
slam_toolbox,
])- Slam toolbox params file:
slam_toolbox:
ros__parameters:
use_sim_time: true
# 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_footprint
scan_topic: /scan
use_map_saver: true
mode: localization #localization mapping
# 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: /home/chocho3/skid_steer_ws/src/skid_steer_slam/maps/map_explanada
#map_file_name: /home/mantecada01/skid_steer_ws/src/skid_steer_slam/maps/mapa_explanada_v1
# map_file_name: /home/mantecada01/skid_steer_ws/src/skid_steer_slam/maps/map_explanada
map_file_name: /home/mau/TT_map_1
#map_start_pose: [0.0, 15.0, 0.0]
map_start_at_dock: true
debug_logging: true
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
min_laser_range: 0.05 #for rastering images
max_laser_range: 25.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
position_covariance_scale: 0.01
yaw_covariance_scale: 0.01
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.01
minimum_travel_heading: 0.01
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: 7.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: 10.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
min_pass_through: 2
occupancy_threshold: 0.1