diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index f7610696f8..ad5954b6a4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/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 { + 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("path", "Path created by ComputePathThroughPoses node"), BT::InputPort>( "goals", "Destinations to plan through"), @@ -81,6 +84,9 @@ class ComputePathThroughPosesAction BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::OutputPort( + "compute_path_through_poses_error_code", "The compute path through poses error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index e980012cc4..1c7f0526df 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/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 { + 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("path", "Path created by ComputePathToPose node"), - BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathToPose node"), + BT::OutputPort( + "compute_path_to_pose_error_code", "The compute path to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 425875c24c..95e5a0917d 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -87,6 +87,8 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node + "The compute path through poses + pose error code" diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index c7421d5599..ffcbd328ae 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -41,6 +41,8 @@ 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; } @@ -48,6 +50,7 @@ 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; } @@ -55,6 +58,8 @@ 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; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index b5e0e9084c..68a909b52c 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/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(xml_tag_name, action_name, conf) +: BtActionNode(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; } diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 622ee0f0db..e607d98ccc 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/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_ diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index a1f609a9e0..c37297c69e 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/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 diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index ad4f71c3b1..fee711f974 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/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; using ActionServerThroughPoses = nav2_util::SimpleActionServer; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c7a90bcbbe..e597522430 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/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(); 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::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); } }