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*/)
{