Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Jump to bottom

[Recover] possible use-after-free bug in nav2_costmap_2d::FootprintSubscriber::getFootprint() #2507

Closed
easylyou opened this issue Aug 13, 2021 · 5 comments
Closed

[Recover] possible use-after-free bug in nav2_costmap_2d::FootprintSubscriber::getFootprint() #2507

easylyou opened this issue Aug 13, 2021 · 5 comments
Labels
in progress

Comments

@easylyou
Copy link
Contributor

Bug report

Required Info:

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

Steps to reproduce issue

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

I manually reviewed the related code and found the possible cause.
It looks like a bug of data race, but I can't be sure.

// in navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp

bool
FootprintSubscriber::getFootprint(...)
{
  ...
  //1: footprint_->polygon is doing copy construct, but not finished
  //3: copying, boom! use after free.
  footprint = toPointVector(
    std::make_shared<geometry_msgs::msg::Polygon>(footprint_->polygon));
  auto & footprint_stamp = footprint_->header.stamp;
  ...
}


void
FootprintSubscriber::footprint_callback()
{
  //2: set the new footprint_, so the old footprint_ is released 
  footprint_ = msg;
  ...
}

Expected behavior

Process should not crash.

Actual behavior

=================================================================
==12174==ERROR: AddressSanitizer: heap-use-after-free on address 0x610000211f40 at pc 0x0000004fef2a bp 0x7f99e7154430 sp 0x7f99e7153bf8
READ of size 12 at 0x610000211f40 thread T13
    #0 0x4fef29 in __asan_memcpy (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4fef29)
    #1 0x7f99f4760060 in void std::_Construct<geometry_msgs::msg::Point32_<std::allocator<void> >, geometry_msgs::msg::Point32_<std::allocator<void> > const&>(geometry_msgs::msg::Point32_<std::allocator<void> >*, geometry_msgs::msg::Point32_<std::allocator<void> > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_construct.h:75:38
    #2 0x7f99f475fd61 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::__uninitialized_copy<false>::__uninit_copy<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*>(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:83:3
    #3 0x7f99f475fb15 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::uninitialized_copy<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*>(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:137:14
    #4 0x7f99f475f062 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::__uninitialized_copy_a<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*, geometry_msgs::msg::Point32_<std::allocator<void> > >(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:307:14
    #5 0x7f99f475e58c in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::vector(std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:555:4
    #6 0x7f99f3e84d68 in geometry_msgs::msg::Polygon_<std::allocator<void> >::Polygon_(geometry_msgs::msg::Polygon_<std::allocator<void> > const&) /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon__struct.hpp:35:8
    #7 0x7f99f3e94a64 in void __gnu_cxx::new_allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >::construct<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >*, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #8 0x7f99f3e94950 in void std::allocator_traits<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >::construct<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >&, geometry_msgs::msg::Polygon_<std::allocator<void> >*, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #9 0x7f99f39354e6 in std::_Sp_counted_ptr_inplace<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #10 0x7f99f3934f36 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >*&, std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #11 0x7f99f3934c07 in std::__shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #12 0x7f99f39349cc in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >::shared_ptr<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #13 0x7f99f3934728 in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > > std::allocate_shared<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > const&, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #14 0x7f99f3878713 in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > > std::make_shared<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #15 0x7f99f38757bb in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&, rclcpp::Time&, rclcpp::Duration) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:77:5
    #16 0x7f99f3875dc0 in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&, rclcpp::Duration&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:93:10
    #17 0x7f99f387600b in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:100:10
    #18 0x7f99f398f473 in nav2_costmap_2d::CostmapTopicCollisionChecker::getFootprint(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:95:23
    #19 0x7f99f398f098 in nav2_costmap_2d::CostmapTopicCollisionChecker::scorePose(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:89:43
    #20 0x7f99f398c96e in nav2_costmap_2d::CostmapTopicCollisionChecker::isCollisionFree(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:58:9
    #21 0x7f99e75b499f in nav2_recoveries::Spin::isCollisionFree(double const&, geometry_msgs::msg::Twist_<std::allocator<void> >*, geometry_msgs::msg::Pose2D_<std::allocator<void> >&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/plugins/spin.cpp:164:30
    #22 0x7f99e75b3a3c in nav2_recoveries::Spin::onCycleUpdate() /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/plugins/spin.cpp:133:8
    #23 0x7f99e75d0a82 in nav2_recoveries::Recovery<nav2_msgs::action::Spin>::execute() /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/include/nav2_recoveries/recovery.hpp:201:15
    #24 0x7f99e75d6a60 in void std::__invoke_impl<void, void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>(std::__invoke_memfun_deref, void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #25 0x7f99e75d688d in std::__invoke_result<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>::type std::__invoke<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>(void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #26 0x7f99e75d67d5 in void std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()>::__call<void, 0ul>(std::tuple<>&&, std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #27 0x7f99e75d65f3 in void std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()>::operator()<void>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #28 0x7f99e75d6100 in std::_Function_handler<void (), std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #29 0x7f99f5fb9c68 in std::function<void ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #30 0x7f99e760c7a2 in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::work() /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:144:9
    #31 0x7f99e760c0a0 in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()::operator()() const /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:68
    #32 0x7f99e760c040 in nav2_msgs::action::Spin std::__invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(std::__invoke_other, rclcpp_lifecycle::LifecycleNode&&, nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #33 0x7f99e760bf90 in std::__invoke_result<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode...>::type std::__invoke<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #34 0x7f99e760bf58 in void std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #35 0x7f99e760bcc8 in std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #36 0x7f99e760b9cf in std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1362:6
    #37 0x7f99e760b62e in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:285:9
    #38 0x7f99f5f022ff in std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #39 0x7f99f5f01bd4 in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:561:27
    #40 0x7f99f5f02243 in void std::__invoke_impl<void, void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::__invoke_memfun_deref, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #41 0x7f99f5f01fa7 in std::__invoke_result<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>::type std::__invoke<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #42 0x7f99f5f01f18 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:671:4
    #43 0x7f99f5f01df6 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:25
    #44 0x7f99f5f01d72 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::__invoke() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:21
    #45 0x7f99f2fb147e in __pthread_once_slow /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_once.c:116:7
    #46 0x7f99f5ed7e60 in __gthread_once(int*, void (*)()) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/x86_64-linux-gnu/c++/9/bits/gthr-default.h:700:12
    #47 0x7f99f5f01a0c in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:683:17
    #48 0x7f99f5f00c8c in std::__future_base::_State_baseV2::_M_set_result(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>, bool) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:401:2
    #49 0x7f99e7608c84 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1662:3
    #50 0x7f99e76089a0 in nav2_msgs::action::Spin std::__invoke_impl<void, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()>(std::__invoke_other, rclcpp_lifecycle::LifecycleNode&&, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #51 0x7f99e76088f0 in std::__invoke_result<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode...>::type std::__invoke<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()>(nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #52 0x7f99e76088b8 in void std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #53 0x7f99e7608878 in std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #54 0x7f99e7608692 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> > >::_M_run() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:195:13
    #55 0x7f99f2a5cde3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #56 0x7f99f2fa8608 in start_thread /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_create.c:477:8
    #57 0x7f99f273f292 in clone /build/glibc-eX1tMB/glibc-2.31/misc/../sysdeps/unix/sysv/linux/x86_64/clone.S:95

0x610000211f40 is located 0 bytes inside of 192-byte region [0x610000211f40,0x610000212000)
freed by thread T0 here:
    #0 0x52faad in operator delete(void*) (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x52faad)
    #1 0x7f99f475b743 in __gnu_cxx::new_allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >::deallocate(geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:128:2
    #2 0x7f99f475b6eb in std::allocator_traits<std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::deallocate(std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >&, geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:470:13
    #3 0x7f99f475b648 in std::_Vector_base<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::_M_deallocate(geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:351:4
    #4 0x7f99f475b4f0 in std::_Vector_base<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::~_Vector_base() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:332:2
    #5 0x7f99f475ae42 in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::~vector() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:680:7
    #6 0x7f99f3e834d8 in geometry_msgs::msg::Polygon_<std::allocator<void> >::~Polygon_() /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon__struct.hpp:35:8
    #7 0x7f99f3eb82a9 in geometry_msgs::msg::PolygonStamped_<std::allocator<void> >::~PolygonStamped_() /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon_stamped__struct.hpp:37:8
    #8 0x7f99f3ebaf04 in void __gnu_cxx::new_allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >::destroy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >(geometry_msgs::msg::PolygonStamped_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:153:10
    #9 0x7f99f3ebae4e in void std::allocator_traits<std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >::destroy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >(std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&, geometry_msgs::msg::PolygonStamped_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:497:8
    #10 0x7f99f3eba942 in std::_Sp_counted_ptr_inplace<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, (__gnu_cxx::_Lock_policy)2>::_M_dispose() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:557:2
    #11 0x532a46 in std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:155:6
    #12 0x550295 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::operator=(std::__shared_count<(__gnu_cxx::_Lock_policy)2> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:749:15
    #13 0x7f99f3879929 in std::__shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2>::operator=(std::__shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1168:69
    #14 0x7f99f3878af5 in std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >::operator=(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:309:65
    #15 0x7f99f38754b1 in nav2_costmap_2d::FootprintSubscriber::footprint_callback(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:106:14
    #16 0x7f99f38de78e in void std::__invoke_impl<void, void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #17 0x7f99f38de41a in std::__invoke_result<void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >::type std::__invoke<void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >(void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #18 0x7f99f38de2c7 in void std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::__call<void, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #19 0x7f99f38de0a2 in void std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::operator()<std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, void>(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #20 0x7f99f38ddcad in std::_Function_handler<void (std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #21 0x7f99f3920765 in std::function<void (std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::operator()(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #22 0x7f99f39233bc in rclcpp::AnySubscriptionCallback<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #23 0x7f99f38e9db3 in rclcpp::Subscription<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #24 0x7f99f30da02b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #25 0x7f99f30da8ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #26 0x7f99f30db0a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)

