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

@@ -336,11 +336,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;
@@ -350,11 +352,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
@@ -364,8 +363,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) {
@@ -408,13 +405,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);
  }
 }
 
@@ -469,39 +495,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);
  }
 }