Skip to content

[Zenoh] AMCL intermittent crash on Raspberry Pi 5 with stack smashing detected #5470

@EricoMeger

Description

@EricoMeger
  • 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:

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

Entire 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: true

Launch 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 ld

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions