Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Controller] null ptr during mapping and navigating possibly caused by bugs in costmap_queue #3234

Closed
Cryst4L9527 opened this issue Oct 6, 2022 · 0 comments
Closed

[Controller] null ptr during mapping and navigating possibly caused by bugs in costmap_queue #3234

Cryst4L9527 opened this issue Oct 6, 2022 · 0 comments

Comments

@Cryst4L9527
Copy link
Contributor

Bug report

Required Info:

  • Operating System:
    • Ubuntu 20.04
  • ROS2 Version:
    • foxy
  • Version or commit hash:
  • DDS implementation:
    • Fast-RTPS（default）

Steps to reproduce issue

just like the issue #3231 ,except for the configall.yaml:

/loc2d_ros:
  ros__parameters:
    base_frame_id: base_footprint
    d_thresh: 0.02
    global_frame_id: map
    initial_pos_a: 0.0
    initial_pos_x: -2.0
    initial_pos_y: -0.5
    odom_frame_id: odom
    scan_topic: /scan
    transform_tolerance: 0.2
    use_sim_time: true
amcl:
  ros__parameters:
    alpha1: 10.4
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 2.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
    initial_pose:
      x: -2.0
      y: -0.5
      yaw: 0.0
      z: 0.0
    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: 2127
    min_particles: 499
    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: differential
    save_pose_rate: 0.5
    set_initial_pose: true
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    use_sim_time: true
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
amcl_map_client:
  ros__parameters:
    use_sim_time: true
amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: true
bt_navigator:
  ros__parameters:
    default_bt_xml_filename: /home/shx/ros2_nav_fuzz/src/fuzz/scripts/config/navigate_w_replanning_and_recovery.xml
    global_frame: map
    odom_topic: /odom
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_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_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_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_distance_traveled_condition_bt_node
    robot_base_frame: base_link
    use_sim_time: true
bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: true
cartographer_node:
  ros__parameters:
    use_sim_time: true
controller_server:
  ros__parameters:
    FollowPath:
      BaseObstacle.scale: 0.02
      GoalAlign.forward_point_distance: 3.3000000000000003
      GoalAlign.scale: 24.5
      GoalDist.scale: 24.0
      PathAlign.forward_point_distance: 0.1
      PathAlign.scale: 32.0
      PathDist.scale: 32.0
      RotateToGoal.lookahead_time: -1.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 3.8
      acc_lim_theta: 3.1
      acc_lim_x: 2.4
      acc_lim_y: -0.1
      angular_granularity: 0.025
      critics:
      - RotateToGoal
      - Oscillation
      - BaseObstacle
      - GoalAlign
      - PathAlign
      - PathDist
      - GoalDist
      debug_trajectory_details: true
      decel_lim_theta: -3.2
      decel_lim_x: -4.0
      decel_lim_y: 0.0
      linear_granularity: 0.05
      max_speed_xy: 0.22
      max_vel_theta: 1.0
      max_vel_x: 0.22
      max_vel_y: 0.0
      min_speed_theta: 0.0
      min_speed_xy: 0.0
      min_vel_x: 0.0
      min_vel_y: 0.0
      plugin: dwb_core::DWBLocalPlanner
      short_circuit_trajectory_evaluation: true
      sim_time: 1.7
      stateful: true
      trans_stopped_velocity: 0.25
      transform_tolerance: 0.2
      vtheta_samples: 20
      vx_samples: 19
      vy_samples: 5
      xy_goal_tolerance: -1.4500000000000002
    controller_frequency: 19.9
    controller_plugins:
    - FollowPath
    min_theta_velocity_threshold: 0.001
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: -9.0
    use_sim_time: true
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: true
ekf_localization:
  ros__parameters:
    gnss_pose_topic: gnss_pose
    imu_topic: imu
    odom_topic: odom
    pub_period: 18
    reference_frame_id: map
    robot_frame_id: base_link
    use_gnss: false
    use_odom: true
    var_gnss: -11.100000000000001
    var_imu_acc: 11.81
    var_imu_w: -2.5900000000000003
    var_odom: -10.8
global_costmap:
  global_costmap:
    ros__parameters:
      always_send_full_costmap: true
      global_frame: map
      inflation_layer:
        cost_scaling_factor: -3.7
        inflation_radius: 12.05
        plugin: nav2_costmap_2d::InflationLayer
      obstacle_layer:
        enabled: true
        observation_sources: scan
        plugin: nav2_costmap_2d::ObstacleLayer
        scan:
          clearing: true
          data_type: LaserScan
          marking: true
          max_obstacle_height: 0.5
          topic: /scan
      plugins:
      - static_layer
      - obstacle_layer
      - voxel_layer
      - inflation_layer
      publish_frequency: 5.2
      resolution: 3.55
      robot_base_frame: base_link
      robot_radius: -9.4
      static_layer:
        map_subscribe_transient_local: true
        plugin: nav2_costmap_2d::StaticLayer
      update_frequency: -1.4000000000000004
      use_sim_time: true
      voxel_layer:
        enabled: true
        mark_threshold: -61
        max_obstacle_height: 14.600000000000001
        observation_sources: pointcloud
        origin_z: -4.0
        plugin: nav2_costmap_2d::VoxelLayer
        pointcloud:
          clearing: true
          data_type: PointCloud2
          marking: true
          max_obstacle_height: -7.600000000000001
          topic: /intel_realsense_r200_depth/points
        publish_voxel_map: true
        z_resolution: 7.75
        z_voxels: 104
  global_costmap_client:
    ros__parameters:
      use_sim_time: true
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: true
lifecycle_manager_localization:
  ros__parameters:
    autostart: true
    node_names:
    - map_server
    - amcl
    use_sim_time: true
lifecycle_manager_mapserver:
  ros__parameters:
    autostart: true
    node_names:
    - map_server
    use_sim_time: true
lifecycle_manager_navigation:
  ros__parameters:
    autostart: true
    node_names:
    - controller_server
    - planner_server
    - recoveries_server
    - bt_navigator
    - waypoint_follower
    use_sim_time: true
local_costmap:
  local_costmap:
    ros__parameters:
      always_send_full_costmap: true
      global_frame: odom
      height: -90
      inflation_layer:
        cost_scaling_factor: -10.0
        plugin: nav2_costmap_2d::InflationLayer
      obstacle_layer:
        enabled: true
        observation_sources: scan
        plugin: nav2_costmap_2d::ObstacleLayer
        scan:
          clearing: true
          data_type: LaserScan
          marking: true
          max_obstacle_height: 10.4
          topic: /scan
      plugins:
      - obstacle_layer
      - voxel_layer
      - inflation_layer
      publish_frequency: 2.6
      resolution: 7.3500000000000005
      robot_base_frame: base_link
      robot_radius: -10.9
      rolling_window: true
      static_layer:
        map_subscribe_transient_local: false
      update_frequency: 13.0
      use_sim_time: true
      voxel_layer:
        enabled: true
        mark_threshold: -62
        max_obstacle_height: -10.200000000000001
        observation_sources: pointcloud
        origin_z: 6.300000000000001
        plugin: nav2_costmap_2d::VoxelLayer
        pointcloud:
          clearing: true
          data_type: PointCloud2
          marking: true
          max_obstacle_height: 13.3
          topic: /intel_realsense_r200_depth/points
        publish_voxel_map: true
        z_resolution: 7.45
        z_voxels: 12
      width: -12
  local_costmap_client:
    ros__parameters:
      use_sim_time: true
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: true
map_saver:
  ros__parameters:
    free_thresh_default: -0.65
    occupied_thresh_default: 4.750000000000001
    save_map_timeout: 5119
    use_sim_time: true
map_server:
  ros__parameters:
    use_sim_time: true
    yaml_filename: /home/shx/ros2_nav_fuzz/src/fuzz/scripts/config/turtlebot3_world.yaml
occupancy_grid_node:
  ros__parameters:
    use_sim_time: true
