From 93a501ac4b0da00ac827c4e5ee2326ba0955bbdf Mon Sep 17 00:00:00 2001 From: RBT22 Date: Fri, 11 Oct 2024 09:42:12 +0200 Subject: [PATCH] fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas * adding support for rest of servers + review comments Signed-off-by: Steve Macenski * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski * fix vel smoother unit tests Signed-off-by: Steve Macenski * fixing docking server unit testing Signed-off-by: Steve Macenski * fixing last bits Signed-off-by: Steve Macenski --------- Signed-off-by: Kemal Bektas Signed-off-by: Steve Macenski Co-authored-by: Kemal Bektas --- nav2_behaviors/src/behavior_server.cpp | 3 +- nav2_bt_navigator/src/bt_navigator.cpp | 6 ++-- .../src/collision_detector_node.cpp | 3 +- .../src/collision_monitor_node.cpp | 3 +- nav2_controller/src/controller_server.cpp | 4 ++- nav2_planner/src/planner_server.cpp | 3 +- nav2_smoother/src/nav2_smoother.cpp | 5 +-- .../src/velocity_smoother.cpp | 36 +++++++++++++------ .../test/test_velocity_smoother.cpp | 15 ++++---- .../src/waypoint_follower.cpp | 3 +- 10 files changed, 54 insertions(+), 27 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index d6f7568c400..9e23df9720e 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -65,7 +65,7 @@ BehaviorServer::~BehaviorServer() } nav2_util::CallbackReturn -BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -92,6 +92,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9a64342a648..c50b62c9bff 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -103,7 +103,7 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -170,6 +170,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) get_logger(), "Failed to create navigator id %s of type %s." " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -183,11 +184,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); for (size_t i = 0; i != navigators_.size(); i++) { if (!navigators_[i]->on_activate()) { + on_deactivate(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 2d6ff099a34..d1e1a6c9dcc 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector() } nav2_util::CallbackReturn -CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -57,6 +57,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 390220d8fee..21569b96673 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -41,7 +41,7 @@ CollisionMonitor::~CollisionMonitor() } nav2_util::CallbackReturn -CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -58,6 +58,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 6705b386b76..8467aa2b3e0 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -77,7 +77,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring controller interface"); @@ -160,6 +160,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create goal checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -188,6 +189,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9e41863ccda..c1950ebc33a 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -76,7 +76,7 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -111,6 +111,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 219204e05ff..cebf9ac2773 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -61,7 +61,7 @@ SmootherServer::~SmootherServer() } nav2_util::CallbackReturn -SmootherServer::on_configure(const rclcpp_lifecycle::State &) +SmootherServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring smoother server"); @@ -98,6 +98,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) *costmap_sub_, *footprint_sub_, this->get_name()); if (!loadSmootherPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -155,7 +156,7 @@ bool SmootherServer::loadSmootherPlugins() } nav2_util::CallbackReturn -SmootherServer::on_activate(const rclcpp_lifecycle::State &) +SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index cb2ca199169..54e6f048806 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother() } nav2_util::CallbackReturn -VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) +VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring velocity smoother"); auto node = shared_from_this(); @@ -76,16 +76,23 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of deceleration! These should be negative to slow down!"); + RCLCPP_ERROR( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_accels_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of acceleration! These should be positive to speed up!"); + RCLCPP_ERROR( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > max_velocities_[i]) { - throw std::runtime_error( - "Min velocities are higher than max velocities!"); + RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } } @@ -104,9 +111,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) { - throw std::runtime_error( - "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 representing (x, y, theta)."); + RCLCPP_ERROR( + get_logger(), + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 representing (x, y, theta)."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Get control type @@ -116,7 +126,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) open_loop_ = false; odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); } else { - throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + RCLCPP_ERROR( + get_logger(), + "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Setup inputs / outputs diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index c58e20b22c4..4380b10b498 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -39,7 +39,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother public: VelSmootherShim() : VelocitySmoother() {} - void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + nav2_util::CallbackReturn configure(const rclcpp_lifecycle::State & state) + { + return this->on_configure(state); + } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} @@ -587,10 +590,10 @@ TEST(VelocitySmootherTest, testInvalidParams) std::vector max_vels{0.0, 0.0}; // invalid size smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) @@ -606,13 +609,13 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testDynamicParameter) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2bb95fea7dd..429956087c6 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -49,7 +49,7 @@ WaypointFollower::~WaypointFollower() } nav2_util::CallbackReturn -WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -92,6 +92,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", ex.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS;