NullPtr bug during the `computeVelocityCommands()` during shutdown

## Bug report

**Required Info:**

- Operating System:
  -  ubuntu 22.04
- ROS2 Version:
  -  humble
- Version or commit hash:
  -   the latest
- DDS implementation:
  -  the defaulted

#### Steps to reproduce issue


The bug occurs during normal use of nav2_mppi_controller. By changing the controller plugin to MPPI while keeping the rest of the configuration the same as the official default, start navigation2. The YAML configuration for changing the controller plugin to MPPI is:

```yaml
controller_server:
  ros__parameters:
    FollowPath:
      AckermannConstraints:
        min_turning_r: 0.2
      ConstraintCritic:
        cost_power: 1
        cost_weight: 4.0
        enabled: true
      CostCritic:
        collision_cost: 1000000.0
        consider_footprint: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        enabled: true
        near_goal_distance: 1.0
        trajectory_point_step: 2
      GoalAngleCritic:
        cost_power: 1
        cost_weight: 3.0
        enabled: true
        threshold_to_consider: 0.5
      GoalCritic:
        cost_power: 1
        cost_weight: 5.0
        enabled: true
        threshold_to_consider: 1.4
      ObstaclesCritic:
        collision_cost: 10000.0
        collision_margin_distance: 0.1
        consider_footprint: false
        cost_power: 1
        cost_scaling_factor: 10.0
        critical_weight: 20.0
        enabled: true
        inflation_radius: 0.55
        near_goal_distance: 0.5
        repulsion_weight: 1.5
      PathAlignCritic:
        cost_power: 1
        cost_weight: 14.0
        enabled: true
        max_path_occupancy_ratio: 0.05
        offset_from_furthest: 20
        threshold_to_consider: 0.5
        trajectory_point_step: 4
        use_path_orientations: false
      PathAngleCritic:
        cost_power: 1
        cost_weight: 2.0
        enabled: true
        max_angle_to_furthest: 1.0
        mode: 0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
      PathFollowCritic:
        cost_power: 1
        cost_weight: 5.0
        enabled: true
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PreferForwardCritic:
        cost_power: 1
        cost_weight: 5.0
        enabled: true
        threshold_to_consider: 0.5
      TrajectoryVisualizer:
        time_step: 3
        trajectory_step: 5
      TwirlingCritic:
        enabled: true
        twirling_cost_power: 1
        twirling_cost_weight: 10.0
      VelocityDeadbandCritic:
        cost_power: 1
        cost_weight: 35.0
        deadband_velocities:
          - 0.05
          - 0.05
          - 0.05
        enabled: true
      batch_size: 2000
      critics:
        - ConstraintCritic
        - CostCritic
        - GoalCritic
        - GoalAngleCritic
        - PathAlignCritic
        - PathFollowCritic
        - PathAngleCritic
        - PreferForwardCritic
      gamma: 0.015
      iteration_count: 1
      model_dt: 0.05
      motion_model: DiffDrive
      plugin: nav2_mppi_controller::MPPIController
      prune_distance: 1.7
      regenerate_noises: false
      reset_period: 1.0
      temperature: 0.3
      time_steps: 56
      transform_tolerance: 0.1
      visualize: false
      vx_max: 0.5
      vx_min: -0.35
      vx_std: 0.2
      vy_max: 0.5
      vy_std: 0.2
      wz_max: 1.9
      wz_std: 0.4
    controller_frequency: 20.0
    controller_plugins:
      - FollowPath
    failure_tolerance: 0.3
    general_goal_checker:
      plugin: nav2_controller::SimpleGoalChecker
      stateful: true
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    goal_checker_plugins:
      - general_goal_checker
    min_theta_velocity_threshold: 0.001
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    progress_checker:
      movement_time_allowance: 10.0
      plugin: nav2_controller::SimpleProgressChecker
      required_movement_radius: 0.5
    progress_checker_plugin: progress_checker
    use_sim_time: true
```


#### Expected behavior

no NullPtr bug occured. 

