diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index f3eb1bd0f4..a29aacb5d5 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -107,7 +107,7 @@ def generate_launch_description(): load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ - SetParameter("use_sim_time", use_sim_time), + SetParameter('use_sim_time', use_sim_time), Node( condition=LaunchConfigurationEquals('map', ''), package='nav2_map_server', @@ -165,7 +165,7 @@ def generate_launch_description(): load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ - SetParameter("use_sim_time", use_sim_time), + SetParameter('use_sim_time', use_sim_time), LoadComposableNodes( target_container=container_name_full, condition=LaunchConfigurationEquals('map', ''), diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 4cc93be54c..f4a67e4d6e 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -108,7 +108,7 @@ def generate_launch_description(): load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ - SetParameter("use_sim_time", use_sim_time), + SetParameter('use_sim_time', use_sim_time), Node( package='nav2_controller', executable='controller_server', @@ -193,7 +193,7 @@ def generate_launch_description(): load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ - SetParameter("use_sim_time", use_sim_time), + SetParameter('use_sim_time', use_sim_time), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 6621778425..5bbbd43247 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -17,7 +17,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration @@ -81,7 +81,7 @@ def generate_launch_description(): start_map_server = GroupAction( actions=[ - SetParameter("use_sim_time", use_sim_time), + SetParameter('use_sim_time', use_sim_time), Node( package='nav2_map_server', executable='map_saver_server', diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 5861949a2c..05bd9da04b 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_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 8c40709be6..d09e0a580f 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -225,6 +225,11 @@ int main(int argc, char ** argv) "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback); rclcpp::spin(g_node->get_node_base_interface()); + + g_node.reset(); + pub_marked.reset(); + pub_unknown.reset(); + rclcpp::shutdown(); return 0; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index aa8983c153..6ef994e93a 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -299,6 +299,15 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) dyn_params_handler.reset(); + stop(); + + // Map thread stuff + map_update_thread_shutdown_ = true; + + if (map_update_thread_->joinable()) { + map_update_thread_->join(); + } + footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); @@ -306,12 +315,6 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) layer_pub->on_deactivate(); } - stop(); - - // Map thread stuff - map_update_thread_shutdown_ = true; - map_update_thread_->join(); - return nav2_util::CallbackReturn::SUCCESS; } @@ -575,18 +578,23 @@ void Costmap2DROS::stop() { stop_updates_ = true; - std::vector> * plugins = layered_costmap_->getPlugins(); - std::vector> * filters = layered_costmap_->getFilters(); - // unsubscribe from topics - for (std::vector>::iterator plugin = plugins->begin(); - plugin != plugins->end(); ++plugin) - { - (*plugin)->deactivate(); - } - for (std::vector>::iterator filter = filters->begin(); - filter != filters->end(); ++filter) - { - (*filter)->deactivate(); + + // layered_costmap_ is set only if on_configure has been called + if (layered_costmap_) { + std::vector> * plugins = layered_costmap_->getPlugins(); + std::vector> * filters = layered_costmap_->getFilters(); + + // unsubscribe from topics + for (std::vector>::iterator plugin = plugins->begin(); + plugin != plugins->end(); ++plugin) + { + (*plugin)->deactivate(); + } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); ++filter) + { + (*filter)->deactivate(); + } } initialized_ = false; stopped_ = true; 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 edd3454ffe..51c771323e 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 f18436f21f..53c3f1b134 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_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f58bdac31f..6b1d548171 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -25,6 +25,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" @@ -69,6 +70,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) PlannerServer::~PlannerServer() { + /* + * Backstop ensuring this state is destroyed, even if deactivate/cleanup are + * never called. + */ planners_.clear(); costmap_thread_.reset(); } @@ -194,7 +199,19 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) action_server_pose_->deactivate(); action_server_poses_->deactivate(); plan_publisher_->on_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(); + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -218,13 +235,24 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_poses_.reset(); plan_publisher_.reset(); tf_.reset(); - costmap_ros_->cleanup(); + + /* + * Double check whether something else transitioned it to INACTIVE + * already, e.g. the rcl preshutdown callback. + */ + if (costmap_ros_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + costmap_ros_->cleanup(); + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { it->second->cleanup(); } + planners_.clear(); + costmap_thread_.reset(); costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py index 476368f44d..2117e21a55 100644 --- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py +++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py @@ -22,10 +22,10 @@ """ from math import cos, sin -from nav2_simple_commander.line_iterator import LineIterator + +from geometry_msgs.msg import Point32, Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D -from geometry_msgs.msg import Polygon -from geometry_msgs.msg import Point32 +from nav2_simple_commander.line_iterator import LineIterator NO_INFORMATION = 255 LETHAL_OBSTACLE = 254 @@ -146,7 +146,7 @@ def worldToMapValidated(self, wx: float, wy: float): """ if self.costmap_ is None: raise ValueError( - "Costmap not specified, use setCostmap to specify the costmap first") + 'Costmap not specified, use setCostmap to specify the costmap first') return self.costmap_.worldToMapValidated(wx, wy) def pointCost(self, x: int, y: int): @@ -165,7 +165,7 @@ def pointCost(self, x: int, y: int): """ if self.costmap_ is None: raise ValueError( - "Costmap not specified, use setCostmap to specify the costmap first") + 'Costmap not specified, use setCostmap to specify the costmap first') return self.costmap_.getCostXY(x, y) def setCostmap(self, costmap: PyCostmap2D): diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py index db19d4180c..7fe50a11ec 100644 --- a/nav2_simple_commander/nav2_simple_commander/line_iterator.py +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -50,22 +50,22 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0): """ if type(x0) not in [int, float]: - raise TypeError("x0 must be a number (int or float)") + raise TypeError('x0 must be a number (int or float)') if type(y0) not in [int, float]: - raise TypeError("y0 must be a number (int or float)") + raise TypeError('y0 must be a number (int or float)') if type(x1) not in [int, float]: - raise TypeError("x1 must be a number (int or float)") + raise TypeError('x1 must be a number (int or float)') if type(y1) not in [int, float]: - raise TypeError("y1 must be a number (int or float)") + raise TypeError('y1 must be a number (int or float)') if type(step_size) not in [int, float]: - raise TypeError("step_size must be a number (int or float)") + raise TypeError('step_size must be a number (int or float)') if step_size <= 0: - raise ValueError("step_size must be a positive number") + raise ValueError('step_size must be a positive number') self.x0_ = x0 self.y0_ = y0 @@ -88,7 +88,7 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0): else: self.valid_ = False raise ValueError( - "Line has zero length (All 4 points have same coordinates)") + 'Line has zero length (All 4 points have same coordinates)') def isValid(self): """Check if line is valid.""" diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index a225766f03..8ffc4b653b 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -13,11 +13,11 @@ # limitations under the License. import unittest -from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker + +from geometry_msgs.msg import Point32, Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D +from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker from nav_msgs.msg import OccupancyGrid -from geometry_msgs.msg import Polygon -from geometry_msgs.msg import Point32 LETHAL_OBSTACLE = 254 diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 4c5e97420a..87c348ea70 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -12,8 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest from cmath import sqrt +import unittest + from nav2_simple_commander.line_iterator import LineIterator diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index edab4390ec..57294873ec 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -17,11 +17,10 @@ import time from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Path +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose, FollowPath, SmoothPath from nav2_simple_commander.robot_navigator import BasicNavigator - +from nav_msgs.msg import Path import rclpy -from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses, SmoothPath def main(argv=sys.argv[1:]): @@ -53,7 +52,7 @@ def main(argv=sys.argv[1:]): path.poses.append(goal_pose) path.poses.append(goal_pose1) - navigator._waitForNodeToActivate("controller_server") + navigator._waitForNodeToActivate('controller_server') follow_path = { 'unknown': FollowPath.Goal().UNKNOWN, 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, @@ -72,10 +71,10 @@ def main(argv=sys.argv[1:]): time.sleep(0.5) assert navigator.result_future.result().result.error_code == error_code, \ - "Follow path error code does not match" + 'Follow path error code does not match' else: - assert False, "Follow path was rejected" + assert False, 'Follow path was rejected' goal_pose = PoseStamped() goal_pose.header.frame_id = 'map' @@ -87,7 +86,7 @@ def main(argv=sys.argv[1:]): goal_pose.pose.orientation.z = 0.0 goal_pose.pose.orientation.w = 1.0 - navigator._waitForNodeToActivate("planner_server") + navigator._waitForNodeToActivate('planner_server') compute_path_to_pose = { 'unknown': ComputePathToPose.Goal().UNKNOWN, 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, @@ -101,7 +100,7 @@ def main(argv=sys.argv[1:]): for planner, error_code in compute_path_to_pose.items(): result = navigator._getPathImpl(initial_pose, goal_pose, planner) - assert result.error_code == error_code, "Compute path to pose error does not match" + assert result.error_code == error_code, 'Compute path to pose error does not match' # Check compute path through error codes goal_pose1 = goal_pose @@ -121,7 +120,7 @@ def main(argv=sys.argv[1:]): for planner, error_code in compute_path_through_poses.items(): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) - assert result.error_code == error_code, "Compute path through pose error does not match" + assert result.error_code == error_code, 'Compute path through pose error does not match' # Check compute path to pose error codes pose = PoseStamped() @@ -140,7 +139,7 @@ def main(argv=sys.argv[1:]): a_path.poses.append(pose) a_path.poses.append(pose1) - navigator._waitForNodeToActivate("smoother_server") + navigator._waitForNodeToActivate('smoother_server') smoother = { 'invalid_smoother': SmoothPath.Goal().INVALID_SMOOTHER, 'unknown': SmoothPath.Goal().UNKNOWN, @@ -152,7 +151,7 @@ def main(argv=sys.argv[1:]): for smoother, error_code in smoother.items(): result = navigator._smoothPathImpl(a_path, smoother) - assert result.error_code == error_code, "Smoother error does not match" + assert result.error_code == error_code, 'Smoother error does not match' navigator.lifecycleShutdown() rclpy.shutdown() diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index 8a2ceb8481..d70c56ac8e 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -19,9 +19,9 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess -from launch_testing.legacy import LaunchTestService -from launch_ros.actions import Node from launch.actions import GroupAction +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService def generate_launch_description(): diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 2e88a63a7a..796047677e 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -18,7 +18,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped -from nav2_msgs.action import FollowWaypoints, ComputePathToPose +from nav2_msgs.action import ComputePathToPose, FollowWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index d4ba168849..6da348c5f1 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 9868a38bbb..e09ae54b5d 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()); diff --git a/nav2_util/test/test_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index fd204df47f..07ef0177d7 100644 --- a/nav2_util/test/test_lifecycle_node.cpp +++ b/nav2_util/test/test_lifecycle_node.cpp @@ -47,3 +47,39 @@ TEST(LifecycleNode, MultipleRclcppNodesExitCleanly) std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); } + +TEST(LifecycleNode, OnPreshutdownCbFires) +{ + // Ensure the on_rcl_preshutdown_cb fires + + class MyNodeType : public nav2_util::LifecycleNode + { +public: + MyNodeType( + const std::string & node_name) + : nav2_util::LifecycleNode(node_name) {} + + bool fired = false; + +protected: + void on_rcl_preshutdown() override + { + fired = true; + + nav2_util::LifecycleNode::on_rcl_preshutdown(); + } + }; + + auto node = std::make_shared("test_node"); + + ASSERT_EQ(node->fired, false); + + rclcpp::shutdown(); + + ASSERT_EQ(node->fired, true); + + // Fire dtor to ensure nothing insane happens, e.g. exceptions. + node.reset(); + + SUCCEED(); +}