previously allocated by thread T0 here:
    #0 0x52f24d in operator new(unsigned long) (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x52f24d)
    #1 0x7f99f36a2654 in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::_M_default_append(unsigned long) (/opt/ros/foxy/lib/libsensor_msgs__rosidl_typesupport_introspection_cpp.so+0x12654)

Thread T13 created by T0 here:
    #0 0x4ea88a in pthread_create (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4ea88a)
    #1 0x7f99f2a5d0a8 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 0x7f99e7606413 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1659:14
    #3 0x7f99e76061b4 in void __gnu_cxx::new_allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #4 0x7f99e7605d20 in void std::allocator_traits<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #5 0x7f99e76056a6 in std::_Sp_counted_ptr_inplace<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #6 0x7f99e76050f6 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(nav2_msgs::action::Spin*&, std::_Sp_alloc_shared_tag<rclcpp_lifecycle::LifecycleNode>, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #7 0x7f99e7604dc7 in std::__shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #8 0x7f99e7604b8c in std::shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #9 0x7f99e76048e8 in std::shared_ptr<nav2_msgs::action::Spin> std::allocate_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(rclcpp_lifecycle::LifecycleNode const&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #10 0x7f99e76045b3 in std::shared_ptr<nav2_msgs::action::Spin> std::make_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #11 0x7f99e7603e60 in std::shared_ptr<std::__future_base::_State_baseV2> std::__future_base::_S_make_async_state<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(nav2_msgs::action::Spin&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1700:14
    #12 0x7f99e75fc7af in std::future<std::__invoke_result<std::decay<nav2_msgs::action::Spin>::type, std::decay<rclcpp_lifecycle::LifecycleNode>::type...>::type> std::async<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(std::launch, nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1714:18
    #13 0x7f99e75daa8c in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >) /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:27
    #14 0x7f99e7616dae in void std::__invoke_impl<void, void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >(std::__invoke_memfun_deref, void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #15 0x7f99e7616b5a in std::__invoke_result<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >::type std::__invoke<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >(void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #16 0x7f99e7616a87 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::__call<void, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #17 0x7f99e7616892 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::operator()<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >, void>(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #18 0x7f99e76163bd in std::_Function_handler<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #19 0x7f99e75e8125 in std::function<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::operator()(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #20 0x7f99e75ded96 in rclcpp_action::Server<nav2_msgs::action::Spin>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_t>, std::array<unsigned char, 16ul>, std::shared_ptr<void>) /opt/ros/foxy/include/rclcpp_action/server.hpp:429:5
    #21 0x7f99f32feb2c in rclcpp_action::ServerBase::execute_goal_request_received() (/opt/ros/foxy/lib/librclcpp_action.so+0x15b2c)