planner_server:
  ros__parameters:
    GridBased:
      allow_unknown: true
      plugin: nav2_navfn_planner/NavfnPlanner
      tolerance: 11.4
      use_astar: false
    expected_planner_frequency: 9.899999999999999
    planner_plugins:
    - GridBased
    use_sim_time: true
planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: true
recoveries_server:
  ros__parameters:
    backup:
      plugin: nav2_recoveries/BackUp
    costmap_topic: local_costmap/costmap_raw
    cycle_frequency: 18.9
    footprint_topic: local_costmap/published_footprint
    global_frame: odom
    max_rotational_vel: -2.7
    min_rotational_vel: -11.200000000000001
    recovery_plugins:
    - spin
    - backup
    - wait
    robot_base_frame: base_link
    rotational_acc_lim: -3.9000000000000004
    simulate_ahead_time: 6.5
    spin:
      plugin: nav2_recoveries/Spin
    transform_timeout: 4.0
    use_sim_time: true
    wait:
      plugin: nav2_recoveries/Wait
robot_state_publisher:
  ros__parameters:
    use_sim_time: true
rtabmap:
  ros__parameters:
    RGBD/NeighborLinkRefining: 'True'
    Reg/Strategy: '1'
    approx_sync: true
    frame_id: base_footprint
    subscribe_depth: false
    subscribe_rgb: false
    subscribe_scan: true
    use_sim_time: true
rtabmap_camera:
  ros__parameters:
    frame_id: base_footprint
    subscribe_depth: true
    use_sim_time: true
slam_toolbox:
  ros__parameters:
    angle_variance_penalty: -5.6000000000000005
    base_frame: base_footprint
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_loss_function: None
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    coarse_angle_resolution: -5.6651
    coarse_search_angle_offset: -4.9510000000000005
    correlation_search_space_dimension: -2.6
    correlation_search_space_resolution: 2.01
    correlation_search_space_smear_deviation: 1.4000000000000001
    debug_logging: false
    distance_variance_penalty: 13.0
    do_loop_closing: true
    enable_interactive_mode: true
    fine_search_angle_offset: -7.39651
    link_match_minimum_response_fine: 6.1
    link_scan_maximum_distance: -10.0
    loop_match_maximum_variance_coarse: -9.700000000000001
    loop_match_minimum_chain_size: 121
    loop_match_minimum_response_coarse: 8.95
    loop_match_minimum_response_fine: 1.25
    loop_search_maximum_distance: -7.100000000000001
    loop_search_space_dimension: 19.3
    loop_search_space_resolution: -1.55
    loop_search_space_smear_deviation: 2.63
    map_frame: map
    map_update_interval: -3.9000000000000004
    max_laser_range: 31.200000000000003
    minimum_angle_penalty: -4.8
    minimum_distance_penalty: 6.2
    minimum_time_interval: 10.4
    minimum_travel_distance: 5.6000000000000005
    minimum_travel_heading: 0.0
    mode: mapping
    odom_frame: odom
    resolution: 10.55
    scan_buffer_maximum_scan_distance: -0.8000000000000007
    scan_buffer_size: -104
    scan_topic: /scan
    solver_plugin: solver_plugins::CeresSolver
    stack_size_to_use: 40000021
    tf_buffer_duration: 42.6
    throttle_scans: -42
    transform_publish_period: -10.180000000000001
    transform_timeout: -4.0
    use_response_expansion: true
    use_scan_barycenter: true
    use_scan_matching: true
    use_sim_time: true

Expected behavior

Process should not crash.

Actual behavior

the program crashed with the Asan information below:

