Controller exceptions

nav2_constrained_smoother/src/constrained_smoother.cpp

@@ -21,7 +21,7 @@
 #include <vector>
 
 #include "nav2_constrained_smoother/constrained_smoother.hpp"
-#include "nav2_core/exceptions.hpp"
+#include "nav2_core/planner_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 +138,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 nav2_core::PlannerException(
             "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,33 @@ void ControllerServer::computeControl()
           controller_frequency_);
       }
     }
-  } catch (nav2_core::PlannerException & e) {
-    RCLCPP_ERROR(this->get_logger(), e.what());
+  } catch (nav2_core::FailedToMakeProgress & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<nav2_msgs::action::FollowPath::Result> result;
+    result->error_code = nav2_msgs::action::FollowPath::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<nav2_msgs::action::FollowPath::Result> result;
+    result->error_code = nav2_msgs::action::FollowPath::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<nav2_msgs::action::FollowPath::Result> result;
+    result->error_code = nav2_msgs::action::FollowPath::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<nav2_msgs::action::FollowPath::Result> result;
+    result->error_code = nav2_msgs::action::FollowPath::Goal::UNKNOWN;
+    action_server_->terminate_current(result);
     return;
   }
 
@@ -426,7 +447,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 +467,11 @@ void ControllerServer::computeAndPublishVelocity()
   geometry_msgs::msg::PoseStamped pose;
 
   if (!getRobotPose(pose)) {
-    throw nav2_core::PlannerException("Failed to obtain robot pose");
+    throw nav2_core::ControllerException("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 +485,7 @@ void ControllerServer::computeAndPublishVelocity()
       nav_2d_utils::twist2Dto3D(twist),
       goal_checkers_[current_goal_checker_].get());
     last_valid_cmd_time_ = now();
-  } catch (nav2_core::PlannerException & e) {
+  } catch (nav2_core::ControllerException & 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 +499,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::ControllerException(e.what());
     }
   }
 

nav2_core/include/nav2_core/controller_exceptions.hpp

@@ -0,0 +1,60 @@
+// 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) {}
+};
+
+}  // 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_;
 };

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -44,11 +44,11 @@
 #include "dwb_msgs/msg/critic_score.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
 #include "nav_2d_utils/conversions.hpp"
-#include "nav_2d_utils/parameters.hpp"
 #include "nav_2d_utils/tf_help.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -133,7 +133,9 @@ void DWBLocalPlanner::configure(
     loadCritics();
   } catch (const std::exception & e) {
     RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what());
-    throw;
+    throw nav2_core::ControllerException(
+            "Couldn't load critics! Caught exception: " +
+            std::string(e.what()));
   }
 }
 
@@ -214,7 +216,9 @@ DWBLocalPlanner::loadCritics()
       plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_);
     } catch (const std::exception & e) {
       RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!");
-      throw;
+      throw nav2_core::ControllerException(
+              "Couldn't initialize critic plugin: " +
+              std::string(e.what()));
     }
     RCLCPP_INFO(logger_, "Critic plugin initialized");
   }
@@ -253,9 +257,15 @@ DWBLocalPlanner::computeVelocityCommands(
     geometry_msgs::msg::TwistStamped cmd_vel;
     cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity);
     return cmd_vel;
-  } catch (const nav2_core::PlannerException & e) {
+  } catch (const nav2_core::ControllerTFError & e) {
     pub_->publishEvaluation(results);
-    throw;
+    throw e;
+  } catch (const nav2_core::InvalidPath & e) {
+    pub_->publishEvaluation(results);
+    throw e;
+  } catch (const nav2_core::ControllerException & e) {
+    pub_->publishEvaluation(results);
+    throw e;
   }
 }
 
@@ -333,7 +343,9 @@ DWBLocalPlanner::computeVelocityCommands(
     pub_->publishLocalPlan(pose.header, empty_traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 
-    throw;
+    throw nav2_core::ControllerException(
+            "Could not find a legal trajectory: " +
+            std::string(e.what()));
   }
 }
 
@@ -441,7 +453,7 @@ DWBLocalPlanner::transformGlobalPlan(
   const nav_2d_msgs::msg::Pose2DStamped & pose)
 {
   if (global_plan_.poses.empty()) {
-    throw nav2_core::PlannerException("Received plan with zero length");
+    throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
   // let's get the pose of the robot in the frame of the plan
@@ -450,8 +462,8 @@ DWBLocalPlanner::transformGlobalPlan(
       tf_, global_plan_.header.frame_id, pose,
       robot_pose, transform_tolerance_))
   {
-    throw dwb_core::
-          PlannerTFException("Unable to transform robot pose into global plan's frame");
+    throw nav2_core::
+          ControllerTFError("Unable to transform robot pose into global plan's frame");
   }
 
   // we'll discard points on the plan that are outside the local costmap
@@ -535,7 +547,7 @@ DWBLocalPlanner::transformGlobalPlan(
   }
 
   if (transformed_plan.poses.empty()) {
-    throw nav2_core::PlannerException("Resulting plan has 0 poses in it.");
+    throw nav2_core::ControllerTFError("Resulting plan has 0 poses in it.");
   }
   return transformed_plan;
 }

nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp

@@ -49,7 +49,7 @@ const geometry_msgs::msg::Pose2D & getClosestPose(
   rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset);
   const unsigned int num_poses = trajectory.poses.size();
   if (num_poses == 0) {
-    throw nav2_core::PlannerException("Cannot call getClosestPose on empty trajectory.");
+    throw nav2_core::ControllerException("Cannot call getClosestPose on empty trajectory.");
   }
   unsigned int closest_index = num_poses;
   double closest_diff = 0.0;
@@ -73,7 +73,7 @@ geometry_msgs::msg::Pose2D projectPose(
   rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset);
   const unsigned int num_poses = trajectory.poses.size();
   if (num_poses == 0) {
-    throw nav2_core::PlannerException("Cannot call projectPose on empty trajectory.");
+    throw nav2_core::ControllerException("Cannot call projectPose on empty trajectory.");
   }
   if (goal_time <= (trajectory.time_offsets[0])) {
     return trajectory.poses[0];