[Planner] possible use-after-free bug in nav2_costmap_2d

[easylyou]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04.2 LTS
• ROS2 Version:
  • Foxy
• Version or commit hash:
  • a7955d8
• DDS implementation:
  • default

Steps to reproduce issue

I had done some tests. And it can be reproduced.

[INFO] [1628747203.189238115] [global_costmap.global_costmap]: Configuring
[INFO] [1628747203.199641398] [global_costmap.global_costmap]: Using plugin "static_layer"
[INFO] [1628747203.205795632] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[INFO] [1628747203.208597874] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[INFO] [1628747203.208680902] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[INFO] [1628747203.211488645] [global_costmap.global_costmap]: Subscribed to Topics: scan
[INFO] [1628747203.218346306] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[INFO] [1628747203.218425410] [global_costmap.global_costmap]: Using plugin "voxel_layer"
[INFO] [1628747203.220963960] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 384 X 384 at 0.050000 m/pi

According to the log of a real launch, it is possible that a resizing costmap of staticLayer happens before voxel_layer complete the initializing.

in navigation2/nav2_voxel_grid/src/voxel_grid.cpp

void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
{
  ...
  //2. a resize happends
  delete[] data_;
  
  ...

  data_ = new uint32_t[size_x_ * size_y_];

  ...
}

void VoxelGrid::reset()
{
  //1. VoxelGrid is initializing
  //3. boom! use after free
  uint32_t * col = data_;
  for (unsigned int i = 0; i < size_x_ * size_y_; ++i) {
    *col = unknown_col;
    ++col;
  }
}

Expected behavior

Process should not crash.

Actual behavior

==178418==ERROR: AddressSanitizer: heap-use-after-free on address 0x6020000560f0 at pc 0x7fc6b81ae43a bp 0x7ffc328e9410 sp 0x7ffc328e9408
WRITE of size 4 at 0x6020000560f0 thread T0
    #0 0x7fc6b81ae439 in nav2_voxel_grid::VoxelGrid::reset() /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:103:10
    #1 0x7fc6b98b235c in nav2_costmap_2d::VoxelLayer::resetMaps() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:126:15
    #2 0x7fc6b8e42a38 in nav2_costmap_2d::Costmap2D::resizeMap(unsigned int, unsigned int, double, double, double) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d.cpp:88:3
    #3 0x7fc6b90e0fc4 in nav2_costmap_2d::CostmapLayer::matchSize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_layer.cpp:59:3
    #4 0x7fc6b975b9f8 in nav2_costmap_2d::ObstacleLayer::onInitialize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp:104:18
    #5 0x7fc6b98aefea in nav2_costmap_2d::VoxelLayer::onInitialize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:62:18
    #6 0x7fc6b8e4ed1d in nav2_costmap_2d::Layer::initialize(nav2_costmap_2d::LayeredCostmap*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, tf2_ros::Buffer*, std::shared_ptr<rclcpp_lifecycle::LifecycleNode>, std::shared_ptr<rclcpp::Node>, std::shared_ptr<rclcpp::Node>) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/layer.cpp:61:3
    #7 0x7fc6b8e71ca5 in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:153:13
    #8 0x7fc6b9ccc52c in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #9 0x7fc6b9b6dc27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)
    #10 0x7fc6b9b6e16c in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::change_state(unsigned char, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2c16c)
    #11 0x7fc6b9b6e677 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2c677)
    #12 0x7fc6b9b65821 in std::_Function_handler<void (std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >), std::_Bind<void (rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::* (rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl*, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<rmw_request_id_t>&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >&&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x23821)
    #13 0x7fc6b9b6817a in rclcpp::Service<lifecycle_msgs::srv::ChangeState>::handle_request(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2617a)
    #14 0x7fc6b80245dc  (/opt/ros/foxy/lib/librclcpp.so+0xd65dc)
    #15 0x7fc6b802502b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #16 0x7fc6b802529b in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd729b)
    #17 0x7fc6b802613c in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd813c)
    #18 0x7fc6b802aa4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #19 0x7fc6b80284e7 in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) (/opt/ros/foxy/lib/librclcpp.so+0xda4e7)
    #20 0x4c90d9 in main /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/main.cpp:25:3
    #21 0x7fc6b76b00b2 in __libc_start_main /build/glibc-eX1tMB/glibc-2.31/csu/../csu/libc-start.c:308:16
    #22 0x41e68d in _start (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x41e68d)

