[nav2_amcl] heap-buffer-overflow during mapping and navigating possibly caused by bugs in map_cspace.cpp

[Cryst4L9527]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • foxy
• Version or commit hash:
  • a7955d8
• 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: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.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: 2000
    min_particles: 500
    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: 0.1
      GoalAlign.scale: 24.0
      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: 5.0
      acc_lim_theta: 3.2
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      angular_granularity: 0.025
      critics:
      - RotateToGoal
      - Oscillation
      - BaseObstacle
      - GoalAlign
      - PathAlign
      - PathDist
      - GoalDist
      debug_trajectory_details: true
      decel_lim_theta: -3.2
      decel_lim_x: -2.5
      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: 20
      vy_samples: 5
      xy_goal_tolerance: 0.25
    controller_frequency: 20.0
    controller_plugins:
    - FollowPath
    min_theta_velocity_threshold: 0.001
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    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: 10
    reference_frame_id: map
    robot_frame_id: base_link
    use_gnss: false
    use_odom: true
    var_gnss: 0.1
    var_imu_acc: 0.01
    var_imu_w: 0.01
    var_odom: 0.1
global_costmap:
  global_costmap:
    ros__parameters:
      always_send_full_costmap: true
      global_frame: map
      inflation_layer:
        cost_scaling_factor: 1.0
        inflation_radius: 0.55
        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: 2.0
          topic: /scan
      plugins:
      - static_layer
      - obstacle_layer
      - voxel_layer
      - inflation_layer
      publish_frequency: 1.0
      resolution: 0.05
      robot_base_frame: base_link
      robot_radius: 0.1
      static_layer:
        map_subscribe_transient_local: true
        plugin: nav2_costmap_2d::StaticLayer
      update_frequency: 1.0
      use_sim_time: true
      voxel_layer:
        enabled: true
        mark_threshold: 0
        max_obstacle_height: 2.0
        observation_sources: pointcloud
        origin_z: 0.0
        plugin: nav2_costmap_2d::VoxelLayer
        pointcloud:
          clearing: true
          data_type: PointCloud2
          marking: true
          max_obstacle_height: 2.0
          topic: /intel_realsense_r200_depth/points
        publish_voxel_map: true
        z_resolution: 0.05
        z_voxels: 16
  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: 3
      inflation_layer:
        cost_scaling_factor: 1.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: 2.0
          topic: /scan
      plugins:
      - obstacle_layer
      - voxel_layer
      - inflation_layer
      publish_frequency: 2.0
      resolution: 0.05
      robot_base_frame: base_link
      robot_radius: 0.1
      rolling_window: true
      static_layer:
        map_subscribe_transient_local: false
      update_frequency: 5.0
      use_sim_time: true
      voxel_layer:
        enabled: true
        mark_threshold: 0
        max_obstacle_height: 2.0
        observation_sources: pointcloud
        origin_z: 0.0
        plugin: nav2_costmap_2d::VoxelLayer
        pointcloud:
          clearing: true
          data_type: PointCloud2
          marking: true
          max_obstacle_height: 2.0
          topic: /intel_realsense_r200_depth/points
        publish_voxel_map: true
        z_resolution: 0.05
        z_voxels: 16
      width: 3
  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.25
    occupied_thresh_default: 0.65
    save_map_timeout: 5000
    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: 0.5
      use_astar: false
    expected_planner_frequency: 20.0
    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: 10.0
    footprint_topic: local_costmap/published_footprint
    global_frame: odom
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    recovery_plugins:
    - spin
    - backup
    - wait
    robot_base_frame: base_link
    rotational_acc_lim: 3.2
    simulate_ahead_time: 2.0
    spin:
      plugin: nav2_recoveries/Spin
    transform_timeout: 0.1
    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: 1.0
    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: 0.0349
    coarse_search_angle_offset: 0.349
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1
    debug_logging: false
    distance_variance_penalty: 0.5
    do_loop_closing: true
    enable_interactive_mode: true
    fine_search_angle_offset: 0.00349
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 1.5
    loop_match_maximum_variance_coarse: 3.0
    loop_match_minimum_chain_size: 10
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45
    loop_search_maximum_distance: 3.0
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03
    map_frame: map
    map_update_interval: 5.0
    max_laser_range: 20.0
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    minimum_time_interval: 0.5
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.5
    mode: mapping
    odom_frame: odom
    resolution: 0.05
    scan_buffer_maximum_scan_distance: 10.0
    scan_buffer_size: 10
    scan_topic: /scan
    solver_plugin: solver_plugins::CeresSolver
    stack_size_to_use: 40000000
    tf_buffer_duration: 30.0
    throttle_scans: 1
    transform_publish_period: 0.02
    transform_timeout: 0.2
    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:

