diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index be7cf77d8ca..562af9f128b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -66,7 +66,6 @@ class AmclNode : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't // respond until we're in the active state diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index e4e4c37e9f1..d31adf34718 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -389,13 +389,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -AmclNode::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 76e63c7d5da..5c87558a2ed 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -82,11 +82,6 @@ class BtNavigator : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::NavigateToPose; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 69bee0ff5f5..eb151d8a212 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -222,13 +222,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -BtNavigator::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 3d4e7be277e..3180b2c1440 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -102,12 +102,6 @@ class ControllerServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in Error state - * @param state LifeCycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::FollowPath; using ActionServer = nav2_util::SimpleActionServer; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 6527b71e829..ffad06c55d9 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -228,13 +228,6 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -ControllerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn ControllerServer::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5524f5737d3..541a246a513 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -97,7 +97,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Subscribes to sensor topics if necessary and starts costmap diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 75c7552628b..21374d28e2b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -274,13 +274,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -Costmap2DROS::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index ef3b7849603..9c9b57d2928 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -86,12 +86,6 @@ class MapSaver : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Map saving service callback diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index d3ace49e90a..700b1753ea2 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -77,12 +77,6 @@ class MapServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Load the map YAML, image from map file name and diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 34107978afe..8d23ad8233e 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -95,13 +95,6 @@ MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapSaver::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 490049e4b9e..2e2e9453ab2 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -160,13 +160,6 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 3e67e6d089e..c1043ed0945 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -89,12 +89,6 @@ class PlannerServer : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using ActionServer = nav2_util::SimpleActionServer; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6e42c131b79..fc8b4ab9b61 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -197,13 +197,6 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -PlannerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 1c02fc1f598..bb9efa571c1 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -46,7 +46,6 @@ class RecoveryServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; std::shared_ptr tf_; std::shared_ptr transform_listener_; diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 8fab3b6e18e..3f108edb438 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -169,13 +169,6 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -RecoveryServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index cb48feeab6c..9180133817f 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -123,6 +123,14 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecycleNode::shared_from_this()); } + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_FATAL( + get_logger(), + "Lifecycle node %s does not have error state implemented", get_name()); + return nav2_util::CallbackReturn::SUCCESS; + } + // bond connection to lifecycle manager void createBond(); void destroyBond(); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 98d85f932b2..b24147fb9b8 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -92,11 +92,6 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Action server callbacks diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 69407589253..d216c82b47a 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -101,13 +101,6 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -WaypointFollower::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) {