0x6020000560f1 is located 0 bytes to the right of 1-byte region [0x6020000560f0,0x6020000560f1)
freed by thread T13 here:
    #0 0x4c6e9d in operator delete[](void*) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c6e9d)
    #1 0x7fc6b81ad9fc in nav2_voxel_grid::VoxelGrid::resize(unsigned int, unsigned int, unsigned int) /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:72:3
    #2 0x7fc6b98b2263 in nav2_costmap_2d::VoxelLayer::matchSize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:108:15
    #3 0x7fc6b8e58ac5 in nav2_costmap_2d::LayeredCostmap::resizeMap(unsigned int, unsigned int, double, double, double, bool) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/layered_costmap.cpp:99:16
    #4 0x7fc6b96420e3 in nav2_costmap_2d::StaticLayer::processMap(nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:184:23
    #5 0x7fc6b963df32 in nav2_costmap_2d::StaticLayer::incomingMap(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:265:5
    #6 0x7fc6b96b35de in void std::__invoke_impl<void, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #7 0x7fc6b96b326a in std::__invoke_result<void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >::type std::__invoke<void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >(void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #8 0x7fc6b96b3117 in void std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::__call<void, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #9 0x7fc6b96b2ef2 in void std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::operator()<std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >, void>(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #10 0x7fc6b96b2afd in std::_Function_handler<void (std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #11 0x7fc6b96f58b5 in std::function<void (std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::operator()(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #12 0x7fc6b96f850c in rclcpp::AnySubscriptionCallback<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #13 0x7fc6b96beb03 in rclcpp::Subscription<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #14 0x7fc6b802502b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #15 0x7fc6b80258ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #16 0x7fc6b80260a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)

previously allocated by thread T0 here:
    #0 0x4c664d in operator new[](unsigned long) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c664d)
    #1 0x7fc6b81ad307 in nav2_voxel_grid::VoxelGrid::VoxelGrid(unsigned int, unsigned int, unsigned int) /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:55:11
    #2 0x7fc6b98bf96c in nav2_costmap_2d::VoxelLayer::VoxelLayer() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp:62:5
    #3 0x7fc6b98bf82f in class_loader::impl::MetaObject<nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer>::create() const /opt/ros/foxy/include/class_loader/meta_object.hpp:218:16
    #4 0x7fc6b8f77d98 in nav2_costmap_2d::Layer* class_loader::impl::createInstance<nav2_costmap_2d::Layer>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, class_loader::ClassLoader*) /opt/ros/foxy/include/class_loader/class_loader_core.hpp:314:22
    #5 0x7fc6b8f76fa6 in nav2_costmap_2d::Layer* class_loader::ClassLoader::createRawInstance<nav2_costmap_2d::Layer>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) /opt/ros/foxy/include/class_loader/class_loader.hpp:327:18
    #6 0x7fc6b8f7661b in std::unique_ptr<nav2_costmap_2d::Layer, std::function<void (nav2_costmap_2d::Layer*)> > class_loader::ClassLoader::createUniqueInstance<nav2_costmap_2d::Layer>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) /opt/ros/foxy/include/class_loader/class_loader.hpp:149:18
    #7 0x7fc6b8f75c20 in std::unique_ptr<nav2_costmap_2d::Layer, std::function<void (nav2_costmap_2d::Layer*)> > class_loader::MultiLibraryClassLoader::createUniqueInstance<nav2_costmap_2d::Layer>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) /opt/ros/foxy/include/class_loader/multi_library_class_loader.hpp:157:20
    #8 0x7fc6b8f7491b in pluginlib::ClassLoader<nav2_costmap_2d::Layer>::createUniqueInstance(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) /opt/ros/foxy/include/pluginlib/./class_loader_imp.hpp:224:47
    #9 0x7fc6b8e85abe in pluginlib::ClassLoader<nav2_costmap_2d::Layer>::createSharedInstance(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) /opt/ros/foxy/include/pluginlib/./class_loader_imp.hpp:168:10
    #10 0x7fc6b8e71a8b in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:149:52
    #11 0x7fc6b9ccc52c in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #12 0x7fc6b9b6dc27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)