==560519==ERROR: AddressSanitizer: heap-buffer-overflow on address 0x6020000420f8 at pc 0x7fba662a4f45 bp 0x7fba5cfbe0b0 sp 0x7fba5cfbe0a8
READ of size 8 at 0x6020000420f8 thread T10
    #0 0x7fba662a4f44 in enqueue(map_t*, int, int, int, int, std::priority_queue<CellData, std::vector<CellData, std::allocator<CellData> >, std::less<CellData> >&, CachedDistanceMap*, unsigned char*) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:102:21
    #1 0x7fba662a669f in map_update_cspace /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:155:7
    #2 0x7fba662b7525 in nav2_amcl::LikelihoodFieldModel::LikelihoodFieldModel(double, double, double, double, unsigned long, map_t*) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp:38:3
    #3 0x7fba669a5aa5 in nav2_amcl::AmclNode::createLaserObject() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:1051:14
    #4 0x7fba66998cf5 in nav2_amcl::AmclNode::addNewScanner(int&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::msg::PoseStamped_<std::allocator<void> >&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:749:21
    #5 0x7fba66995106 in nav2_amcl::AmclNode::laserReceived(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:649:10
    #6 0x7fba66b03bae in void std::__invoke_impl<void, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(std::__invoke_memfun_deref, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #7 0x7fba66b0383a in std::__invoke_result<void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>::type std::__invoke<void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #8 0x7fba66b036e7 in void std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::__call<void, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, 0ul, 1ul>(std::tuple<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #9 0x7fba66b034f2 in void std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, void>(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #10 0x7fba66b02e4d in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #11 0x7fba66b01ffd in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #12 0x7fba66b0190d in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #13 0x7fba66ac3465 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #14 0x7fba66b02524 in message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) /opt/ros/foxy/include/message_filters/signal1.h:74:5
    #15 0x7fba66a7e8a5 in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/signal1.h:117:15
    #16 0x7fba66a7e588 in message_filters::SimpleFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::signalMessage(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/simple_filter.h:133:13
    #17 0x7fba66af9785 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::messageReady(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/tf2_ros/message_filter.h:643:13
    #18 0x7fba66aee19b in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long) /opt/ros/foxy/include/tf2_ros/message_filter.h:537:7
    #19 0x7fba66afd3d2 in void std::__invoke_impl<void, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(std::__invoke_memfun_deref, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #20 0x7fba66afd077 in std::__invoke_result<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>::type std::__invoke<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #21 0x7fba66afceee in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::__call<void, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, 0ul, 1ul, 2ul>(std::tuple<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&>&&, std::_Index_tuple<0ul, 1ul, 2ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #22 0x7fba66afcc62 in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::operator()<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, void>(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #23 0x7fba66afc2cd in std::_Function_handler<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&), std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)> >::_M_invoke(std::_Any_data const&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #24 0x7fba671c92e7 in std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&)>::operator()(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) const /usr/include/c++/9/bits/std_function.h:688:14
    #25 0x7fba671c92e7 in operator() obj-x86_64-linux-gnu/./src/buffer.cpp:263:15
    #26 0x7fba671c92e7 in _M_invoke /usr/include/c++/9/bits/std_function.h:300:37
    #27 0x7fba67149330 in std::function<void (unsigned long, 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&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult)>::operator()(unsigned long, 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&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, tf2::TransformableResult) const /usr/include/c++/9/bits/std_function.h:688:14
    #28 0x7fba67149330 in tf2::BufferCore::testTransformableRequests() obj-x86_64-linux-gnu/./src/buffer_core.cpp:1493:13
    #29 0x7fba6714abc3 in tf2::BufferCore::setTransformImpl(tf2::Transform const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) obj-x86_64-linux-gnu/./src/buffer_core.cpp:337:28
    #30 0x7fba6714c5fc in tf2::BufferCore::setTransform(geometry_msgs::msg::TransformStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) obj-x86_64-linux-gnu/./src/buffer_core.cpp:234:26
    #31 0x7fba671d0695 in tf2_ros::TransformListener::subscription_callback(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool) obj-x86_64-linux-gnu/./src/transform_listener.cpp:102:27
    #32 0x7fba671d801c in void std::__invoke_impl<void, void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool&>(std::__invoke_memfun_deref, void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&, bool&) /usr/include/c++/9/bits/invoke.h:73:46
    #33 0x7fba671d801c in std::__invoke_result<void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool&>::type std::__invoke<void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool&>(void (tf2_ros::TransformListener::*&)(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool), tf2_ros::TransformListener*&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&, bool&) /usr/include/c++/9/bits/invoke.h:95:40
    #34 0x7fba671d801c in void std::_Bind<void (tf2_ros::TransformListener::* (tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)>::__call<void, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&, 0ul, 1ul, 2ul>(std::tuple<std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul, 2ul>) /usr/include/c++/9/functional:400:24
    #35 0x7fba671d801c in void std::_Bind<void (tf2_ros::TransformListener::* (tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)>::operator()<std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, void>(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) /usr/include/c++/9/functional:484:24
    #36 0x7fba671d801c in std::_Function_handler<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >), std::_Bind<void (tf2_ros::TransformListener::* (tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) /usr/include/c++/9/bits/std_function.h:300:37
    #37 0x7fba671e4660 in std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>::operator()(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >) const /usr/include/c++/9/bits/std_function.h:688:14
    #38 0x7fba671e4660 in rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #39 0x7fba671e4ece in rclcpp::Subscription<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:5
    #40 0x7fba66fc002b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #41 0x7fba66fc08ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #42 0x7fba66fc10a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)
    #43 0x7fba66fc5a4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #44 0x7fba671d08e1 in operator() obj-x86_64-linux-gnu/./src/transform_listener.cpp:79:21
    #45 0x7fba671d08e1 in __invoke_impl<void, tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > /usr/include/c++/9/bits/invoke.h:60:36
    #46 0x7fba671d08e1 in __invoke<tf2_ros::TransformListener::initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)>, std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> > /usr/include/c++/9/bits/invoke.h:95:40
    #47 0x7fba671d08e1 in _M_invoke<0, 1> /usr/include/c++/9/thread:244:26
    #48 0x7fba671d08e1 in operator() /usr/include/c++/9/thread:251:31
    #49 0x7fba671d08e1 in _M_run /usr/include/c++/9/thread:195:13
    #50 0x7fba66178de3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #51 0x7fba66e96608 in start_thread (/lib/x86_64-linux-gnu/libpthread.so.0+0x9608)
    #52 0x7fba65e5b292 in clone (/lib/x86_64-linux-gnu/libc.so.6+0x122292)