AddressSanitizer:DEADLYSIGNAL
=================================================================
==615363==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000000 (pc 0x7f6cfa3ab3f6 bp 0x7f6cece89930 sp 0x7f6cece89900 T14)
==615363==The signal is caused by a READ memory access.
==615363==Hint: address points to the zero page.
    #0 0x7f6cfa3ab3f6 in std::vector<double, std::allocator<double> >::operator[](unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:1043:25
    #1 0x7f6ced79d86d in costmap_queue::CostmapQueue::distanceLookup(unsigned int, unsigned int, unsigned int, unsigned int) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp:182:12
    #2 0x7f6ced79b2c4 in costmap_queue::CostmapQueue::enqueueCell(unsigned int, unsigned int, unsigned int, unsigned int, unsigned int) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/costmap_queue/src/costmap_queue.cpp:76:21
    #3 0x7f6ced79aff4 in costmap_queue::CostmapQueue::enqueueCell(unsigned int, unsigned int) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/costmap_queue/src/costmap_queue.cpp:65:3
    #4 0x7f6ced76a240 in dwb_critics::GoalDistCritic::prepare(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&, nav_2d_msgs::msg::Twist2D_<std::allocator<void> > const&, geometry_msgs::msg::Pose2D_<std::allocator<void> > const&, nav_2d_msgs::msg::Path2D_<std::allocator<void> > const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp:58:11
    #5 0x7f6ced7769b3 in dwb_critics::GoalAlignCritic::prepare(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&, nav_2d_msgs::msg::Twist2D_<std::allocator<void> > const&, geometry_msgs::msg::Pose2D_<std::allocator<void> > const&, nav_2d_msgs::msg::Path2D_<std::allocator<void> > const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/dwb_critics/src/goal_align.cpp:70:26
    #6 0x7f6cedc24be5 in dwb_core::DWBLocalPlanner::computeVelocityCommands(nav_2d_msgs::msg::Pose2DStamped_<std::allocator<void> > const&, nav_2d_msgs::msg::Twist2D_<std::allocator<void> > const&, std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation_<std::allocator<void> > >&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp:336:17
    #7 0x7f6cedc233d5 in dwb_core::DWBLocalPlanner::computeVelocityCommands(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, geometry_msgs::msg::Twist_<std::allocator<void> > const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp:286:50
    #8 0x5fa246 in nav2_controller::ControllerServer::computeAndPublishVelocity() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_controller/src/nav2_controller.cpp:363:40
    #9 0x5f09b9 in nav2_controller::ControllerServer::computeControl() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_controller/src/nav2_controller.cpp:296:7
    #10 0x813c50 in void std::__invoke_impl<void, void (nav2_controller::ControllerServer::*&)(), nav2_controller::ControllerServer*&>(std::__invoke_memfun_deref, void (nav2_controller::ControllerServer::*&)(), nav2_controller::ControllerServer*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #11 0x813a7d in std::__invoke_result<void (nav2_controller::ControllerServer::*&)(), nav2_controller::ControllerServer*&>::type std::__invoke<void (nav2_controller::ControllerServer::*&)(), nav2_controller::ControllerServer*&>(void (nav2_controller::ControllerServer::*&)(), nav2_controller::ControllerServer*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #12 0x8139c5 in void std::_Bind<void (nav2_controller::ControllerServer::* (nav2_controller::ControllerServer*))()>::__call<void, 0ul>(std::tuple<>&&, std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #13 0x813813 in void std::_Bind<void (nav2_controller::ControllerServer::* (nav2_controller::ControllerServer*))()>::operator()<void>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #14 0x813440 in std::_Function_handler<void (), std::_Bind<void (nav2_controller::ControllerServer::* (nav2_controller::ControllerServer*))()> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #15 0x847eb8 in std::function<void ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #16 0x8464a2 in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::work() /home/r1/ros2_nav_fuzz/install/nav2_util/include/nav2_util/simple_action_server.hpp:144:9
    #17 0x845da0 in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()::operator()() const /home/r1/ros2_nav_fuzz/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:68
    #18 0x845d40 in nav2_msgs::action::FollowPath std::__invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(std::__invoke_other, rclcpp::Node&&, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #19 0x845c90 in std::__invoke_result<nav2_msgs::action::FollowPath, rclcpp::Node...>::type std::__invoke<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(nav2_msgs::action::FollowPath&&, rclcpp::Node&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #20 0x845c58 in void std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #21 0x8459c8 in std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #22 0x8456cf in std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1362:6
    #23 0x84532e in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:285:9
    #24 0x844b9f in std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #25 0x844474 in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:561:27
    #26 0x844ae3 in void std::__invoke_impl<void, void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::__invoke_memfun_deref, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #27 0x844847 in std::__invoke_result<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>::type std::__invoke<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #28 0x8447b8 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:671:4
    #29 0x844696 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:25
    #30 0x844612 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::__invoke() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:21
    #31 0x7f6cf8d6647e in __pthread_once_slow (/lib/x86_64-linux-gnu/libpthread.so.0+0x1247e)
    #32 0x5fc8c0 in __gthread_once(int*, void (*)()) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/x86_64-linux-gnu/c++/9/bits/gthr-default.h:700:12
    #33 0x8442ac in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:683:17
    #34 0x842fcc in std::__future_base::_State_baseV2::_M_set_result(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>, bool) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:401:2
    #35 0x842984 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1662:3
    #36 0x8426a0 in nav2_msgs::action::FollowPath std::__invoke_impl<void, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()>(std::__invoke_other, rclcpp::Node&&, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #37 0x8425f0 in std::__invoke_result<nav2_msgs::action::FollowPath, rclcpp::Node...>::type std::__invoke<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()>(nav2_msgs::action::FollowPath&&, rclcpp::Node&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #38 0x8425b8 in void std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #39 0x842578 in std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #40 0x842392 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&)::'lambda'()> > >::_M_run() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:195:13
    #41 0x7f6cf87f2de3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #42 0x7f6cf8d5d608 in start_thread (/lib/x86_64-linux-gnu/libpthread.so.0+0x9608)
    #43 0x7f6cf84d7292 in clone (/lib/x86_64-linux-gnu/libc.so.6+0x122292)