Thread T13 created by T0 here:
    #0 0x481b7a in pthread_create (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x481b7a)
    #1 0x7fc6b7ac90a8 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 0x7fc6b86eb693 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 0x7fc6b86eb41d in nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) /home/r1/ros2_clang_navigation/src/navigation2/nav2_util/src/node_thread.cpp:25:13
    #4 0x7fc6b9dc8c7c in nav2_util::NodeThread::NodeThread<std::shared_ptr<nav2_costmap_2d::Costmap2DROS> >(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/node_thread.hpp:32:5
    #5 0x7fc6b9cdc439 in std::_MakeUniq<nav2_util::NodeThread>::__single_object std::make_unique<nav2_util::NodeThread, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&>(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/unique_ptr.h:857:34
    #6 0x7fc6b9cca3de in nav2_planner::PlannerServer::PlannerServer() /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:56:21
    #7 0x4cbc37 in void __gnu_cxx::new_allocator<nav2_planner::PlannerServer>::construct<nav2_planner::PlannerServer>(nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #8 0x4cb6ae in void std::allocator_traits<std::allocator<nav2_planner::PlannerServer> >::construct<nav2_planner::PlannerServer>(std::allocator<nav2_planner::PlannerServer>&, nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #9 0x4caf33 in std::_Sp_counted_ptr_inplace<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<>(std::allocator<nav2_planner::PlannerServer>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #10 0x4ca88b in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer> >(nav2_planner::PlannerServer*&, std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #11 0x4ca56c in std::__shared_ptr<nav2_planner::PlannerServer, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<nav2_planner::PlannerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #12 0x4ca35f in std::shared_ptr<nav2_planner::PlannerServer>::shared_ptr<std::allocator<nav2_planner::PlannerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #13 0x4ca119 in std::shared_ptr<nav2_planner::PlannerServer> std::allocate_shared<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer> >(std::allocator<nav2_planner::PlannerServer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #14 0x4c995b in std::shared_ptr<nav2_planner::PlannerServer> std::make_shared<nav2_planner::PlannerServer>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #15 0x4c90a3 in main /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/main.cpp:24:15
    #16 0x7fc6b76b00b2 in __libc_start_main /build/glibc-eX1tMB/glibc-2.31/csu/../csu/libc-start.c:308:16

SUMMARY: AddressSanitizer: heap-use-after-free /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:103:10 in nav2_voxel_grid::VoxelGrid::reset()
Shadow bytes around the buggy address:
  0x0c0480002bc0: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002bd0: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002be0: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002bf0: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002c00: fa fa fd fa fa fa fd fa fa fa fd fd fa fa fd fa
=>0x0c0480002c10: fa fa fd fa fa fa fd fd fa fa fd fa fa fa[fd]fa
  0x0c0480002c20: fa fa fd fa fa fa fd fa fa fa fd fa fa fa fd fa
  0x0c0480002c30: fa fa fd fd fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002c40: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002c50: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
  0x0c0480002c60: 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
==178418==ABORTING

==130143==ERROR: AddressSanitizer: heap-use-after-free on address 0x6030000ea800 at pc 0x7f66db15ae15 bp 0x7f66cef5c840 sp 0x7f66cef5c838
READ of size 8 at 0x6030000ea800 thread T13
    #0 0x7f66db15ae14 in std::__shared_ptr<nav2_costmap_2d::Layer, (__gnu_cxx::_Lock_policy)2>::get() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1310:16
    #1 0x7f66db15adbb in std::__shared_ptr_access<nav2_costmap_2d::Layer, (__gnu_cxx::_Lock_policy)2, false, false>::_M_get() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1021:66
    #2 0x7f66db1595b8 in std::__shared_ptr_access<nav2_costmap_2d::Layer, (__gnu_cxx::_Lock_policy)2, false, false>::operator->() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1015:9
    #3 0x7f66db153a61 in nav2_costmap_2d::LayeredCostmap::resizeMap(unsigned int, unsigned int, double, double, double, bool) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/layered_costmap.cpp:99:5
    #4 0x7f66db93d0e3 in nav2_costmap_2d::StaticLayer::processMap(nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:184:23
    #5 0x7f66db938f32 in nav2_costmap_2d::StaticLayer::incomingMap(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:265:5
    #6 0x7f66db9ae5de in void std::__invoke_impl<void, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #7 0x7f66db9ae26a in std::__invoke_result<void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >::type std::__invoke<void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >(void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #8 0x7f66db9ae117 in void std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::__call<void, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #9 0x7f66db9adef2 in void std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::operator()<std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >, void>(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #10 0x7f66db9adafd in std::_Function_handler<void (std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), std::_Bind<void (nav2_costmap_2d::StaticLayer::* (nav2_costmap_2d::StaticLayer*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #11 0x7f66db9f08b5 in std::function<void (std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::operator()(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #12 0x7f66db9f350c in rclcpp::AnySubscriptionCallback<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #13 0x7f66db9b9b03 in rclcpp::Subscription<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #14 0x7f66da32002b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #15 0x7f66da3208ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #16 0x7f66da3210a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)
    #17 0x7f66da325a4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #18 0x7f66da9e7679 in nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0::operator()() const /home/r1/ros2_clang_navigation/src/navigation2/nav2_util/src/node_thread.cpp:29:17
    #19 0x7f66da9e74d0 in void std::__invoke_impl<void, nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0>(std::__invoke_other, 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/invoke.h:60:14
    #20 0x7f66da9e7420 in std::__invoke_result<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0>::type std::__invoke<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/invoke.h:95:14
    #21 0x7f66da9e73e8 in void std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #22 0x7f66da9e73a8 in std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #23 0x7f66da9e71c2 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> > >::_M_run() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:195:13
    #24 0x7f66d9dc3de3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #25 0x7f66da22f608 in start_thread /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_create.c:477:8
    #26 0x7f66d9aa6292 in clone /build/glibc-eX1tMB/glibc-2.31/misc/../sysdeps/unix/sysv/linux/x86_64/clone.S:95