#### Actual behavior

the Asan report of this NullPtr bug is as following:
```
 ================================================================= 
 ==605052==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000060 (pc 0x741423520880 bp 0x61f0000ea4d8 sp 0x7414125f57b8 T23) 
 ==605052==The signal is caused by a READ memory access. 
 ==605052==Hint: address points to the zero page. 
     #0 0x741423520880 in nav2_costmap_2d::Costmap2D::worldToMap(double, double, unsigned int&, unsigned int&) const (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so+0xc3880) (BuildId: ae00021bac019132f382130b42d6275d543c55a4) 
     #1 0x741414cd1704 in mppi::PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_mppi_controller/lib/libmppi_controller.so+0xec704) (BuildId: 6464b5c753e86bc8fa48c1a26a3f20eee21259fb) 
     #2 0x741414cd1d0e in mppi::PathHandler::transformPath(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_mppi_controller/lib/libmppi_controller.so+0xecd0e) (BuildId: 6464b5c753e86bc8fa48c1a26a3f20eee21259fb) 
     #3 0x741414c428c9 in nav2_mppi_controller::MPPIController::computeVelocityCommands(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, geometry_msgs::msg::Twist_<std::allocator<void> > const&, nav2_core::GoalChecker*) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_mppi_controller/lib/libmppi_controller.so+0x5d8c9) (BuildId: 6464b5c753e86bc8fa48c1a26a3f20eee21259fb) 
     #4 0x741421ca506b in nav2_controller::ControllerServer::computeAndPublishVelocity() (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x2a506b) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #5 0x741421c96561 in nav2_controller::ControllerServer::computeControl() (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x296561) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #6 0x741421e1eec3 in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::work() (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41eec3) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #7 0x741421e1e234 in std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::operator()() const (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41e234) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #8 0x741421e1df47 in std::enable_if<is_invocable_r_v<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>, std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>&>, std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> >::type std::__invoke_r<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>, std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>&>(std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41df47) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #9 0x741421e1dd88 in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void> >::_M_invoke(std::_Any_data const&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41dd88) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #10 0x7414232f5b46 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*) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_util/lib/libnav2_util_core.so+0x7fb46) (BuildId: 074014a12aa30b6de43159e7eb335b66f35e5343) 
     #11 0x741421099ee7 in __pthread_once_slow nptl/./nptl/pthread_once.c:116:7 
     #12 0x741421e1b951 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_M_run() (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41b951) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #13 0x7414214dc252  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc252) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2) 
     #14 0x741421094ac2 in start_thread nptl/./nptl/pthread_create.c:442:8 
     #15 0x74142112684f  misc/../sysdeps/unix/sysv/linux/x86_64/clone3.S:81 
  
 AddressSanitizer can not provide additional info. 
 SUMMARY: AddressSanitizer: SEGV (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so+0xc3880) (BuildId: ae00021bac019132f382130b42d6275d543c55a4) in nav2_costmap_2d::Costmap2D::worldToMap(double, double, unsigned int&, unsigned int&) const 
 Thread T23 created by T15 here: 
     #0 0x5a47bc68d87c in __interceptor_pthread_create (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/nav2_controller/controller_server+0x9387c) (BuildId: 3220191749087a73a7eee69dd741a727d5302202) 
     #1 0x7414214dc328 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+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2) 
     #2 0x741421e1b2b2 in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>::_Async_state_impl<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()&&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41b2b2) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #3 0x741421e1aa58 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>, std::allocator<void>, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()> >, void>*&, std::_Sp_alloc_shared_tag<std::allocator<void> >, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()&&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x41aa58) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #4 0x741421e18d03 in std::future<std::__invoke_result<std::decay<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>::type>::type> std::async<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()>(std::launch, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::'lambda'()&&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x418d03) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #5 0x741421e01e29 in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x401e29) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #6 0x741421e22ac7 in void std::__invoke_impl<void, void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> > >(std::__invoke_memfun_deref, void (nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >), nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >&&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x422ac7) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #7 0x741421e09986 in rclcpp_action::Server<nav2_msgs::action::FollowPath>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_s>, std::array<unsigned char, 16ul>, std::shared_ptr<void>) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x409986) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #8 0x741422fe8246 in rclcpp_action::ServerBase::execute_goal_request_received(std::shared_ptr<void>&) (/opt/ros/humble/lib/librclcpp_action.so+0x13246) (BuildId: 4dfcc4cee7010878193255b3a622d5194654caa8) 
  
 Thread T15 created by T0 here: 
     #0 0x5a47bc68d87c in __interceptor_pthread_create (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/nav2_controller/controller_server+0x9387c) (BuildId: 3220191749087a73a7eee69dd741a727d5302202) 
     #1 0x7414214dc328 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+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2) 
     #2 0x741421dfb2a5 in nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::SimpleActionServer<std::shared_ptr<nav2_util::LifecycleNode> >(std::shared_ptr<nav2_util::LifecycleNode>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::function<void ()>, std::function<void ()>, std::chrono::duration<long, std::ratio<1l, 1000l> >, bool, rcl_action_server_options_s const&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x3fb2a5) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #3 0x741421c94cc8 in std::__detail::_MakeUniq<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath> >::__single_object std::make_unique<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>, std::shared_ptr<nav2_util::LifecycleNode>, char const (&) [12], std::_Bind<void (nav2_controller::ControllerServer::* (nav2_controller::ControllerServer*))()>, std::nullptr_t, std::chrono::duration<long, std::ratio<1l, 1000l> >, bool>(std::shared_ptr<nav2_util::LifecycleNode>&&, char const (&) [12], std::_Bind<void (nav2_controller::ControllerServer::* (nav2_controller::ControllerServer*))()>&&, std::nullptr_t&&, std::chrono::duration<long, std::ratio<1l, 1000l> >&&, bool&&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x294cc8) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #4 0x741421c8a121 in nav2_controller::ControllerServer::on_configure(rclcpp_lifecycle::State const&) (/home/goes/ROS_Workstation/nav2_humble_experiment/install/nav2_controller/lib/libcontroller_server_core.so+0x28a121) (BuildId: f6be86e576eea5a4c9c0de506e472b69f7a05ca9) 
     #5 0x741422de58ec  (/opt/ros/humble/lib/librclcpp_lifecycle.so+0x288ec) (BuildId: 97f6428dc1ee45fd402b522b3b8e6b4fcfeabe76) 
  
 ==605052==ABORTING 
```