AddressSanitizer can not provide additional info.
SUMMARY: AddressSanitizer: SEGV /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:1043:25 in std::vector<double, std::allocator<double> >::operator[](unsigned long)
Thread T14 created by T9 here:
    #0 0x59607a in pthread_create (/home/r1/ros2_nav_fuzz/build/nav2_controller/controller_server+0x59607a)
    #1 0x7f6cf87f30a8 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd70a8)
    #2 0x840113 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1659:14
    #3 0x83feb4 in void __gnu_cxx::new_allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #4 0x83fa20 in void std::allocator_traits<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> > >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #5 0x83f3a6 in std::_Sp_counted_ptr_inplace<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #6 0x83edf6 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(nav2_msgs::action::FollowPath*&, std::_Sp_alloc_shared_tag<rclcpp::Node>, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #7 0x83eac7 in std::__shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #8 0x83e88c in std::shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >::shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #9 0x83e5e8 in std::shared_ptr<nav2_msgs::action::FollowPath> std::allocate_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(rclcpp::Node const&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #10 0x83e2b3 in std::shared_ptr<nav2_msgs::action::FollowPath> std::make_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(rclcpp::Node&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #11 0x83dba0 in std::shared_ptr<std::__future_base::_State_baseV2> std::__future_base::_S_make_async_state<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> > >(nav2_msgs::action::FollowPath&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1700:14
    #12 0x838b5f in std::future<std::__invoke_result<std::decay<nav2_msgs::action::FollowPath>::type, std::decay<rclcpp::Node>::type...>::type> std::async<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(std::launch, nav2_msgs::action::FollowPath&&, rclcpp::Node&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1714:18
    #13 0x817bdc in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >) /home/r1/ros2_nav_fuzz/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:27
    #14 0x84f62e in void std::__invoke_impl<void, void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> > >(std::__invoke_memfun_deref, void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #15 0x84f3da in std::__invoke_result<void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> > >::type std::__invoke<void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> > >(void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #16 0x84f307 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::* (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)>::__call<void, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #17 0x84f112 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::* (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)>::operator()<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >, void>(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #18 0x84ec3d in std::_Function_handler<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>::* (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath, rclcpp::Node>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #19 0x824b35 in std::function<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)>::operator()(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #20 0x81b7d6 in rclcpp_action::Server<nav2_msgs::action::FollowPath>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_t>, std::array<unsigned char, 16ul>, std::shared_ptr<void>) /opt/ros/foxy/include/rclcpp_action/server.hpp:429:5
    #21 0x7f6cf90abb2c in rclcpp_action::ServerBase::execute_goal_request_received() (/opt/ros/foxy/lib/librclcpp_action.so+0x15b2c)