SUMMARY: AddressSanitizer: heap-use-after-free (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4fef29) in __asan_memcpy
Shadow bytes around the buggy address:
  0x0c208003a390: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208003a3a0: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208003a3b0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208003a3c0: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208003a3d0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
=>0x0c208003a3e0: fa fa fa fa fa fa fa fa[fd]fd fd fd fd fd fd fd
  0x0c208003a3f0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208003a400: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208003a410: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208003a420: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208003a430: fd fd fd fd fd fd fd fd fd fd fd fd fd fd 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
==12174==ABORTING

==83523==ERROR: AddressSanitizer: heap-use-after-free on address 0x610000196d88 at pc 0x0000004fef2a bp 0x7ff006d4e430 sp 0x7ff006d4dbf8
READ of size 12 at 0x610000196d88 thread T13
    #0 0x4fef29 in __asan_memcpy (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4fef29)
    #1 0x7ff01435a060 in void std::_Construct<geometry_msgs::msg::Point32_<std::allocator<void> >, geometry_msgs::msg::Point32_<std::allocator<void> > const&>(geometry_msgs::msg::Point32_<std::allocator<void> >*, geometry_msgs::msg::Point32_<std::allocator<void> > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_construct.h:75:38
    #2 0x7ff014359d61 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::__uninitialized_copy<false>::__uninit_copy<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*>(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:83:3
    #3 0x7ff014359b15 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::uninitialized_copy<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*>(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:137:14
    #4 0x7ff014359062 in geometry_msgs::msg::Point32_<std::allocator<void> >* std::__uninitialized_copy_a<__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*, geometry_msgs::msg::Point32_<std::allocator<void> > >(__gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, __gnu_cxx::__normal_iterator<geometry_msgs::msg::Point32_<std::allocator<void> > const*, std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > >, geometry_msgs::msg::Point32_<std::allocator<void> >*, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_uninitialized.h:307:14
    #5 0x7ff01435858c in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::vector(std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:555:4
    #6 0x7ff013a7ed68 in geometry_msgs::msg::Polygon_<std::allocator<void> >::Polygon_(geometry_msgs::msg::Polygon_<std::allocator<void> > const&) /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon__struct.hpp:35:8
    #7 0x7ff013a8ea64 in void __gnu_cxx::new_allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >::construct<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >*, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #8 0x7ff013a8e950 in void std::allocator_traits<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >::construct<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >&, geometry_msgs::msg::Polygon_<std::allocator<void> >*, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #9 0x7ff01352f4e6 in std::_Sp_counted_ptr_inplace<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #10 0x7ff01352ef36 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >*&, std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #11 0x7ff01352ec07 in std::__shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #12 0x7ff01352e9cc in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >::shared_ptr<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::_Sp_alloc_shared_tag<std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #13 0x7ff01352e728 in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > > std::allocate_shared<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(std::allocator<geometry_msgs::msg::Polygon_<std::allocator<void> > > const&, geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #14 0x7ff013472713 in std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > > std::make_shared<geometry_msgs::msg::Polygon_<std::allocator<void> >, geometry_msgs::msg::Polygon_<std::allocator<void> >&>(geometry_msgs::msg::Polygon_<std::allocator<void> >&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #15 0x7ff01346f7bb in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&, rclcpp::Time&, rclcpp::Duration) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:77:5
    #16 0x7ff01346fdc0 in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&, rclcpp::Duration&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:93:10
    #17 0x7ff01347000b in nav2_costmap_2d::FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > >&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:100:10
    #18 0x7ff013589473 in nav2_costmap_2d::CostmapTopicCollisionChecker::getFootprint(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:95:23
    #19 0x7ff013589098 in nav2_costmap_2d::CostmapTopicCollisionChecker::scorePose(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:89:43
    #20 0x7ff01358696e in nav2_costmap_2d::CostmapTopicCollisionChecker::isCollisionFree(geometry_msgs::msg::Pose2D_<std::allocator<void> > const&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp:58:9
    #21 0x7ff0071d299f in nav2_recoveries::Spin::isCollisionFree(double const&, geometry_msgs::msg::Twist_<std::allocator<void> >*, geometry_msgs::msg::Pose2D_<std::allocator<void> >&) /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/plugins/spin.cpp:164:30
    #22 0x7ff0071d1a3c in nav2_recoveries::Spin::onCycleUpdate() /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/plugins/spin.cpp:133:8
    #23 0x7ff0071eea82 in nav2_recoveries::Recovery<nav2_msgs::action::Spin>::execute() /home/r1/ros2_clang_navigation/src/navigation2/nav2_recoveries/include/nav2_recoveries/recovery.hpp:201:15
    #24 0x7ff0071f4a60 in void std::__invoke_impl<void, void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>(std::__invoke_memfun_deref, void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #25 0x7ff0071f488d in std::__invoke_result<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>::type std::__invoke<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&>(void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::*&)(), nav2_recoveries::Recovery<nav2_msgs::action::Spin>*&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #26 0x7ff0071f47d5 in void std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()>::__call<void, 0ul>(std::tuple<>&&, std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #27 0x7ff0071f45f3 in void std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()>::operator()<void>() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #28 0x7ff0071f4100 in std::_Function_handler<void (), std::_Bind<void (nav2_recoveries::Recovery<nav2_msgs::action::Spin>::* (nav2_recoveries::Recovery<nav2_msgs::action::Spin>*))()> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #29 0x7ff015bb3c68 in std::function<void ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #30 0x7ff00722a7a2 in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::work() /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:144:9
    #31 0x7ff00722a0a0 in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()::operator()() const /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:68
    #32 0x7ff00722a040 in nav2_msgs::action::Spin std::__invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(std::__invoke_other, rclcpp_lifecycle::LifecycleNode&&, nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #33 0x7ff007229f90 in std::__invoke_result<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode...>::type std::__invoke<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #34 0x7ff007229f58 in void std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #35 0x7ff007229cc8 in std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #36 0x7ff0072299cf in std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1362:6
    #37 0x7ff00722962e in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::_M_invoke(std::_Any_data const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:285:9
    #38 0x7ff015afc2ff in std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #39 0x7ff015afbbd4 in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:561:27
    #40 0x7ff015afc243 in void std::__invoke_impl<void, void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::__invoke_memfun_deref, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #41 0x7ff015afbfa7 in std::__invoke_result<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>::type std::__invoke<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #42 0x7ff015afbf18 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:671:4
    #43 0x7ff015afbdf6 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:25
    #44 0x7ff015afbd72 in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&)::'lambda0'()::__invoke() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:676:21
    #45 0x7ff012bab47e in __pthread_once_slow /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_once.c:116:7
    #46 0x7ff015ad1e60 in __gthread_once(int*, void (*)()) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/x86_64-linux-gnu/c++/9/bits/gthr-default.h:700:12
    #47 0x7ff015afba0c in void std::call_once<void (std::__future_base::_State_baseV2::*)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*>(std::once_flag&, void (std::__future_base::_State_baseV2::*&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*&&, bool*&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/mutex:683:17
    #48 0x7ff015afac8c in std::__future_base::_State_baseV2::_M_set_result(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>, bool) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:401:2
    #49 0x7ff007226c84 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()::operator()() const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1662:3
    #50 0x7ff0072269a0 in nav2_msgs::action::Spin std::__invoke_impl<void, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()>(std::__invoke_other, rclcpp_lifecycle::LifecycleNode&&, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:60:14
    #51 0x7ff0072268f0 in std::__invoke_result<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode...>::type std::__invoke<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()>(nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #52 0x7ff0072268b8 in void std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> >::_M_invoke<0ul>(std::_Index_tuple<0ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:244:13
    #53 0x7ff007226878 in std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> >::operator()() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:251:11
    #54 0x7ff007226692 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&)::'lambda'()> > >::_M_run() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/thread:195:13
    #55 0x7ff012656de3  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xd6de3)
    #56 0x7ff012ba2608 in start_thread /build/glibc-eX1tMB/glibc-2.31/nptl/pthread_create.c:477:8
    #57 0x7ff012339292 in clone /build/glibc-eX1tMB/glibc-2.31/misc/../sysdeps/unix/sysv/linux/x86_64/clone.S:95

0x610000196d88 is located 72 bytes inside of 192-byte region [0x610000196d40,0x610000196e00)
freed by thread T0 here:
    #0 0x52faad in operator delete(void*) (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x52faad)
    #1 0x7ff014355743 in __gnu_cxx::new_allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >::deallocate(geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:128:2
    #2 0x7ff0143556eb in std::allocator_traits<std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::deallocate(std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > >&, geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:470:13
    #3 0x7ff014355648 in std::_Vector_base<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::_M_deallocate(geometry_msgs::msg::Point32_<std::allocator<void> >*, unsigned long) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:351:4
    #4 0x7ff0143554f0 in std::_Vector_base<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::~_Vector_base() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:332:2
    #5 0x7ff014354e42 in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::~vector() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/stl_vector.h:680:7
    #6 0x7ff013a7d4d8 in geometry_msgs::msg::Polygon_<std::allocator<void> >::~Polygon_() /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon__struct.hpp:35:8
    #7 0x7ff013ab22a9 in geometry_msgs::msg::PolygonStamped_<std::allocator<void> >::~PolygonStamped_() /opt/ros/foxy/include/geometry_msgs/msg/detail/polygon_stamped__struct.hpp:37:8
    #8 0x7ff013ab4f04 in void __gnu_cxx::new_allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >::destroy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >(geometry_msgs::msg::PolygonStamped_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:153:10
    #9 0x7ff013ab4e4e in void std::allocator_traits<std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >::destroy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >(std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&, geometry_msgs::msg::PolygonStamped_<std::allocator<void> >*) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:497:8
    #10 0x7ff013ab4942 in std::_Sp_counted_ptr_inplace<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, (__gnu_cxx::_Lock_policy)2>::_M_dispose() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:557:2
    #11 0x532a46 in std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_release() /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:155:6
    #12 0x550295 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::operator=(std::__shared_count<(__gnu_cxx::_Lock_policy)2> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:749:15
    #13 0x7ff013473929 in std::__shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2>::operator=(std::__shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, (__gnu_cxx::_Lock_policy)2> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1168:69
    #14 0x7ff013472af5 in std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >::operator=(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:309:65
    #15 0x7ff01346f4b1 in nav2_costmap_2d::FootprintSubscriber::footprint_callback(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >) /home/r1/ros2_clang_navigation/src/navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp:106:14
    #16 0x7ff0134d878e in void std::__invoke_impl<void, void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #17 0x7ff0134d841a in std::__invoke_result<void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >::type std::__invoke<void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > > >(void (nav2_costmap_2d::FootprintSubscriber::*&)(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), nav2_costmap_2d::FootprintSubscriber*&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #18 0x7ff0134d82c7 in void std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::__call<void, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #19 0x7ff0134d80a2 in void std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::operator()<std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, void>(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #20 0x7ff0134d7cad in std::_Function_handler<void (std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >), std::_Bind<void (nav2_costmap_2d::FootprintSubscriber::* (nav2_costmap_2d::FootprintSubscriber*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #21 0x7ff01351a765 in std::function<void (std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >)>::operator()(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #22 0x7ff01351d3bc in rclcpp::AnySubscriptionCallback<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<geometry_msgs::msg::PolygonStamped_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:163:7
    #23 0x7ff0134e3db3 in rclcpp::Subscription<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<geometry_msgs::msg::PolygonStamped_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #24 0x7ff012cd402b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #25 0x7ff012cd48ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #26 0x7ff012cd50a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)