0x6030000ea800 is located 16 bytes inside of 32-byte region [0x6030000ea7f0,0x6030000ea810)
freed by thread T0 here:
    #0 0x4c6d9d in operator delete(void*) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c6d9d)
    #1 0x7f66db15abb3 in __gnu_cxx::new_allocator<std::shared_ptr<nav2_costmap_2d::Layer> >::deallocate(std::shared_ptr<nav2_costmap_2d::Layer>*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:128:2
    #2 0x7f66db15ab5b in std::allocator_traits<std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::deallocate(std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> >&, std::shared_ptr<nav2_costmap_2d::Layer>*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:470:13
    #3 0x7f66db15aab8 in std::_Vector_base<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::_M_deallocate(std::shared_ptr<nav2_costmap_2d::Layer>*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:351:4
    #4 0x7f66db18bde7 in void std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::_M_realloc_insert<std::shared_ptr<nav2_costmap_2d::Layer> const&>(__gnu_cxx::__normal_iterator<std::shared_ptr<nav2_costmap_2d::Layer>*, std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > > >, std::shared_ptr<nav2_costmap_2d::Layer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/vector.tcc:500:7
    #5 0x7f66db18b883 in std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::push_back(std::shared_ptr<nav2_costmap_2d::Layer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:1195:4
    #6 0x7f66db180c3a in nav2_costmap_2d::LayeredCostmap::addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp:118:14
    #7 0x7f66db16cb08 in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:150:23
    #8 0x7f66dbfc752c in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #9 0x7f66dbe68c27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)

previously allocated by thread T0 here:
    #0 0x4c653d in operator new(unsigned long) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c653d)
    #1 0x7f66db18c903 in __gnu_cxx::new_allocator<std::shared_ptr<nav2_costmap_2d::Layer> >::allocate(unsigned long, void const*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:114:27
    #2 0x7f66db18c86f in std::allocator_traits<std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::allocate(std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> >&, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:444:20
    #3 0x7f66db18c448 in std::_Vector_base<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::_M_allocate(unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:343:20
    #4 0x7f66db18bbef in void std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::_M_realloc_insert<std::shared_ptr<nav2_costmap_2d::Layer> const&>(__gnu_cxx::__normal_iterator<std::shared_ptr<nav2_costmap_2d::Layer>*, std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > > >, std::shared_ptr<nav2_costmap_2d::Layer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/vector.tcc:440:33
    #5 0x7f66db18b883 in std::vector<std::shared_ptr<nav2_costmap_2d::Layer>, std::allocator<std::shared_ptr<nav2_costmap_2d::Layer> > >::push_back(std::shared_ptr<nav2_costmap_2d::Layer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:1195:4
    #6 0x7f66db180c3a in nav2_costmap_2d::LayeredCostmap::addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp:118:14
    #7 0x7f66db16cb08 in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:150:23
    #8 0x7f66dbfc752c in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #9 0x7f66dbe68c27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)

Thread T13 created by T0 here:
    #0 0x481b7a in pthread_create (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x481b7a)
    #1 0x7f66d9dc40a8 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 0x7f66da9e6693 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 0x7f66da9e641d in nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) /home/r1/ros2_clang_navigation/src/navigation2/nav2_util/src/node_thread.cpp:25:13
    #4 0x7f66dc0c3c7c in nav2_util::NodeThread::NodeThread<std::shared_ptr<nav2_costmap_2d::Costmap2DROS> >(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/node_thread.hpp:32:5
    #5 0x7f66dbfd7439 in std::_MakeUniq<nav2_util::NodeThread>::__single_object std::make_unique<nav2_util::NodeThread, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&>(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/unique_ptr.h:857:34
    #6 0x7f66dbfc53de in nav2_planner::PlannerServer::PlannerServer() /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:56:21
    #7 0x4cbc37 in void __gnu_cxx::new_allocator<nav2_planner::PlannerServer>::construct<nav2_planner::PlannerServer>(nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #8 0x4cb6ae in void std::allocator_traits<std::allocator<nav2_planner::PlannerServer> >::construct<nav2_planner::PlannerServer>(std::allocator<nav2_planner::PlannerServer>&, nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #9 0x4caf33 in std::_Sp_counted_ptr_inplace<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<>(std::allocator<nav2_planner::PlannerServer>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #10 0x4ca88b in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer> >(nav2_planner::PlannerServer*&, std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #11 0x4ca56c in std::__shared_ptr<nav2_planner::PlannerServer, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<nav2_planner::PlannerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #12 0x4ca35f in std::shared_ptr<nav2_planner::PlannerServer>::shared_ptr<std::allocator<nav2_planner::PlannerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #13 0x4ca119 in std::shared_ptr<nav2_planner::PlannerServer> std::allocate_shared<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer> >(std::allocator<nav2_planner::PlannerServer> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #14 0x4c995b in std::shared_ptr<nav2_planner::PlannerServer> std::make_shared<nav2_planner::PlannerServer>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #15 0x4c90a3 in main /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/main.cpp:24:15
    #16 0x7f66d99ab0b2 in __libc_start_main /build/glibc-eX1tMB/glibc-2.31/csu/../csu/libc-start.c:308:16

SUMMARY: AddressSanitizer: heap-use-after-free /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1310:16 in std::__shared_ptr<nav2_costmap_2d::Layer, (__gnu_cxx::_Lock_policy)2>::get() const
Shadow bytes around the buggy address:
  0x0c06800154b0: fd fd fd fd fa fa fd fd fd fd fa fa fd fd fd fd
  0x0c06800154c0: fa fa fd fd fd fd fa fa fd fd fd fd fa fa fd fd
  0x0c06800154d0: fd fd fa fa fd fd fd fd fa fa fd fd fd fd fa fa
  0x0c06800154e0: fd fd fd fd fa fa fd fd fd fd fa fa fd fd fd fd
  0x0c06800154f0: fa fa fd fd fd fa fa fa 00 00 00 fa fa fa fd fd
=>0x0c0680015500:[fd]fd fa fa fd fd fd fd fa fa fd fd fd fa fa fa
  0x0c0680015510: fd fd fd fa fa fa fd fd fd fa fa fa fd fd fd fa
  0x0c0680015520: fa fa fd fd fd fd fa fa 00 00 07 fa fa fa 00 00
  0x0c0680015530: 00 07 fa fa fd fd fd fd fa fa fd fd fd fd fa fa
  0x0c0680015540: fd fd fd fd fa fa 00 00 00 03 fa fa fd fd fd fd
  0x0c0680015550: fa fa fd fd fd fa fa fa 00 00 04 fa fa fa fd fd
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
==130143==ABORTING

