Expections (ros-navigation#3244)

* added result codes for global planner

* code review

* code review

* cleanup

* cleanup

* update smac lattice planner

* update planner instances

* cleanup

* updates

* renaming

* fixes

* cpplint

* uncrusitfy

* code review

* navfn exceptions

* theta_star_planner

* fix code review

* wrote timeout exception

* consistent exception throwing across planners

* code review

* remove

* uncrusitfy

* uncrusify

* catch exception

* expect throw

* update string of exceptions

* throw with coords

* removed start == goal error code

* code review

* code review

* uncrustify

* code review

* message order

* remove remarks

* update xml

* update xml

* Update nav2_behavior_tree/nav2_tree_nodes.xml

* fix

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Joshua Wallace <josho.wallace.com>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -75,6 +75,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
     return providedBasicPorts(
       {
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+        BT::OutputPort<nav2_msgs::action::ComputePathToPose::Result::_error_code_type>(
+          "compute_path_to_pose_error_code", "The compute path to pose error code"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>(
           "start", "Start pose of the path if overriding current robot pose"),

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -77,6 +77,7 @@
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
+      <output_port name="compute_path_to_pose_error_code">"The compute path to pose error code"</output_port>
     </Action>
 
     <Action ID="ComputePathThroughPoses">

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -40,20 +40,27 @@ void ComputePathToPoseAction::on_tick()
 BT::NodeStatus ComputePathToPoseAction::on_success()
 {
   setOutput("path", result_.result->path);
+  // Set empty error code, action was successful
+  result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE;
+  setOutput("compute_path_to_pose_error_code", result_.result->error_code);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus ComputePathToPoseAction::on_aborted()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
+  setOutput("compute_path_to_pose_error_code", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
 BT::NodeStatus ComputePathToPoseAction::on_cancelled()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
+  // Set empty error code, action was cancelled
+  result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE;
+  setOutput("compute_path_to_pose_error_code", result_.result->error_code);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_core/include/nav2_core/exceptions.hpp

@@ -46,9 +46,57 @@ namespace nav2_core
 class PlannerException : public std::runtime_error
 {
 public:
-  explicit PlannerException(const std::string description)
+  explicit PlannerException(const std::string & description)
   : std::runtime_error(description) {}
-  using Ptr = std::shared_ptr<PlannerException>;
+};
+
+class StartOccupied : public PlannerException
+{
+public:
+  explicit StartOccupied(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class GoalOccupied : public PlannerException
+{
+public:
+  explicit GoalOccupied(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class StartOutsideMapBounds : public PlannerException
+{
+public:
+  explicit StartOutsideMapBounds(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class GoalOutsideMapBounds : public PlannerException
+{
+public:
+  explicit GoalOutsideMapBounds(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class NoValidPathCouldBeFound : public PlannerException
+{
+public:
+  explicit NoValidPathCouldBeFound(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class PlannerTimedOut : public PlannerException
+{
+public:
+  explicit PlannerTimedOut(const std::string & description)
+  : PlannerException(description) {}
+};
+
+class PlannerTFError : public PlannerException
+{
+public:
+  explicit PlannerTFError(const std::string & description)
+  : PlannerException(description) {}
 };
 
 }  // namespace nav2_core

nav2_msgs/action/ComputePathToPose.action

@@ -1,3 +1,15 @@
+# Error codes
+# Note: The expected priority order of the errors should match the message order
+int16 NONE=0
+int16 UNKNOWN=1
+int16 TF_ERROR=2
+int16 START_OUTSIDE_MAP=3
+int16 GOAL_OUTSIDE_MAP=4
+int16 START_OCCUPIED=5
+int16 GOAL_OCCUPIED=6
+int16 TIMEOUT=7
+int16 NO_VALID_PATH=8
+
 #goal definition
 geometry_msgs/PoseStamped goal
 geometry_msgs/PoseStamped start
@@ -7,5 +19,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st
 #result definition
 nav_msgs/Path path
 builtin_interfaces/Duration planning_time
+int16 error_code
 ---
 #feedback definition

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -25,6 +25,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/global_planner.hpp"
+#include "nav2_core/exceptions.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_navfn_planner/navfn.hpp"
 #include "nav2_util/robot_utils.hpp"

nav2_navfn_planner/src/navfn_planner.cpp

@@ -134,6 +134,30 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
 #ifdef BENCHMARK_TESTING
   steady_clock::time_point a = steady_clock::now();
 #endif
+  unsigned int mx_start, my_start, mx_goal, my_goal;
+  if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) {
+    throw nav2_core::StartOutsideMapBounds(
+            "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
+            std::to_string(start.pose.position.y) + ") was outside bounds");
+  }
+
+  if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) {
+    throw nav2_core::GoalOutsideMapBounds(
+            "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
+            std::to_string(goal.pose.position.y) + ") was outside bounds");
+  }
+
+  if (costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+    throw nav2_core::StartOccupied(
+            "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
+            std::to_string(start.pose.position.y) + ") was in lethal cost");
+  }
+
+  if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+    throw nav2_core::GoalOccupied(
+            "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
+            std::to_string(goal.pose.position.y) + ") was in lethal cost");
+  }
 
   // Update planner based on the new costmap size
   if (isPlannerOutOfDate()) {
@@ -148,12 +172,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
   if (start.pose.position.x == goal.pose.position.x &&
     start.pose.position.y == goal.pose.position.y)
   {
-    unsigned int mx, my;
-    costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
-    if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) {
-      RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
-      return path;
-    }
     path.header.stamp = clock_->now();
     path.header.frame_id = global_frame_;
     geometry_msgs::msg::PoseStamped pose;
@@ -172,9 +190,8 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
   }
 
   if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
-    RCLCPP_WARN(
-      logger_, "%s: failed to create plan with "
-      "tolerance %.2f.", name_.c_str(), tolerance_);
+    throw nav2_core::NoValidPathCouldBeFound(
+            "Failed to create plan with tolerance of: " + std::to_string(tolerance_) );
   }
 
 
@@ -221,14 +238,7 @@ NavfnPlanner::makePlan(
     start.position.x, start.position.y, goal.position.x, goal.position.y);
 
   unsigned int mx, my;
-  if (!worldToMap(wx, wy, mx, my)) {
-    RCLCPP_WARN(
-      logger_,
-      "Cannot create a plan: the robot's start position is off the global"
-      " costmap. Planning will always fail, are you sure"
-      " the robot has been properly localized?");
-    return false;
-  }
+  worldToMap(wx, wy, mx, my);
 
   // clear the starting cell within the costmap because we know it can't be an obstacle
   clearRobotCell(mx, my);
@@ -251,14 +261,7 @@ NavfnPlanner::makePlan(
   wx = goal.position.x;
   wy = goal.position.y;
 
-  if (!worldToMap(wx, wy, mx, my)) {
-    RCLCPP_WARN(
-      logger_,
-      "The goal sent to the planner is off the global costmap."
-      " Planning will always fail to this goal.");
-    return false;
-  }
-
+  worldToMap(wx, wy, mx, my);
   int map_goal[2];
   map_goal[0] = mx;
   map_goal[1] = my;
@@ -385,13 +388,7 @@ NavfnPlanner::getPlanFromPotential(
 
   // the potential has already been computed, so we won't update our copy of the costmap
   unsigned int mx, my;
-  if (!worldToMap(wx, wy, mx, my)) {
-    RCLCPP_WARN(
-      logger_,
-      "The goal sent to the navfn planner is off the global costmap."
-      " Planning will always fail to this goal.");
-    return false;
-  }
+  worldToMap(wx, wy, mx, my);
 
   int map_goal[2];
   map_goal[0] = mx;

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -39,6 +39,7 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_core/global_planner.hpp"
 #include "nav2_msgs/srv/is_path_valid.hpp"
+#include "nav2_core/exceptions.hpp"
 
 namespace nav2_planner
 {
@@ -152,21 +153,17 @@ class PlannerServer : public nav2_util::LifecycleNode
    */
   template<typename T>
   bool getStartPose(
-    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
     typename std::shared_ptr<const typename T::Goal> goal,
     geometry_msgs::msg::PoseStamped & start);
 
   /**
    * @brief Transform start and goal poses into the costmap
    * global frame for path planning plugins to utilize
-   * @param action_server Action server to terminate if required
    * @param start The starting pose to transform
    * @param goal Goal pose to transform
    * @return bool If successful in transforming poses
    */
-  template<typename T>
   bool transformPosesToGlobalFrame(
-    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
     geometry_msgs::msg::PoseStamped & curr_start,
     geometry_msgs::msg::PoseStamped & curr_goal);
 
@@ -180,15 +177,10 @@ class PlannerServer : public nav2_util::LifecycleNode
    */
   template<typename T>
   bool validatePath(
-    std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
     const geometry_msgs::msg::PoseStamped & curr_goal,
     const nav_msgs::msg::Path & path,
     const std::string & planner_id);
 
-  // Our action server implements the ComputePathToPose action
-  std::unique_ptr<ActionServerToPose> action_server_pose_;
-  std::unique_ptr<ActionServerThroughPoses> action_server_poses_;
-
   /**
    * @brief The action server callback which calls planner to get the path
    * ComputePathToPose
@@ -216,13 +208,23 @@ class PlannerServer : public nav2_util::LifecycleNode
    */
   void publishPlan(const nav_msgs::msg::Path & path);
 
+  void exceptionWarning(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal,
+    const std::string & planner_id,
+    const std::exception & ex);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
    */
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
+  // Our action server implements the ComputePathToPose action
+  std::unique_ptr<ActionServerToPose> action_server_pose_;
+  std::unique_ptr<ActionServerThroughPoses> action_server_poses_;
+
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::mutex dynamic_params_lock_;