fix: handle transition failures in all servers (ros-navigation#4708)

* fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
kiwicampus · Oct 8, 2024 · 6311978 · 6311978
1 parent 4e62a89
commit 6311978
Show file tree

Hide file tree

Showing 12 changed files with 117 additions and 33 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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<Navigator>(node);
   dock_db_ = std::make_unique<DockDatabase>(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
@@ -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<opennav_docking::DockingServer>();
+
+  // Setup 1 instance of the test failure dock & its plugin instance
+  node->declare_parameter(
+    "docks",
+    rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
+  node->declare_parameter(
+    "test_dock.type",
+    rclcpp::ParameterValue(std::string{"dock_plugin"}));
+  node->declare_parameter(
+    "test_dock.pose",
+    rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
+  node->declare_parameter(
+    "dock_plugins",
+    rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
+  node->declare_parameter(
+    "dock_plugin.plugin",
+    rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));
+
   node->on_configure(rclcpp_lifecycle::State());
   std::shared_ptr<const DockRobot::Goal> goal = std::make_shared<const DockRobot::Goal>();
   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<opennav_docking::DockingServer>();
+
+  // Setup 1 instance of the test failure dock & its plugin instance
+  node->declare_parameter(
+    "docks",
+    rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
+  node->declare_parameter(
+    "test_dock.type",
+    rclcpp::ParameterValue(std::string{"dock_plugin"}));
+  node->declare_parameter(
+    "test_dock.pose",
+    rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
+  node->declare_parameter(
+    "dock_plugins",
+    rclcpp::ParameterValue(std::vector<std::string>{"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
@@ -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
@@ -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
@@ -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<nav2_util::OdomSmoother>(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;
     }
   }