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

* fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <[email protected]> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <[email protected]> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <[email protected]> * fix vel smoother unit tests Signed-off-by: Steve Macenski <[email protected]> * fixing docking server unit testing Signed-off-by: Steve Macenski <[email protected]> * fixing last bits Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Kemal Bektas <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Kemal Bektas <[email protected]>
EnjoyRobotics · Oct 11, 2024 · 93a501a · 93a501a
1 parent edaf470
commit 93a501a
Show file tree

Hide file tree

Showing 10 changed files with 54 additions and 27 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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<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

diff --git 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<double> 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
@@ -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;