==65951==ERROR: AddressSanitizer: heap-use-after-free on address 0x6020000572d0 at pc 0x7fa65c72364f bp 0x7fa6511e9820 sp 0x7fa6511e9818
WRITE of size 4 at 0x6020000572d0 thread T13
    #0 0x7fa65c72364e in nav2_voxel_grid::VoxelGrid::reset() /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:103:10
    #1 0x7fa65d2b7268 in nav2_costmap_2d::VoxelLayer::matchSize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:107:18
    #2 0x7fa65cdf4a3f in nav2_costmap_2d::LayeredCostmap::resizeMap(unsigned int, unsigned int, double, double, double, bool) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/layered_costmap.cpp:99:16
    #3 0x7fa65d136920 in nav2_costmap_2d::StaticLayer::processMap(nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:184:23
    #4 0x7fa65d13289a in nav2_costmap_2d::StaticLayer::incomingMap(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/static_layer.cpp:265:5
    #5 0x7fa65d17474f in void std::__invoke_impl<void, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_costmap_2d::StaticLayer::*&)(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >), nav2_costmap_2d::StaticLayer*&, std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #6 0x7fa65d19d07d in std::function<void (std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >)>::operator()(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #7 0x7fa65d19d07d in rclcpp::AnySubscriptionCallback<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #8 0x7fa65d180f99 in rclcpp::Subscription<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<nav_msgs::msg::OccupancyGrid_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #9 0x7fa65c59c02b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #10 0x7fa65c59c8ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #11 0x7fa65c59d0a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)
    #12 0x7fa65c5a1a4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #13 0x7fa65cb1e4b9 in nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0::operator()() const /home/r1/ros2_clang_navigation/src/navigation2/nav2_util/src/node_thread.cpp:29:17
    #14 0x7fa65cb1e4b9 in void std::__invoke_impl<void, nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0>(std::__invoke_other, 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/invoke.h:60:14
    #15 0x7fa65cb1e4b9 in std::__invoke_result<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0>::type std::__invoke<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/invoke.h:95:14
    #16 0x7fa65cb1e4b9 in void std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #17 0x7fa65cb1e4b9 in std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #18 0x7fa65cb1e4b9 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::$_0> > >::_M_run() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:195:13
    #19 0x7fa65c03fde3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #20 0x7fa65c4ab608 in start_thread /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_create.c:477:8
    #21 0x7fa65bd22292 in clone /build/glibc-eX1tMB/glibc-2.31/misc/../sysdeps/unix/sysv/linux/x86_64/clone.S:95

0x6020000572d1 is located 0 bytes to the right of 1-byte region [0x6020000572d0,0x6020000572d1)
freed by thread T0 here:
    #0 0x4c6e5d in operator delete[](void*) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c6e5d)
    #1 0x7fa65c722ffd in nav2_voxel_grid::VoxelGrid::resize(unsigned int, unsigned int, unsigned int) /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:72:3
    #2 0x7fa65d2b5282 in nav2_costmap_2d::VoxelLayer::onInitialize() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:98:3
    #3 0x7fa65ce0b85c in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:153:13
    #4 0x7fa65d4fb029 in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #5 0x7fa65d498c27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)

previously allocated by thread T0 here:
    #0 0x4c660d in operator new[](unsigned long) (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x4c660d)
    #1 0x7fa65c7227c6 in nav2_voxel_grid::VoxelGrid::VoxelGrid(unsigned int, unsigned int, unsigned int) /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:55:11
    #2 0x7fa65d2be8d1 in nav2_costmap_2d::VoxelLayer::VoxelLayer() /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp:62:5
    #3 0x7fa65ceb22a8 in nav2_costmap_2d::Layer* class_loader::ClassLoader::createRawInstance<nav2_costmap_2d::Layer>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) /opt/ros/foxy/include/class_loader/class_loader.hpp:327:18

Thread T13 created by T0 here:
    #0 0x481b3a in pthread_create (/home/r1/ros2_clang_navigation/build/nav2_planner/planner_server+0x481b3a)
    #1 0x7fa65c0400a8 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 0x7fa65d5a265d in nav2_util::NodeThread::NodeThread<std::shared_ptr<nav2_costmap_2d::Costmap2DROS> >(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/node_thread.hpp:32:5
    #3 0x7fa65d515ac9 in std::_MakeUniq<nav2_util::NodeThread>::__single_object std::make_unique<nav2_util::NodeThread, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&>(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/unique_ptr.h:857:34
    #4 0x7fa65d4f6ef9 in nav2_planner::PlannerServer::PlannerServer() /home/r1/ros2_clang_navigation/src/navigation2/nav2_planner/src/planner_server.cpp:56:21
    #5 0x4c9ea2 in void __gnu_cxx::new_allocator<nav2_planner::PlannerServer>::construct<nav2_planner::PlannerServer>(nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #6 0x4c9ea2 in void std::allocator_traits<std::allocator<nav2_planner::PlannerServer> >::construct<nav2_planner::PlannerServer>(std::allocator<nav2_planner::PlannerServer>&, nav2_planner::PlannerServer*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #7 0x4c9ea2 in std::_Sp_counted_ptr_inplace<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<>(std::allocator<nav2_planner::PlannerServer>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #8 0x4c9ea2 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<nav2_planner::PlannerServer, std::allocator<nav2_planner::PlannerServer> >(nav2_planner::PlannerServer*&, std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #9 0x4c9ea2 in std::__shared_ptr<nav2_planner::PlannerServer, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<nav2_planner::PlannerServer> >(std::_Sp_alloc_shared_tag<std::allocator<nav2_planner::PlannerServer> >) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #10 0x7fa65bc270b2 in __libc_start_main /build/glibc-eX1tMB/glibc-2.31/csu/../csu/libc-start.c:308:16

