-
Notifications
You must be signed in to change notification settings - Fork 1.6k
Closed
Description
- Operating System:
- Ubuntu 24.04
- Computer:
- Raspberry Pi 5 8 gb
- ROS2 Version:
- Jazzy installed via deb packages
- Version or commit hash:
- ros-jazzy-navigation2 1.3.7-1noble.20250624.214056
- ros-jazzy-nav2-amcl 1.3.7-1noble.20250624.150636
- DDS implementation:
- Eclipse Zenoh
Steps to reproduce issue
I cannot reliably reproduce the crash; it happens intermittently during startup of the navigation stack with AMCL.
Typical steps that sometimes trigger it:
- Launch the full navigation stack:
ros2 launch nav_pkg nav.launch.py
Expected behavior
AMCL should start and configure correctly
Actual behavior
Occasionally, AMCL crashes during startup with a stack smashing detected error at initTransforms. The crash happens intermittently and is not reliably reproducible.
[map_server-1] [INFO] [1755174650.889985773] [map_io]: Read map /home/ubuntu/ros2_ws/src/nav_hub/maps/mapa360.pgm: 782 X 1167 map @ 0.036 m/cell
[lifecycle_manager-10] [INFO] [1755174650.901656774] [lifecycle_manager_navigation]: Configuring amcl
[amcl-3] [INFO] [1755174650.902333369] [amcl]: Configuring
[amcl-3] [INFO] [1755174650.902474739] [amcl]: initTransforms
[amcl-3] *** stack smashing detected ***: terminated
Error on backtrace:
#0 __pthread_kill_implementation (threadid=281474840551456, signo=signo@entry=6, no_tid=no_tid@entry=0) at ./nptl/pthread_kill.c:44
#1 0x0000fffff76f7670 in __pthread_kill_internal (signo=6, threadid=<optimized out>) at ./nptl/pthread_kill.c:78
#2 0x0000fffff76acb3c in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#3 0x0000fffff7697e00 in __GI_abort () at ./stdlib/abort.c:79
#4 0x0000fffff76eabf4 in __libc_message_impl (fmt=fmt@entry=0xfffff77d2b18 "*** %s ***: terminated\n") at ../sysdeps/posix/libc_fatal.c:134
#5 0x0000fffff77681a8 in __GI___fortify_fail (msg=msg@entry=0xfffff77d2b30 "stack smashing detected") at ./debug/fortify_fail.c:24
#6 0x0000fffff7769164 in __stack_chk_fail () at ./debug/stack_chk_fail.c:24
#7 0x0000fffff6f7338c in eigen_decomposition () from /opt/ros/jazzy/lib/libpf_lib.so
#8 0x0000fffff6f73410 in pf_matrix_unitary () from /opt/ros/jazzy/lib/libpf_lib.so
#9 0x0000fffff6f7353c in pf_pdf_gaussian_alloc () from /opt/ros/jazzy/lib/libpf_lib.so
#10 0x0000fffff6f73668 in pf_init () from /opt/ros/jazzy/lib/libpf_lib.so
#11 0x0000fffff7eddb44 in nav2_amcl::AmclNode::initParticleFilter() () from /opt/ros/jazzy/lib/libamcl_core.so
#12 0x0000fffff7ece6fc in nav2_amcl::AmclNode::on_configure(rclcpp_lifecycle::State const&) () from /opt/ros/jazzy/lib/libamcl_core.so
Reproduction instructions
Cannot reliably reproduce. Typical usage that sometimes triggers the crash:
ros2 launch nav_pkg nav.launch.py
Wait for AMCL to configure and initialize. The crash may appear in the console and GDB backtrace shows the failure in eigen_decomposition() inside libpf_lib.so.
Additional information
- The crash occurs intermittently: approximately 2 out of every 5 launches trigger it. However, it is not consistent, and sometimes it does not occur at all.
- I'm using a LDROBOT STL27L LiDAR publishing ~2170 points per scan with some NaN values in ranges and intensity
- I tried tweaking some AMCL parameters: reducing max_beams, max_particles, and min_particles, but the crash persisted. I also considered that the issue might be related to the initial pose, so I tried setting set_initial_pose and always_reset_initial_pose to true, but that did not help.
- I also suspected the map could be related, and tried creating a new one and testing maps known to work fine on Humble, but the problem persisted.
- htop memory and CPU usage at the moment of the crash: Watch Here
AMCL parameters
amcl:
ros__parameters:
use_sim_time: false
alpha1: 0.35
alpha2: 0.3
alpha3: 0.2
alpha4: 0.2
# alpha5: 0.2
base_frame_id: "base_footprint"
introspection_mode: "disabled"
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: 15.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 80
max_particles: 2000
min_particles: 800
odom_frame_id: "odom"
pf_err: 0.01
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: -1.0
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.15
update_min_d: 0.20
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.005
scan_topic: scan
map_topic: map
set_initial_pose: false
always_reset_initial_pose: false
first_map_only: false
initial_pose:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0Entire nav params, if relevant:
Nav params
bt_navigator:
ros__parameters:
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
use_sim_time: false
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 2000
action_server_result_timeout: 900.0
navigators: ["navigate_to_pose"]
navigate_to_pose:
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
default_nav_to_pose_bt_xml: "/home/ubuntu/ros2_ws/src/nav_hub/btree/custom_tree.xml"
# '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 is used to add custom BT plugins to the executor (vector of strings).
# Built-in plugins are added automatically
# plugin_lib_names: []
error_code_names:
- compute_path_error_code
- follow_path_error_code
map_server:
ros__parameters:
frame_id: map
topic_name: map
use_sim_time: false
yaml_filename: "/home/ubuntu/ros2_ws/src/nav_hub/maps/mapa360.yaml"
controller_server:
ros__parameters:
controller_frequency: 5.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
goal_checker_plugins: ["goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.35
movement_time_allowance: 10.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: True
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 15
model_dt: 0.2
batch_size: 5000
vx_std: 0.2
vy_std: 0.0
wz_std: 0.4
vx_max: 0.35
vx_min: -0.35
vy_max: 0.0
wz_max: 1.5
ax_max: 3.0
ax_min: -3.0
ay_min: 0.0
ay_max: 0.0
az_max: 3.5
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: false
# reset_period: 1.0 # (only in Humble)
regenerate_noises: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
TrajectoryValidator:
plugin: "mppi::DefaultOptimalTrajectoryValidator"
collision_lookahead_time: 2.0
consider_footprint: false
# AckermannConstraints:
# min_turning_r: 0.2
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 1.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 1.4
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
mode: 0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: false
rolling_window: true
track_unknown_space: true
lethal_cost_threshold: 100
width: 5
height: 5
resolution: 0.036
footprint: "[[0.16, -0.25], [0.16, 0.25], [-0.40, 0.25], [-0.40, -0.25]]"
# robot_radius: 0.35
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 2.58
inflation_radius: 1.75
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
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_footprint
use_sim_time: false
# robot_radius: 0.35
footprint: "[[0.16, -0.25], [0.16, 0.25], [-0.40, 0.25], [-0.40, -0.25]]"
resolution: 0.036
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
track_unknown_space: true
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:
enabled: True
plugin: "nav2_costmap_2d::StaticLayer"
track_unknown_space: true
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 2.58
inflation_radius: 1.75
enabled: True
always_send_full_costmap: True
amcl:
ros__parameters:
use_sim_time: false
alpha1: 0.35
alpha2: 0.3
alpha3: 0.2
alpha4: 0.2
# alpha5: 0.2
base_frame_id: "base_footprint"
introspection_mode: "disabled"
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: 15.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 80
max_particles: 2000
min_particles: 800
odom_frame_id: "odom"
pf_err: 0.01
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: -1.0
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.15
update_min_d: 0.20
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.005
scan_topic: scan
map_topic: map
set_initial_pose: false
always_reset_initial_pose: false
first_map_only: false
initial_pose:
x: 0.0
y: 0.0
z: 0.0
yaw: 0.0
planner_server:
ros__parameters:
use_sim_time: false
expected_planner_frequency: 20.0
planner_plugins: ["GridBased"]
costmap_update_timeout: 0.5
GridBased:
plugin: "nav2_navfn_planner::NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: false
unknown_cost_value: 255
smoother_server:
ros__parameters:
use_sim_time: false
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
local_costmap_topic: local_costmap/costmap_raw
global_costmap_topic: global_costmap/costmap_raw
local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_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"
local_frame: odom
global_frame: map
robot_base_frame: base_footprint
transform_tolerance: 0.1
simulate_ahead_time: 2.0
max_rotational_vel: 0.3
min_rotational_vel: 0.1
rotational_acc_lim: 3.2
enable_stamped_cmd_vel: true
robot_state_publisher:
ros__parameters:
use_sim_time: false
waypoint_follower:
ros__parameters:
use_sim_time: false
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:
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.5, 0.0, 2.0]
min_velocity: [-0.5, 0.0, -2.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
enable_stamped_cmd_vel: trueLaunch being used, if relevant:
Navigation Launch
import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.conditions import IfCondition
from nav2_common.launch import RewrittenYaml
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode, ParameterFile
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav_hub')
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
use_composition = LaunchConfiguration('use_composition')
container_name = LaunchConfiguration('container_name')
container_name_full = (namespace, '/', container_name)
use_respawn = LaunchConfiguration('use_respawn')
log_level = LaunchConfiguration('log_level')
lifecycle_nodes = ['map_server',
'amcl',
'controller_server',
'smoother_server',
'planner_server',
'behavior_server',
'bt_navigator',
'waypoint_follower'
#'velocity_smoother'
]
remappings = []
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'autostart': autostart}
configured_params = ParameterFile(
RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True),
allow_substs=True)
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'config', 'nav_dinamico.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
declare_container_name_cmd = DeclareLaunchArgument(
'container_name', default_value='nav2_container',
description='the name of conatiner that nodes will load in if use composition')
declare_use_respawn_cmd = DeclareLaunchArgument(
'use_respawn', default_value='False',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
declare_log_level_cmd = DeclareLaunchArgument(
'log_level', default_value='info',
description='log level')
load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[
Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[configured_params],
),
Node(
package='nav2_controller',
executable='controller_server',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
),
Node(
package='nav2_amcl',
executable='amcl',
name='amcl',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
# arguments=['--ros-args', '--log-level', log_level],
# prefix=['xterm -e gdb -ex run --args'],
# arguments=['--ros-args', '--log-level', 'debug']
),
Node(
package='nav2_smoother',
executable='smoother_server',
name='smoother_server',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings),
Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings),
Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
),
Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings),
Node(
package='nav2_waypoint_follower',
executable='waypoint_follower',
name='waypoint_follower',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings),
Node(
package='nav2_velocity_smoother',
executable='velocity_smoother',
name='velocity_smoother',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
arguments=['--ros-args', '--log-level', log_level],
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': lifecycle_nodes}]),
]
)
# Create the launch description and populate
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_container_name_cmd)
ld.add_action(declare_use_respawn_cmd)
ld.add_action(declare_log_level_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(load_nodes)
return ldMetadata
Metadata
Assignees
Labels
No labels