previously allocated by thread T0 here:
    #0 0x52f24d in operator new(unsigned long) (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x52f24d)
    #1 0x7ff01329c654 in std::vector<geometry_msgs::msg::Point32_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point32_<std::allocator<void> > > >::_M_default_append(unsigned long) (/opt/ros/foxy/lib/libsensor_msgs__rosidl_typesupport_introspection_cpp.so+0x12654)

Thread T13 created by T0 here:
    #0 0x4ea88a in pthread_create (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4ea88a)
    #1 0x7ff0126570a8 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 0x7ff007224413 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>::_Async_state_impl(std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1659:14
    #3 0x7ff0072241b4 in void __gnu_cxx::new_allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/ext/new_allocator.h:147:23
    #4 0x7ff007223d20 in void std::allocator_traits<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >::construct<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>*, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/alloc_traits.h:484:8
    #5 0x7ff0072236a6 in std::_Sp_counted_ptr_inplace<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:548:4
    #6 0x7ff0072230f6 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(nav2_msgs::action::Spin*&, std::_Sp_alloc_shared_tag<rclcpp_lifecycle::LifecycleNode>, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:680:6
    #7 0x7ff007222dc7 in std::__shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, (__gnu_cxx::_Lock_policy)2>::__shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr_base.h:1344:14
    #8 0x7ff007222b8c in std::shared_ptr<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >::shared_ptr<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(std::_Sp_alloc_shared_tag<std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> > >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:359:4
    #9 0x7ff0072228e8 in std::shared_ptr<nav2_msgs::action::Spin> std::allocate_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::allocator<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void> >, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(rclcpp_lifecycle::LifecycleNode const&, std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:701:14
    #10 0x7ff0072225b3 in std::shared_ptr<nav2_msgs::action::Spin> std::make_shared<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> >, void>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/shared_ptr.h:717:14
    #11 0x7ff007221e60 in std::shared_ptr<std::__future_base::_State_baseV2> std::__future_base::_S_make_async_state<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()> > >(nav2_msgs::action::Spin&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1700:14
    #12 0x7ff00721a7af in std::future<std::__invoke_result<std::decay<nav2_msgs::action::Spin>::type, std::decay<rclcpp_lifecycle::LifecycleNode>::type...>::type> std::async<nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)::'lambda'()>(std::launch, nav2_msgs::action::Spin&&, rclcpp_lifecycle::LifecycleNode&&...) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/future:1714:18
    #13 0x7ff0071f8a8c in nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >) /home/r1/ros2_clang_navigation/install/nav2_util/include/nav2_util/simple_action_server.hpp:135:27
    #14 0x7ff007234dae in void std::__invoke_impl<void, void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >(std::__invoke_memfun_deref, void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #15 0x7ff007234b5a in std::__invoke_result<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >::type std::__invoke<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> > >(void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #16 0x7ff007234a87 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::__call<void, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&, 0ul, 1ul>(std::tuple<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #17 0x7ff007234892 in void std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::operator()<std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >, void>(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #18 0x7ff0072343bd in std::_Function_handler<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >), std::_Bind<void (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>::* (nav2_util::SimpleActionServer<nav2_msgs::action::Spin, rclcpp_lifecycle::LifecycleNode>*, std::_Placeholder<1>))(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #19 0x7ff007206125 in std::function<void (std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >)>::operator()(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin> >) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #20 0x7ff0071fcd96 in rclcpp_action::Server<nav2_msgs::action::Spin>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_t>, std::array<unsigned char, 16ul>, std::shared_ptr<void>) /opt/ros/foxy/include/rclcpp_action/server.hpp:429:5
    #21 0x7ff012ef8b2c in rclcpp_action::ServerBase::execute_goal_request_received() (/opt/ros/foxy/lib/librclcpp_action.so+0x15b2c)

