fix: handle transition failures in planner/controller/smoother servers

Signed-off-by: Kemal Bektas <[email protected]>
ros-navigation · Sep 30, 2024 · 0ae7c08 · 0ae7c08
1 parent 0ffc0fb
commit 0ae7c08
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 8 deletions.
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;
     }
   }
@@ -254,7 +257,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+ControllerServer::on_activate(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
@@ -265,6 +268,15 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
   ControllerMap::iterator it;
   for (it = controllers_.begin(); it != controllers_.end(); ++it) {
     it->second->activate();
+    try {
+      it->second->activate();
+    } catch (const std::exception & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to activate controller. Exception: %s",
+        ex.what());
+      on_deactivate(state);
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
   vel_publisher_->on_activate();
   action_server_->activate();

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;
     }
   }
@@ -177,7 +178,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+PlannerServer::on_activate(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
@@ -191,7 +192,15 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {
-    it->second->activate();
+    try {
+      it->second->activate();
+    } catch (const std::exception & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to activate global planner. Exception: %s",
+        ex.what());
+      on_deactivate(state);
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
 
   auto node = shared_from_this();

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,14 +163,22 @@ 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");
 
   plan_publisher_->on_activate();
   SmootherMap::iterator it;
   for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
-    it->second->activate();
+    try {
+      it->second->activate();
+    } catch (const std::exception & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to activate smoother. Exception: %s",
+        ex.what());
+      on_deactivate(state);
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
   action_server_->activate();