#### Additional information


----

##### It's a shutdown-issue
First, based on my execution logs, I can confirm this is a shutdown issue.

##### the casue analysis
Below is an analysis of the cause of this bug:

The action_server_ binds the `computeControl()` function as a callback function:

https://github.com/ros-navigation/navigation2/blob/635880d3d6ad1f23eb75d793409189c653797f58/nav2_controller/src/controller_server.cpp#L228-L234

Therefore, when the `action_server_` receives a task command, it will invoke the function path `computeControl()` -> `computeAndPublishVelocity()` -> `mppi::PathHandler::transformPath()` -> .......

NOTICE: from here, the `action_server_` access the internal memory of `nav2_mppi_controller` , during the `computeControl()` execution. 

So that, the bug seems to be caused because `action_server_` is still executing but `mppi` has been freed.

	action_server_ = std::make_unique<ActionServer>(
	shared_from_this(),
	"follow_path",
	std::bind(&ControllerServer::computeControl, this),
	nullptr,
	std::chrono::milliseconds(500),
	true /spin thread/, server_options, use_realtime_priority_ /soft realtime/);

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

NullPtr bug during the `computeVelocityCommands()` during shutdown #4436

Bug report

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

It's a shutdown-issue

the casue analysis

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

NullPtr bug during the computeVelocityCommands() during shutdown #4436

Description

Bug report

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

It's a shutdown-issue

the casue analysis

Metadata

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Issue actions

NullPtr bug during the `computeVelocityCommands()` during shutdown #4436