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_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 1b8f6517f66..e24b3b251e1 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -109,7 +109,7 @@ TEST_F(SpinActionTestFixture, test_ports) R"( - + )"; 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..64e076ae621 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -214,6 +214,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); bt_.reset(); @@ -222,13 +223,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..3b6ba3971fb 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -216,11 +216,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) // Release any allocated resources action_server_.reset(); - for (it = controllers_.begin(); it != controllers_.end(); ++it) { - it->second.reset(); - } odom_sub_.reset(); - vel_publisher_.reset(); action_server_.reset(); goal_checker_->reset(); @@ -228,13 +224,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_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp index 041ae88fbdb..2dd9c5bef89 100644 --- a/nav2_core/include/nav2_core/progress_checker.hpp +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -55,7 +55,7 @@ class ProgressChecker /** * @brief Reset class state upon calling */ - virtual void reset() {} + virtual void reset() = 0; }; } // namespace nav2_core 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/main_server.cpp b/nav2_map_server/src/map_saver/main_server.cpp index c7d2c695eba..320e13cc950 100644 --- a/nav2_map_server/src/map_saver/main_server.cpp +++ b/nav2_map_server/src/map_saver/main_server.cpp @@ -25,15 +25,8 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto logger = rclcpp::get_logger("map_saver_server"); - - try { - auto service_node = std::make_shared(); - rclcpp::spin(service_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(logger, ex.what()); - RCLCPP_ERROR(logger, "Exiting"); - return -1; - } + auto service_node = std::make_shared(); + rclcpp::spin(service_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; } diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 34107978afe..1bc50c512c2 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*/) { @@ -215,7 +208,6 @@ bool MapSaver::saveMapTopicToFile( return false; } - RCLCPP_ERROR(get_logger(), "This situation should never appear"); return false; } diff --git a/nav2_map_server/src/map_server/main.cpp b/nav2_map_server/src/map_server/main.cpp index 8e57cc09bf9..693b3b9a0b1 100644 --- a/nav2_map_server/src/map_server/main.cpp +++ b/nav2_map_server/src/map_server/main.cpp @@ -23,15 +23,8 @@ int main(int argc, char ** argv) { std::string node_name("map_server"); - try { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), ex.what()); - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), "Exiting"); - return -1; - } + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); } 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_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8025ff150dd..0954976bf4b 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -55,6 +55,8 @@ namespace nav2_navfn_planner // if the size of the environment does not change // +// Example usage: +/* int create_nav_plan_astar( COSTTYPE * costmap, int nx, int ny, @@ -100,7 +102,7 @@ create_nav_plan_astar( return len; } - +*/ // // create nav fn buffers 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/CMakeLists.txt b/nav2_util/CMakeLists.txt index 21be6cdfa95..930034f09cf 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(bondcpp REQUIRED) find_package(bond REQUIRED) +find_package(action_msgs REQUIRED) set(dependencies nav2_msgs @@ -32,6 +33,7 @@ set(dependencies rclcpp_lifecycle bondcpp bond + action_msgs ) nav2_package() 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_util/package.xml b/nav2_util/package.xml index 87f9086b9c2..8d5ccf865c6 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -28,6 +28,7 @@ rclcpp_lifecycle launch launch_testing_ament_cmake + action_msgs libboost-program-options @@ -37,6 +38,7 @@ launch launch_testing_ament_cmake std_srvs + action_msgs ament_cmake diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index b999a28b7c9..35043d5df2f 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -516,6 +516,31 @@ TEST_F(ActionTest, test_handle_goal_deactivated) SUCCEED(); } +TEST_F(ActionTest, test_handle_cancel) +{ + auto goal = Fibonacci::Goal(); + goal.order = 14000000; + + // Send the goal + auto future_goal_handle = node_->action_client_->async_send_goal(goal); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); + + // Cancel the goal + auto cancel_response = node_->action_client_->async_cancel_goal(future_goal_handle.get()); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + cancel_response), rclcpp::FutureReturnCode::SUCCESS); + + // Check cancelled + EXPECT_EQ(future_goal_handle.get()->get_status(), rclcpp_action::GoalStatus::STATUS_CANCELING); + + SUCCEED(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); 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*/) {