0x6020000420f8 is located 0 bytes to the right of 8-byte region [0x6020000420f0,0x6020000420f8)
allocated by thread T10 here:
    #0 0x4c664d in operator new[](unsigned long) (/home/r1/ros2_nav_fuzz/build/nav2_amcl/amcl+0x4c664d)
    #1 0x7fba662a75ad in CachedDistanceMap::CachedDistanceMap(double, double) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:43:18
    #2 0x7fba662a4b58 in get_distance_map(double, double) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:83:15
    #3 0x7fba662a59df in map_update_cspace /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:133:29
    #4 0x7fba662b7525 in nav2_amcl::LikelihoodFieldModel::LikelihoodFieldModel(double, double, double, double, unsigned long, map_t*) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp:38:3
    #5 0x7fba669a5aa5 in nav2_amcl::AmclNode::createLaserObject() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:1051:14
    #6 0x7fba66998cf5 in nav2_amcl::AmclNode::addNewScanner(int&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::msg::PoseStamped_<std::allocator<void> >&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:749:21
    #7 0x7fba66995106 in nav2_amcl::AmclNode::laserReceived(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/amcl_node.cpp:649:10
    #8 0x7fba66b03bae in void std::__invoke_impl<void, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(std::__invoke_memfun_deref, void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #9 0x7fba66b0383a in std::__invoke_result<void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>::type std::__invoke<void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(void (nav2_amcl::AmclNode::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), nav2_amcl::AmclNode*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #10 0x7fba66b036e7 in void std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::__call<void, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, 0ul, 1ul>(std::tuple<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #11 0x7fba66b034f2 in void std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, void>(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #12 0x7fba66b02e4d in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (nav2_amcl::AmclNode::* (nav2_amcl::AmclNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #13 0x7fba66b01ffd in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #14 0x7fba66b0190d in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #15 0x7fba66ac3465 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #16 0x7fba66b02524 in message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) /opt/ros/foxy/include/message_filters/signal1.h:74:5
    #17 0x7fba66a7e8a5 in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/signal1.h:117:15
    #18 0x7fba66a7e588 in message_filters::SimpleFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::signalMessage(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/simple_filter.h:133:13
    #19 0x7fba66af9785 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::messageReady(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/tf2_ros/message_filter.h:643:13
    #20 0x7fba66aee19b in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long) /opt/ros/foxy/include/tf2_ros/message_filter.h:537:7
    #21 0x7fba66afd3d2 in void std::__invoke_impl<void, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(std::__invoke_memfun_deref, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #22 0x7fba66afd077 in std::__invoke_result<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>::type std::__invoke<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #23 0x7fba66afceee in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::__call<void, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, 0ul, 1ul, 2ul>(std::tuple<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&>&&, std::_Index_tuple<0ul, 1ul, 2ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #24 0x7fba66afcc62 in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::operator()<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, void>(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #25 0x7fba66afc2cd in std::_Function_handler<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&), std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)> >::_M_invoke(std::_Any_data const&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #26 0x7fba671c92e7 in std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&)>::operator()(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) const /usr/include/c++/9/bits/std_function.h:688:14
    #27 0x7fba671c92e7 in operator() obj-x86_64-linux-gnu/./src/buffer.cpp:263:15
    #28 0x7fba671c92e7 in _M_invoke /usr/include/c++/9/bits/std_function.h:300:37

