Controller exceptions (#3227)

* added result codes for global planner

* code review

* code review

* cleanup

* cleanup

* update smac lattice planner

* update planner instances

* cleanup

* added controller exception

* renaming

* follow path updates

* rename exceptions

* updated regulated pure pursuit

* completed pure pursuit

* completed dwb

* linting fixes

* cleanup

* revert planner server

* revert planner server

* revert planner server

* revert planner server

* code review

* code review

* cleanup

* cleanup

* bug fix

* final cleanup

* set follow path error on bt

* update groot

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

Author: jwallace42

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
  */
 class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
 {
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::FollowPathAction
@@ -46,13 +50,28 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Function to perform some user-defined operation after a timeout
    * waiting for a result that hasn't been received yet
    * @param feedback shared_ptr to latest feedback message
    */
   void on_wait_for_result(
-    std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback) override;
+    std::shared_ptr<const Action::Feedback> feedback) override;
 
   /**
    * @brief Creates list of BT ports
@@ -65,6 +84,8 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "follow_path_error_code", "The follow path error code"),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -113,6 +113,7 @@
       <input_port name="goal_checker_id" default="GoalChecker">Goal checker</input_port>
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="follow_path_error_code">Follow Path error code</output_port>
     </Action>
 
     <Action ID="NavigateToPose">

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -24,7 +24,7 @@ FollowPathAction::FollowPathAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
+: BtActionNode<Action>(xml_tag_name, action_name, conf)
 {
 }
 
@@ -35,8 +35,27 @@ void FollowPathAction::on_tick()
   getInput("goal_checker_id", goal_.goal_checker_id);
 }
 
+BT::NodeStatus FollowPathAction::on_success()
+{
+  setOutput("follow_path_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus FollowPathAction::on_aborted()
+{
+  setOutput("follow_path_error_code", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus FollowPathAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 void FollowPathAction::on_wait_for_result(
-  std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback>/*feedback*/)
 {
   // Grab the new path
   nav_msgs::msg::Path new_path;

nav2_constrained_smoother/src/constrained_smoother.cpp

@@ -21,7 +21,6 @@
 #include <vector>
 
 #include "nav2_constrained_smoother/constrained_smoother.hpp"
-#include "nav2_core/exceptions.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
@@ -138,7 +137,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat
       logger_,
       "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
       plugin_name_.c_str());
-    throw new nav2_core::PlannerException(
+    throw std::runtime_error(
             "Failed to smooth plan, Ceres could not find a usable solution.");
   }
 

nav2_controller/plugins/simple_progress_checker.cpp

@@ -17,7 +17,6 @@
 #include <string>
 #include <memory>
 #include <vector>
-#include "nav2_core/exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"

nav2_controller/src/controller_server.cpp

@@ -19,7 +19,7 @@
 #include <utility>
 #include <limits>
 
-#include "nav2_core/exceptions.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "nav_2d_utils/tf_help.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -353,17 +353,15 @@ void ControllerServer::computeControl()
     if (findControllerId(c_name, current_controller)) {
       current_controller_ = current_controller;
     } else {
-      action_server_->terminate_current();
-      return;
+      throw nav2_core::ControllerException("Failed to find controller name: " + c_name);
     }
 
     std::string gc_name = action_server_->get_current_goal()->goal_checker_id;
     std::string current_goal_checker;
     if (findGoalCheckerId(gc_name, current_goal_checker)) {
       current_goal_checker_ = current_goal_checker;
     } else {
-      action_server_->terminate_current();
-      return;
+      throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name);
     }
 
     setPlannerPath(action_server_->get_current_goal()->path);
@@ -405,10 +403,47 @@ void ControllerServer::computeControl()
           controller_frequency_);
       }
     }
-  } catch (nav2_core::PlannerException & e) {
-    RCLCPP_ERROR(this->get_logger(), e.what());
+  } catch (nav2_core::ControllerTFError & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::TF_ERROR;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::NoValidControl & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::NO_VALID_CONTROL;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::FailedToMakeProgress & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::FAILED_TO_MAKE_PROGRESS;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::PatienceExceeded & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::PATIENCE_EXCEEDED;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::InvalidPath & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::INVALID_PATH;
+    action_server_->terminate_current(result);
+    return;
+  } catch (nav2_core::ControllerException & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
     publishZeroVelocity();
-    action_server_->terminate_current();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    result->error_code = Action::Goal::UNKNOWN;
+    action_server_->terminate_current(result);
     return;
   }
 
@@ -426,7 +461,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
     get_logger(),
     "Providing path to the controller %s", current_controller_.c_str());
   if (path.poses.empty()) {
-    throw nav2_core::PlannerException("Invalid path, Path is empty.");
+    throw nav2_core::InvalidPath("Path is empty.");
   }
   controllers_[current_controller_]->setPlan(path);
 
@@ -446,11 +481,11 @@ void ControllerServer::computeAndPublishVelocity()
   geometry_msgs::msg::PoseStamped pose;
 
   if (!getRobotPose(pose)) {
-    throw nav2_core::PlannerException("Failed to obtain robot pose");
+    throw nav2_core::ControllerTFError("Failed to obtain robot pose");
   }
 
   if (!progress_checker_->check(pose)) {
-    throw nav2_core::PlannerException("Failed to make progress");
+    throw nav2_core::FailedToMakeProgress("Failed to make progress");
   }
 
   nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
@@ -464,7 +499,9 @@ void ControllerServer::computeAndPublishVelocity()
       nav_2d_utils::twist2Dto3D(twist),
       goal_checkers_[current_goal_checker_].get());
     last_valid_cmd_time_ = now();
-  } catch (nav2_core::PlannerException & e) {
+    // Only no valid control exception types are valid to attempt to have control patience, as
+    // other types will not be resolved with more attempts
+  } catch (nav2_core::NoValidControl & e) {
     if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
       RCLCPP_WARN(this->get_logger(), e.what());
       cmd_vel_2d.twist.angular.x = 0;
@@ -478,10 +515,10 @@ void ControllerServer::computeAndPublishVelocity()
       if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
         failure_tolerance_ != -1.0)
       {
-        throw nav2_core::PlannerException("Controller patience exceeded");
+        throw nav2_core::PatienceExceeded("Controller patience exceeded");
       }
     } else {
-      throw nav2_core::PlannerException(e.what());
+      throw nav2_core::NoValidControl(e.what());
     }
   }
 

nav2_core/include/nav2_core/controller_exceptions.hpp

@@ -0,0 +1,67 @@
+// Copyright (c) 2022. Joshua Wallace
+//  Licensed under the Apache License, Version 2.0 (the "License");
+//  you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#ifndef NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_
+#define NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+
+namespace nav2_core
+{
+
+class ControllerException : public std::runtime_error
+{
+public:
+  explicit ControllerException(const std::string & description)
+  : std::runtime_error(description) {}
+};
+
+class ControllerTFError : public ControllerException
+{
+public:
+  explicit ControllerTFError(const std::string & description)
+  : ControllerException(description) {}
+};
+
+class FailedToMakeProgress : public ControllerException
+{
+public:
+  explicit FailedToMakeProgress(const std::string & description)
+  : ControllerException(description) {}
+};
+
+class PatienceExceeded : public ControllerException
+{
+public:
+  explicit PatienceExceeded(const std::string & description)
+  : ControllerException(description) {}
+};
+
+class InvalidPath : public ControllerException
+{
+public:
+  explicit InvalidPath(const std::string & description)
+  : ControllerException(description) {}
+};
+
+class NoValidControl : public ControllerException
+{
+public:
+  explicit NoValidControl(const std::string & description)
+  : ControllerException(description) {}
+};
+
+}  // namespace nav2_core
+
+#endif  // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_

nav2_core/include/nav2_core/exceptions.hpp renamed to nav2_core/include/nav2_core/planner_exceptions.hpp

@@ -33,8 +33,8 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV2_CORE__EXCEPTIONS_HPP_
-#define NAV2_CORE__EXCEPTIONS_HPP_
+#ifndef NAV2_CORE__PLANNER_EXCEPTIONS_HPP_
+#define NAV2_CORE__PLANNER_EXCEPTIONS_HPP_
 
 #include <stdexcept>
 #include <string>
@@ -101,4 +101,4 @@ class PlannerTFError : public PlannerException
 
 }  // namespace nav2_core
 
-#endif  // NAV2_CORE__EXCEPTIONS_HPP_
+#endif  // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_

nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp

@@ -34,35 +34,23 @@
 #ifndef DWB_CORE__EXCEPTIONS_HPP_
 #define DWB_CORE__EXCEPTIONS_HPP_
 
-#include <stdexcept>
 #include <string>
 #include <memory>
 
-#include "nav2_core/exceptions.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 namespace dwb_core
 {
 
-/**
- * @class PlannerTFException
- * @brief Thrown when the planner cannot complete its operation due to TF errors
- */
-class PlannerTFException : public nav2_core::PlannerException
-{
-public:
-  explicit PlannerTFException(const std::string description)
-  : nav2_core::PlannerException(description) {}
-};
-
 /**
  * @class IllegalTrajectoryException
  * @brief Thrown when one of the critics encountered a fatal error
  */
-class IllegalTrajectoryException : public nav2_core::PlannerException
+class IllegalTrajectoryException : public nav2_core::ControllerException
 {
 public:
-  IllegalTrajectoryException(const std::string critic_name, const std::string description)
-  : nav2_core::PlannerException(description), critic_name_(critic_name) {}
+  IllegalTrajectoryException(const std::string & critic_name, const std::string & description)
+  : nav2_core::ControllerException(description), critic_name_(critic_name) {}
   std::string getCriticName() const {return critic_name_;}
 
 protected:

nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp

@@ -39,7 +39,7 @@
 #include <utility>
 #include <string>
 #include "dwb_core/exceptions.hpp"
-#include "nav2_core/exceptions.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 namespace dwb_core
 {
@@ -66,11 +66,11 @@ class IllegalTrajectoryTracker
  * @brief Thrown when all the trajectories explored are illegal
  */
 class NoLegalTrajectoriesException
-  : public nav2_core::PlannerException
+  : public nav2_core::ControllerException
 {
 public:
   explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker & tracker)
-  : PlannerException(tracker.getMessage()),
+  : ControllerException(tracker.getMessage()),
     tracker_(tracker) {}
   IllegalTrajectoryTracker tracker_;
 };