diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index fee711f974a..282ca366f73 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -189,6 +189,13 @@ class PlannerServer : public nav2_util::LifecycleNode */ void computePlan(); + /** + * @brief Perform destruction activity also performed by rcl preshutdown. + */ + void prepDestruction(); + + void on_rcl_preshutdown() override; + /** * @brief The action server callback which calls planner to get the path * ComputePathThroughPoses diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f58bdac31fd..c71aba00b1e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -67,12 +67,25 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) costmap_thread_ = std::make_unique(costmap_ros_); } -PlannerServer::~PlannerServer() +void +PlannerServer::prepDestruction() { planners_.clear(); costmap_thread_.reset(); } +PlannerServer::~PlannerServer() +{ + prepDestruction(); +} + + +void PlannerServer::on_rcl_preshutdown() +{ + prepDestruction(); + LifecycleNode::on_rcl_preshutdown(); +} + nav2_util::CallbackReturn PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) {