Thread T10 created by T0 here:
    #0 0x481b7a in pthread_create (/home/r1/ros2_nav_fuzz/build/nav2_amcl/amcl+0x481b7a)
    #1 0x7fba661790a8 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)

SUMMARY: AddressSanitizer: heap-buffer-overflow /home/r1/ros2_nav_fuzz/src/navigation2/nav2_amcl/src/map/map_cspace.cpp:102:21 in enqueue(map_t*, int, int, int, int, std::priority_queue<CellData, std::vector<CellData, std::allocator<CellData> >, std::less<CellData> >&, CachedDistanceMap*, unsigned char*)
Shadow bytes around the buggy address:
  0x0c04800003c0: fa fa fd fa fa fa fd fa fa fa fd fa fa fa fd fa
  0x0c04800003d0: fa fa fd fa fa fa fd fd fa fa 00 fa fa fa 00 fa
  0x0c04800003e0: fa fa fd fa fa fa fd fa fa fa 00 fa fa fa fd fd
  0x0c04800003f0: fa fa fd fd fa fa 00 fa fa fa 00 fa fa fa 00 fa
  0x0c0480000400: fa fa fd fd fa fa fd fd fa fa fd fa fa fa fd fa
=>0x0c0480000410: fa fa fd fa fa fa fd fa fa fa fd fa fa fa 00[fa]
  0x0c0480000420: fa fa 00 fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480000430: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480000440: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480000450: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480000460: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
Shadow byte legend (one shadow byte represents 8 application bytes):
  Addressable:           00
  Partially addressable: 01 02 03 04 05 06 07 
  Heap left redzone:       fa
  Freed heap region:       fd
  Stack left redzone:      f1
  Stack mid redzone:       f2
  Stack right redzone:     f3
  Stack after return:      f5
  Stack use after scope:   f8
  Global redzone:          f9
  Global init order:       f6
  Poisoned by user:        f7
  Container overflow:      fc
  Array cookie:            ac
  Intra object redzone:    bb
  ASan internal:           fe
  Left alloca redzone:     ca
  Right alloca redzone:    cb
  Shadow gap:              cc
==560519==ABORTING

I'll also explore the root cause of this, just report the event first.