SUMMARY: AddressSanitizer: heap-use-after-free /home/r1/ros2_clang_navigation/src/navigation2/nav2_voxel_grid/src/voxel_grid.cpp:103:10 in nav2_voxel_grid::VoxelGrid::reset()
Shadow bytes around the buggy address:
  0x0c0480002e00: fa fa fd fa fa fa fd fa fa fa fd fd fa fa fd fa
  0x0c0480002e10: fa fa fd fa fa fa fd fa fa fa fd fa fa fa fd fd
  0x0c0480002e20: fa fa fd fa fa fa fd fa fa fa fd fa fa fa fd fd
  0x0c0480002e30: fa fa fd fa fa fa fd fa fa fa fd fd fa fa fd fa
  0x0c0480002e40: fa fa fd fa fa fa fd fd fa fa fd fa fa fa fd fa
=>0x0c0480002e50: fa fa fd fd fa fa fd fa fa fa[fd]fa fa fa fd fa
  0x0c0480002e60: fa fa fd fa fa fa fd fa fa fa fd fa fa fa fd fd
  0x0c0480002e70: fa fa 00 00 fa fa 00 00 fa fa 00 00 fa fa fd fa
  0x0c0480002e80: fa fa fd fa fa fa fd fa fa fa fd fd fa fa fd fd
  0x0c0480002e90: fa fa fd fd fa fa fd fa fa fa fd fa fa fa fd fa
  0x0c0480002ea0: fa fa fd fa fa fa 00 00 fa fa 00 00 fa fa 00 00
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
==65951==ABORTING

[SteveMacenski]

it is possible that a resizing costmap of staticLayer happens before voxel_layer complete the initializing

How? Please show in the code where you're talking about. You've provided alot of GDB output, but its unrealistic for me to try to fish through that if I don't understand what your problem is. If you have a solution, please submit a PR.

[easylyou]

Thanks for your reply! Here are my further analyses. It seems a real bug related to multi-threading hard to be detected. If that happens, robot will be out of control.

layered_costmap_ is a global value that all plugins have.
Every plugin initialize one by one.

in navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp
nav2_util::CallbackReturn
Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
  ...
  // Create the costmap itself
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window_, track_unknown_space_);
  ...
  // Then load and add the plug-ins to the costmap
  for (unsigned int i = 0; i < plugin_names_.size(); ++i) {

    std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types_[i]);
    layered_costmap_->addPlugin(plugin);

    plugin->initialize(
      layered_costmap_, plugin_names_[i], tf_buffer_.get(),
      shared_from_this(), client_node_, rclcpp_node_);
  }
  ...
}

incomingMap() can be called at any time after the plugin of static_layer have done onInitialize().
That is, layered_costmap_->resizeMap() may be called before other plugins complete onInitialize().
That is the key problem.

//in navigation2/nav2_costmap_2d/plugins/static_layer.cpp
void
StaticLayer::onInitialize()
{
  ...
  map_sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
    map_topic_, map_qos,
    std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
  ...
}

void
StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
{
  ...
  layered_costmap_->resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x,
      new_map.info.origin.position.y,
      true);
  ...
}

• First problem: std::vector is unsafe in multithreading.
  thread one : layered_costmap_->addPlugin(); vector->push_back()
  Because vector is added an element, it may expand capacity. All elements will be copied to a new place and the olds will be freed.

Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
  for (unsigned int i = 0; i < plugin_names_.size(); ++i) {

    std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types_[i]);
    layered_costmap_->addPlugin(plugin);

    plugin->initialize(
      layered_costmap_, plugin_names_[i], tf_buffer_.get(),
      shared_from_this(), client_node_, rclcpp_node_);
  }
  ...
}

thread two: iterate vector
It is possible that (*plugin) is pointed to a freed memory. That is the second debug output in my first post.

void LayeredCostmap::resizeMap(...)
{
  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
    plugin != plugins_.end(); ++plugin)
  {
    (*plugin)->matchSize();
  }
}

• Second problem: onInitialize() and matchSize() can be called concurrently! Let's see what will happen in voxel_layer.

void VoxelLayer::onInitialize()
{
  ...
  matchSize();
}

void VoxelLayer::matchSize()
{
  voxel_grid_.resize(size_x_, size_y_, size_z_);
}