Thread T9 created by T0 here:
    #0 0x59607a in pthread_create (/home/r1/ros2_nav_fuzz/build/nav2_controller/controller_server+0x59607a)
    #1 0x7f6cf87f30a8 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd70a8)
    #2 0x7f6cf94d0683 in std::_MakeUniq<std::thread>::__single_object std::make_unique<std::thread, nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0>(nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/unique_ptr.h:857:34
    #3 0x7f6cf94d040d in nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_util/src/node_thread.cpp:25:13
    #4 0x7f6cf94c676c in nav2_util::NodeThread::NodeThread<std::shared_ptr<rclcpp::Node> >(std::shared_ptr<rclcpp::Node>) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_util/include/nav2_util/node_thread.hpp:32:5
    #5 0x7f6cf94c3719 in std::_MakeUniq<nav2_util::NodeThread>::__single_object std::make_unique<nav2_util::NodeThread, std::shared_ptr<rclcpp::Node>&>(std::shared_ptr<rclcpp::Node>&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/unique_ptr.h:857:34
    #6 0x7f6cf94c19e9 in nav2_util::LifecycleNode::LifecycleNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, rclcpp::NodeOptions const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_util/src/lifecycle_node.cpp:58:22
    #7 0x5e19f6 in nav2_controller::ControllerServer::ControllerServer() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_controller/src/nav2_controller.cpp:34:3
    #8 0x5e0137 in void __gnu_cxx::new_allocator<nav2_controller::ControllerServer>::construct<nav2_controller::ControllerServer>(nav2_controller::ControllerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #9 0x5dfbae in void std::allocator_traits<std::allocator<nav2_controller::ControllerServer> >::construct<nav2_controller::ControllerServer>(std::allocator<nav2_controller::ControllerServer>&, nav2_controller::ControllerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #10 0x5df433 in std::_Sp_counted_ptr_inplace<nav2_controller::ControllerServer, std::allocator<nav2_controller::ControllerServer>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<>(std::allocator<nav2_controller::ControllerServer>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #11 0x5ded8b in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<nav2_controller::ControllerServer, std::allocator<nav2_controller::ControllerServer> >(nav2_controller::ControllerServer*&, std::_Sp_alloc_shared_tag<std::allocator<nav2_controller::ControllerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #12 0x5dea6c in std::__shared_ptr<nav2_controller::ControllerServer, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<nav2_controller::ControllerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_controller::ControllerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #13 0x5de85f in std::shared_ptr<nav2_controller::ControllerServer>::shared_ptr<std::allocator<nav2_controller::ControllerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_controller::ControllerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #14 0x5de619 in std::shared_ptr<nav2_controller::ControllerServer> std::allocate_shared<nav2_controller::ControllerServer, std::allocator<nav2_controller::ControllerServer> >(std::allocator<nav2_controller::ControllerServer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #15 0x5dde5b in std::shared_ptr<nav2_controller::ControllerServer> std::make_shared<nav2_controller::ControllerServer>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #16 0x5dd5a3 in main /home/r1/ros2_nav_fuzz/src/navigation2/nav2_controller/src/main.cpp:23:15
    #17 0x7f6cf83dc0b2 in __libc_start_main (/lib/x86_64-linux-gnu/libc.so.6+0x270b2)

==615363==ABORTING

I'll also explore the root cause of this, just report the event first.
@Cryst4L9527 Cryst4L9527 changed the title [Controller] invalid ptr during mapping and navigating possibly caused by bugs in costmap_queue [Controller] null ptr during mapping and navigating possibly caused by bugs in costmap_queue Oct 6, 2022
@Cryst4L9527 Cryst4L9527 mentioned this issue Oct 12, 2022
Merged
7 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@SteveMacenski @Cryst4L9527