exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

@@ -32,6 +32,10 @@ namespace nav2_behavior_tree
 class ComputePathThroughPosesAction
   : public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
 {
+  using Action = nav2_msgs::action::ComputePathThroughPoses;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
@@ -72,7 +76,6 @@ class ComputePathThroughPosesAction
   {
     return providedBasicPorts(
       {
-        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
         BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
           "goals",
           "Destinations to plan through"),
@@ -81,6 +84,9 @@ class ComputePathThroughPosesAction
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
+        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "compute_path_through_poses_error_code", "The compute path through poses error code"),
       });
   }
 };

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

@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
  */
 class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
 {
+  using Action = nav2_msgs::action::ComputePathToPose;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
@@ -57,7 +61,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
   BT::NodeStatus on_aborted() override;
 
   /**
-   * @brief Function to perform some user-defined operation upon cancelation of the action
+   * @brief Function to perform some user-defined operation upon cancellation of the action
    */
   BT::NodeStatus on_cancelled() override;
 
@@ -74,15 +78,15 @@ 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"),
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
+        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "compute_path_to_pose_error_code", "The compute path to pose error code"),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -87,6 +87,8 @@
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
+      <output_port name="compute_path_through_poses_error_code">"The compute path through poses
+        pose error code"</output_port>
     </Action>
 
     <Action ID="RemovePassedGoals">

nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp

@@ -41,20 +41,25 @@ void ComputePathThroughPosesAction::on_tick()
 BT::NodeStatus ComputePathThroughPosesAction::on_success()
 {
   setOutput("path", result_.result->path);
+  // Set empty error code, action was successful
+  setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
+  setOutput("compute_path_through_poses_error_code", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
 BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
+  // Set empty error code, action was cancelled
+  setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -24,7 +24,7 @@ ComputePathToPoseAction::ComputePathToPoseAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
+: BtActionNode<Action>(xml_tag_name, action_name, conf)
 {
 }
 
@@ -41,8 +41,7 @@ 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);
+  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -59,8 +58,7 @@ 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);
+  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_core/include/nav2_core/planner_exceptions.hpp

@@ -99,6 +99,13 @@ class PlannerTFError : public PlannerException
   : PlannerException(description) {}
 };
 
+class NoViapointsGiven : public PlannerException
+{
+public:
+  explicit NoViapointsGiven(const std::string & description)
+  : PlannerException(description) {}
+};
+
 }  // namespace nav2_core
 
 #endif  // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_

nav2_msgs/action/ComputePathThroughPoses.action

@@ -1,3 +1,16 @@
+# 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
+int16 NO_VIAPOINTS_GIVEN=9
+
 #goal definition
 geometry_msgs/PoseStamped[] goals
 geometry_msgs/PoseStamped start
@@ -7,5 +20,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_planner/include/nav2_planner/planner_server.hpp

@@ -107,7 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   using ActionToPose = nav2_msgs::action::ComputePathToPose;
+  using ActionToPoseGoal = ActionToPose::Goal;
   using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
+  using ActionThroughPosesGoal = ActionThroughPoses::Goal;
   using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
   using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;
 

nav2_planner/src/planner_server.cpp

@@ -343,11 +343,13 @@ void PlannerServer::computePlanThroughPoses()
 
   auto start_time = steady_clock_.now();
 
-  // Initialize the ComputePathToPose goal and result
+  // Initialize the ComputePathThroughPoses goal and result
   auto goal = action_server_poses_->get_current_goal();
   auto result = std::make_shared<ActionThroughPoses::Result>();
   nav_msgs::msg::Path concat_path;
 
+  geometry_msgs::msg::PoseStamped curr_start, curr_goal;
+
   try {
     if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) {
       return;
@@ -357,11 +359,8 @@ void PlannerServer::computePlanThroughPoses()
 
     getPreemptedGoalIfRequested(action_server_poses_, goal);
 
-    if (goal->goals.size() == 0) {
-      RCLCPP_WARN(
-        get_logger(),
-        "Compute path through poses requested a plan with no viapoint poses, returning.");
-      action_server_poses_->terminate_current();
+    if (goal->goals.empty()) {
+      throw nav2_core::NoViapointsGiven("No viapoints given");
     }
 
     // Use start pose if provided otherwise use current robot pose
@@ -371,8 +370,6 @@ void PlannerServer::computePlanThroughPoses()
     }
 
     // Get consecutive paths through these points
-    std::vector<geometry_msgs::msg::PoseStamped>::iterator goal_iter;
-    geometry_msgs::msg::PoseStamped curr_start, curr_goal;
     for (unsigned int i = 0; i != goal->goals.size(); i++) {
       // Get starting point
       if (i == 0) {
@@ -415,13 +412,42 @@ void PlannerServer::computePlanThroughPoses()
     }
 
     action_server_poses_->succeeded_current(result);
+  } catch (nav2_core::StartOccupied & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::START_OCCUPIED;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::GoalOccupied & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::NoValidPathCouldBeFound & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::NO_VALID_PATH;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::PlannerTimedOut & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::TIMEOUT;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::StartOutsideMapBounds & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::GoalOutsideMapBounds & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::PlannerTFError & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::TF_ERROR;
+    action_server_poses_->terminate_current(result);
+  } catch (nav2_core::NoViapointsGiven & ex) {
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::NO_VIAPOINTS_GIVEN;
+    action_server_poses_->terminate_current(result);
   } catch (std::exception & ex) {
-    RCLCPP_WARN(
-      get_logger(),
-      "%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"",
-      goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x,
-      goal->goals.back().pose.position.y, ex.what());
-    action_server_poses_->terminate_current();
+    exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
+    result->error_code = ActionThroughPosesGoal::UNKNOWN;
+    action_server_poses_->terminate_current(result);
   }
 }
 
@@ -476,39 +502,38 @@ PlannerServer::computePlan()
         "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
         1 / max_planner_duration_, 1 / cycle_duration.seconds());
     }
-
     action_server_pose_->succeeded_current(result);
   } catch (nav2_core::StartOccupied & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED;
+    result->error_code = ActionToPoseGoal::START_OCCUPIED;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::GoalOccupied & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED;
+    result->error_code = ActionToPoseGoal::GOAL_OCCUPIED;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::NoValidPathCouldBeFound & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH;
+    result->error_code = ActionToPoseGoal::NO_VALID_PATH;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::PlannerTimedOut & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT;
+    result->error_code = ActionToPoseGoal::TIMEOUT;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::StartOutsideMapBounds & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP;
+    result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::GoalOutsideMapBounds & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP;
+    result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP;
     action_server_pose_->terminate_current(result);
   } catch (nav2_core::PlannerTFError & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR;
+    result->error_code = ActionToPoseGoal::TF_ERROR;
     action_server_pose_->terminate_current(result);
   } catch (std::exception & ex) {
     exceptionWarning(start, goal->goal, goal->planner_id, ex);
-    result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN;
+    result->error_code = ActionToPoseGoal::UNKNOWN;
     action_server_pose_->terminate_current(result);
   }
 }