Those codes above show that voxel_grid_.resize() can be called concurrently.

void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
{
   ...

  delete[] data_;
  size_x_ = size_x;
  size_y_ = size_y;
  size_z_ = size_z;

  ...

  data_ = new uint32_t[size_x_ * size_y_];
  uint32_t unknown_col = ~((uint32_t)0) >> 16;
  uint32_t * col = data_;
  for (unsigned int i = 0; i < size_x_ * size_y_; ++i) {
    *col = unknown_col;
    ++col;
  }
}

**Depends on the order of execution in multithreading, it could be use-after-free or double-free. **
That is the first debug output in my first post.

[SteveMacenski]

I have to admit, these seem like incredibly unlikely things to occur, but happy to have fixes! First merged, second I'm thinking about for a little bit but likely to also be merged

[SteveMacenski]

Both merged! I think that closes this ticket out, feel free to comment back if I am incorrect

[dirkmb]

I'm experiencing a similar problem. The planner server crashes when a new map arrives while adding a filter to layered_costmap_.

I found two reasons for that:

1. PR fix data race: addPlugin() and resizeMap() can be executed concurrently #2512 adds a lock to addPlugin but not to addFilter. I think it was just forgotten and should be added (I opened an PR for that fix data race: addFilter() and resizeMap() can be executed concurrently #3518)
2. It’s still possible that a filter/plugin is not initialized when a resizeMap happens which leads to a crash. Details are described below.

The code I’m talking about is in nav2_costmap_2d/src/costmap_2d_ros.cpp in Costmap2DROS::on_configure

   std::shared_ptr<Layer> filter = plugin_loader_.createSharedInstance(filter_types_[i]); 
   layered_costmap_->addFilter(filter); 
   filter->initialize( 
      	layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), shared_from_this(), callback_group_); 

(the same code exists for plugins, so when I’m talking about a filter I also mean a plugin)

As already noted by @easylyou ‘layered_costmap_ is a global value that all plugins have’. This variable is set by the filter->initialize function. Most of the plugins execute a lock before accessing this variable in matchSize(). With one exception: StaticLayer.

So, if multiple static layers exist, and a map is received while Costmap2DROS::on_configure is still adding filter (precisely when a filter was already added but not yet initialized) the resizeMap function will call matchSize on an uninitialized filter (which means that layered_costmap_ is still null) which leads to a segmentation fault.

My suggested fix would be to change the order of addFilter(...) and initialize(...), but that seems to breaks stuff 🙁(I have not analyzed that further, but I think some plugins/filter might relay on this order).

I’m not sure if there is a better way to fix this issue other than this suggestion:

diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp 
index 878dcfa5..5cc95d4e 100644 
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp 
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp 
@@ -149,12 +149,17 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) 
     RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str()); 
  
     std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types_[i]); 
+    // lock the costmap until the plugin is initialized no map update is allowed (filter->layered_costmap_ needs to be set to not case a segfault when 
+    std::unique_lock<Costmap2D::mutex_t> lock(*(layered_costmap_->getCostmap()->getMutex()));  LayeredCostmap::resizeMap is called) 
+ 
     layered_costmap_->addPlugin(plugin); 
  
     // TODO(mjeronimo): instead of get(), use a shared ptr 
     plugin->initialize( 
       layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), 
       shared_from_this(), client_node_, rclcpp_node_); 
+ 
+    lock.unlock(); 
  
     RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); 
   } 
@@ -163,12 +168,17 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) 
     RCLCPP_INFO(get_logger(), "Using costmap filter \"%s\"", filter_names_[i].c_str()); 
  
     std::shared_ptr<Layer> filter = plugin_loader_.createSharedInstance(filter_types_[i]); 
+    // lock the costmap until the filter is initialized no map update is allowed (filter->layered_costmap_ needs to be set to not case a segfault when LayeredCostmap::resizeMap is called) 
+    std::unique_lock<Costmap2D::mutex_t> lock(*(layered_costmap_->getCostmap()->getMutex()));   
+ 
     layered_costmap_->addFilter(filter); 
  
     filter->initialize( 
       layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), 
       shared_from_this(), client_node_, rclcpp_node_); 
  
+    lock.unlock(); 
+ 
     RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); 
   } 

I’m happy to provide a pull request if there is no better solution for the issue.

[SteveMacenski]

Seems good to me! Overall initialization is not something runtime at steady-state, so I'm OK with a little bit of mutex contention if its not at the risk of running at navigation-time