/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

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