Expections (#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="service_name">Service 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_;