diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 5861949a2c3..05bd9da04b5 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" @@ -246,7 +247,19 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->deactivate(); } - costmap_ros_->deactivate(); + + /* + * The costmap is also a lifecycle node, so it may have already fired on_deactivate + * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire + * in the order added, the preshutdown callbacks clearly don't per se, due to using an + * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger + * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 + */ + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + costmap_ros_->deactivate(); + } publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -271,11 +284,16 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) controllers_.clear(); goal_checkers_.clear(); - costmap_ros_->cleanup(); + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + costmap_ros_->cleanup(); + } // Release any allocated resources action_server_.reset(); odom_sub_.reset(); + costmap_thread_.reset(); vel_publisher_.reset(); speed_limit_sub_.reset(); diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index edd3454ffe6..51c771323eb 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -114,6 +114,13 @@ class LifecycleManager : public rclcpp::Node */ bool resume(); + /** + * @brief Perform preshutdown activities before our Context is shutdown. + * Note that this is related to our Context's shutdown sequence, not the + * lifecycle node state machine or shutdown(). + */ + void onRclPreshutdown(); + // Support function for creating service clients /** * @brief Support function for creating service clients @@ -186,6 +193,14 @@ class LifecycleManager : public rclcpp::Node */ void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + /** + * Register our preshutdown callback for this Node's rcl Context. + * The callback fires before this Node's Context is shutdown. + * Note this is not directly related to the lifecycle state machine or the + * shutdown() instance function. + */ + void registerRclPreshutdownCallback(); + // Timer thread to look at bond connections rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::TimerBase::SharedPtr bond_timer_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 2cbe22768c0..ed34d49e38a 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -45,6 +45,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("bond_respawn_max_duration", 10.0); declare_parameter("attempt_respawn_reconnection", true); + registerRclPreshutdownCallback(); + node_names_ = get_parameter("node_names").as_string_array(); get_parameter("autostart", autostart_); double bond_timeout_s; @@ -378,6 +380,35 @@ LifecycleManager::destroyBondTimer() } } +void +LifecycleManager::onRclPreshutdown() +{ + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleManager rcl preshutdown (%s)", + this->get_name()); + + destroyBondTimer(); + + /* + * Dropping the bond map is what we really need here, but we drop the others + * to prevent the bond map being used. Likewise, squash the service thread. + */ + service_thread_.reset(); + node_names_.clear(); + node_map_.clear(); + bond_map_.clear(); +} + +void +LifecycleManager::registerRclPreshutdownCallback() +{ + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + + context->add_pre_shutdown_callback( + std::bind(&LifecycleManager::onRclPreshutdown, this) + ); +} + void LifecycleManager::checkBondConnections() { diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index d4ba1688499..6da348c5f1a 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -167,6 +167,13 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } + /** + * @brief Perform preshutdown activities before our Context is shutdown. + * Note that this is related to our Context's shutdown sequence, not the + * lifecycle node state machine. + */ + virtual void on_rcl_preshutdown(); + /** * @brief Create bond connection to lifecycle manager */ @@ -183,6 +190,19 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode */ void printLifecycleNodeNotification(); + /** + * Register our preshutdown callback for this Node's rcl Context. + * The callback fires before this Node's Context is shutdown. + * Note this is not directly related to the lifecycle state machine. + */ + void register_rcl_preshutdown_callback(); + std::unique_ptr rcl_preshutdown_cb_handle_{nullptr}; + + /** + * Run some common cleanup steps shared between rcl preshutdown and destruction. + */ + void runCleanups(); + // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; }; diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 9868a38bbbe..e09ae54b5d6 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -36,17 +36,20 @@ LifecycleNode::LifecycleNode( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); printLifecycleNodeNotification(); + + register_rcl_preshutdown_callback(); } LifecycleNode::~LifecycleNode() { RCLCPP_INFO(get_logger(), "Destroying"); - // In case this lifecycle node wasn't properly shut down, do it here - if (get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - on_deactivate(get_current_state()); - on_cleanup(get_current_state()); + + runCleanups(); + + if (rcl_preshutdown_cb_handle_) { + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + context->remove_pre_shutdown_callback(*(rcl_preshutdown_cb_handle_.get())); + rcl_preshutdown_cb_handle_.reset(); } } @@ -64,6 +67,47 @@ void LifecycleNode::createBond() bond_->start(); } +void LifecycleNode::runCleanups() +{ + /* + * In case this lifecycle node wasn't properly shut down, do it here. + * We will give the user some ability to clean up properly here, but it's + * best effort; i.e. we aren't trying to account for all possible states. + */ + if (get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + this->deactivate(); + } + + if (get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + this->cleanup(); + } +} + +void LifecycleNode::on_rcl_preshutdown() +{ + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); +} + +void LifecycleNode::register_rcl_preshutdown_callback() +{ + rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context(); + + rcl_preshutdown_cb_handle_ = std::make_unique( + context->add_pre_shutdown_callback( + std::bind(&LifecycleNode::on_rcl_preshutdown, this)) + ); +} + void LifecycleNode::destroyBond() { RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());