From 6311978e0902b807a05883b3764e72f731d446f8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Oct 2024 19:06:49 -0700 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 | 4 +- nav2_controller/src/controller_server.cpp | 6 ++- .../opennav_docking/src/docking_server.cpp | 3 +- .../test/test_docking_server_unit.cpp | 50 ++++++++++++++++++- nav2_planner/src/planner_server.cpp | 3 +- nav2_smoother/src/nav2_smoother.cpp | 5 +- .../src/velocity_smoother.cpp | 49 ++++++++++++------ .../test/test_velocity_smoother.cpp | 15 +++--- .../src/waypoint_follower.cpp | 3 +- 12 files changed, 117 insertions(+), 33 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 1017bf595a..a04aea4031 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -78,7 +78,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"); @@ -91,6 +91,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; } setupResourcesForBehaviorPlugins(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9ae6401674..c49fae0e2b 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -56,7 +56,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"); @@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create navigator id %s." " Exception: %s", navigator_ids[i].c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -141,11 +142,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 d8e03e19ed..7f87bc1bb9 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"); @@ -60,6 +60,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 c0bd15cb63..2b0991c994 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -42,7 +42,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"); @@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + 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 ec9dc3d81b..c221c2db17 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -82,7 +82,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -152,6 +152,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -178,6 +179,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; } } @@ -206,6 +208,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; } } @@ -242,6 +245,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0d5034bd60..b350cb4026 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) } nav2_util::CallbackReturn -DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +DockingServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); auto node = shared_from_this(); @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 144a612f3b..f12741774c 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions) EXPECT_TRUE(false); } } + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, getateGoalDock) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); std::shared_ptr goal = std::make_shared(); auto dock = node->generateGoalDock(goal); - EXPECT_EQ(dock->plugin, nullptr); + EXPECT_NE(dock->plugin, nullptr); EXPECT_EQ(dock->frame, std::string()); node->stashDockData(false, dock, true); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, testDynamicParams) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); @@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } } // namespace opennav_docking diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index bb59b0b023..7233a7ba53 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -79,7 +79,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"); @@ -120,6 +120,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 e7baa5e483..fe462cb1c6 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -63,7 +63,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"); @@ -100,6 +100,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; } @@ -162,7 +163,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 535b6b08f2..9519ab94b6 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,24 +76,35 @@ 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] > 0.0) { - throw std::runtime_error( - "Positive values set of min_velocities! These should be negative!"); + RCLCPP_ERROR( + get_logger(), "Positive values set of min_velocities! These should be negative!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_velocities_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of max_velocities! These should be positive!"); + RCLCPP_ERROR( + get_logger(), "Negative values set of max_velocities! These should be positive!"); + 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; } } @@ -112,9 +123,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 @@ -124,7 +138,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 @@ -144,6 +162,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 25754d0f35..76c5e0498b 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -40,7 +40,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);} @@ -594,10 +597,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) @@ -613,13 +616,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 02a4158d28..0014e79a15 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -54,7 +54,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"); @@ -123,6 +123,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", e.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS;