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>

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 → 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_;
 };