SUMMARY: AddressSanitizer: heap-use-after-free (/home/r1/ros2_clang_navigation/build/nav2_recoveries/recoveries_server+0x4fef29) in __asan_memcpy
Shadow bytes around the buggy address:
  0x0c208002ad60: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208002ad70: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208002ad80: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208002ad90: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208002ada0: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
=>0x0c208002adb0: fd[fd]fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208002adc0: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208002add0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208002ade0: fa fa fa fa fa fa fa fa fd fd fd fd fd fd fd fd
  0x0c208002adf0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c208002ae00: fa fa fa fa fa fa fa fa fd fd fd fd fd fd 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
==83523==ABORTING
@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 13, 2021 •
edited
Loading

What is your actual issue? 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. Right now, I don't see an issue.

@SteveMacenski
Copy link
Member

Get footprint checks

  if (!footprint_received_) {
    return false;
  }

so it would fail if the callback was not yet triggered. that boolean is only set to true after the footprint_ has been set

  footprint_ = msg;
  if (!footprint_received_) {
    footprint_received_ = true;
  }

@easylyou
Copy link
Contributor Author

Thanks for your reply!
I try to figure out the cause. It is helpful to discuss about the cause of the output. If it is a real bug, recover node will die after the bug is trigger. Here are my further analyses.

Every times a new message received from footprint_topic, footprint_callback will be called.
create_subscription(). Here are the related codes

//in navigation2/nav2_recoveries/src/recovery_server.cpp
nav2_util::CallbackReturn
RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
 ...
  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
    shared_from_this(), footprint_topic, 1.0);
  ... 
}

//in navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp
FootprintSubscriber::FootprintSubscriber() 
{
  footprint_sub_ = rclcpp::create_subscription<geometry_msgs::msg::PolygonStamped>(
    node_topics_,
    topic_name, rclcpp::SystemDefaultsQoS(),
    std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
}

Every times the recover try to spin, getFootprint will be called
ActionServer(). Here are the related codes

//in navigation2/nav2_recoveries/include/nav2_recoveries/recovery.hpp
void configure()
{
  ...
    action_server_ = std::make_shared<ActionServer>(
      node_, recovery_name_,
      std::bind(&Recovery::execute, this));
  ...
}

void execute()
{
  ...
  switch (onCycleUpdate()) {
  ...
}

//in navigation2/nav2_recoveries/plugins/spin.cpp
Status Spin::onCycleUpdate()
{
  ...
  if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d))
  ...
}

//in navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
bool CostmapTopicCollisionChecker::isCollisionFree(
  const geometry_msgs::msg::Pose2D & pose)
{
  ...
   if (scorePose(pose) >= LETHAL_OBSTACLE) 
   ...

}

double CostmapTopicCollisionChecker::scorePose(
  const geometry_msgs::msg::Pose2D & pose)
{
  ...
  return collision_checker_.footprintCost(getFootprint(pose));
}

Those mean that getFootprint() and footprint_callback() could run multiple times and run concurrently
Because of multiple runs, the footprint checks can not prevent it.

@easylyou easylyou mentioned this issue Aug 16, 2021
Merged
6 tasks
@SteveMacenski
Copy link
Member

Lets chat in the PR

@SteveMacenski
Copy link
Member

Merging once we have CI turning over, closing issue since solution was approved.

@easylyou easylyou mentioned this issue Aug 24, 2021
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
in progress
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@SteveMacenski @easylyou