From b1105160dcd64b10efc666e048eb982953927cc5 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 14 Oct 2022 16:06:22 -0400 Subject: [PATCH] Controller exceptions (#3227) * added result codes for global planner * code review * code review * cleanup * cleanup * update smac lattice planner * update planner instances * cleanup * added controller exception * renaming * follow path updates * rename exceptions * updated regulated pure pursuit * completed pure pursuit * completed dwb * linting fixes * cleanup * revert planner server * revert planner server * revert planner server * revert planner server * code review * code review * cleanup * cleanup * bug fix * final cleanup * set follow path error on bt * update groot * code review Co-authored-by: Joshua Wallace --- .../plugins/action/follow_path_action.hpp | 23 ++++++- nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../plugins/action/follow_path_action.cpp | 23 ++++++- .../src/constrained_smoother.cpp | 3 +- .../plugins/simple_progress_checker.cpp | 1 - nav2_controller/src/controller_server.cpp | 65 ++++++++++++++---- .../nav2_core/controller_exceptions.hpp | 67 +++++++++++++++++++ ...{exceptions.hpp => planner_exceptions.hpp} | 6 +- .../dwb_core/include/dwb_core/exceptions.hpp | 20 ++---- .../dwb_core/illegal_trajectory_tracker.hpp | 6 +- .../dwb_core/src/dwb_local_planner.cpp | 35 +++++++--- .../dwb_core/src/trajectory_utils.cpp | 4 +- nav2_msgs/action/FollowPath.action | 11 +++ .../nav2_navfn_planner/navfn_planner.hpp | 2 +- .../include/nav2_planner/planner_server.hpp | 4 +- .../src/regulated_pure_pursuit_controller.cpp | 18 ++--- .../test/test_regulated_pp.cpp | 4 +- .../nav2_rotation_shim_controller.hpp | 2 +- .../src/nav2_rotation_shim_controller.cpp | 12 ++-- .../include/nav2_smac_planner/a_star.hpp | 2 +- nav2_smoother/src/nav2_smoother.cpp | 4 +- nav2_smoother/test/test_smoother_server.cpp | 5 +- .../theta_star_planner.hpp | 2 +- 23 files changed, 237 insertions(+), 83 deletions(-) create mode 100644 nav2_core/include/nav2_core/controller_exceptions.hpp rename nav2_core/include/nav2_core/{exceptions.hpp => planner_exceptions.hpp} (95%) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index a97993c773..369e7ab0f9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -29,6 +29,10 @@ namespace nav2_behavior_tree */ class FollowPathAction : public BtActionNode { + using Action = nav2_msgs::action::FollowPath; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::FollowPathAction @@ -46,13 +50,28 @@ class FollowPathAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancellation of the action + */ + BT::NodeStatus on_cancelled() override; + /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet * @param feedback shared_ptr to latest feedback message */ void on_wait_for_result( - std::shared_ptr feedback) override; + std::shared_ptr feedback) override; /** * @brief Creates list of BT ports @@ -65,6 +84,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), + BT::OutputPort( + "follow_path_error_code", "The follow path error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 2a1ec9f6ed..b6a56ef0e5 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -113,6 +113,7 @@ Goal checker Server name Server timeout + Follow Path error code diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 0fc0dc5b57..c90d8fb65a 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -24,7 +24,7 @@ FollowPathAction::FollowPathAction( 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) { } @@ -35,8 +35,27 @@ void FollowPathAction::on_tick() getInput("goal_checker_id", goal_.goal_checker_id); } +BT::NodeStatus FollowPathAction::on_success() +{ + setOutput("follow_path_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus FollowPathAction::on_aborted() +{ + setOutput("follow_path_error_code", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus FollowPathAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + void FollowPathAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr/*feedback*/) { // Grab the new path nav_msgs::msg::Path new_path; diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 4ffa4a21c3..4ce698cda1 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -21,7 +21,6 @@ #include #include "nav2_constrained_smoother/constrained_smoother.hpp" -#include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -138,7 +137,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat logger_, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", plugin_name_.c_str()); - throw new nav2_core::PlannerException( + throw std::runtime_error( "Failed to smooth plan, Ceres could not find a usable solution."); } diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 1ced16dc5b..0b2ec3e739 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -17,7 +17,6 @@ #include #include #include -#include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2a52e296da..f69343fcb8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -19,7 +19,7 @@ #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/node_utils.hpp" @@ -353,8 +353,7 @@ void ControllerServer::computeControl() if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; } else { - action_server_->terminate_current(); - return; + throw nav2_core::ControllerException("Failed to find controller name: " + c_name); } std::string gc_name = action_server_->get_current_goal()->goal_checker_id; @@ -362,8 +361,7 @@ void ControllerServer::computeControl() if (findGoalCheckerId(gc_name, current_goal_checker)) { current_goal_checker_ = current_goal_checker; } else { - action_server_->terminate_current(); - return; + throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name); } setPlannerPath(action_server_->get_current_goal()->path); @@ -405,10 +403,47 @@ void ControllerServer::computeControl() controller_frequency_); } } - } catch (nav2_core::PlannerException & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); + } catch (nav2_core::ControllerTFError & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::TF_ERROR; + action_server_->terminate_current(result); + return; + } catch (nav2_core::NoValidControl & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::NO_VALID_CONTROL; + action_server_->terminate_current(result); + return; + } catch (nav2_core::FailedToMakeProgress & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::FAILED_TO_MAKE_PROGRESS; + action_server_->terminate_current(result); + return; + } catch (nav2_core::PatienceExceeded & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::PATIENCE_EXCEEDED; + action_server_->terminate_current(result); + return; + } catch (nav2_core::InvalidPath & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::INVALID_PATH; + action_server_->terminate_current(result); + return; + } catch (nav2_core::ControllerException & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); - action_server_->terminate_current(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::UNKNOWN; + action_server_->terminate_current(result); return; } catch (std::exception & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); @@ -432,7 +467,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) get_logger(), "Providing path to the controller %s", current_controller_.c_str()); if (path.poses.empty()) { - throw nav2_core::PlannerException("Invalid path, Path is empty."); + throw nav2_core::InvalidPath("Path is empty."); } controllers_[current_controller_]->setPlan(path); @@ -452,11 +487,11 @@ void ControllerServer::computeAndPublishVelocity() geometry_msgs::msg::PoseStamped pose; if (!getRobotPose(pose)) { - throw nav2_core::PlannerException("Failed to obtain robot pose"); + throw nav2_core::ControllerTFError("Failed to obtain robot pose"); } if (!progress_checker_->check(pose)) { - throw nav2_core::PlannerException("Failed to make progress"); + throw nav2_core::FailedToMakeProgress("Failed to make progress"); } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -470,7 +505,9 @@ void ControllerServer::computeAndPublishVelocity() nav_2d_utils::twist2Dto3D(twist), goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); - } catch (nav2_core::PlannerException & e) { + // Only no valid control exception types are valid to attempt to have control patience, as + // other types will not be resolved with more attempts + } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { RCLCPP_WARN(this->get_logger(), e.what()); cmd_vel_2d.twist.angular.x = 0; @@ -484,10 +521,10 @@ void ControllerServer::computeAndPublishVelocity() if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && failure_tolerance_ != -1.0) { - throw nav2_core::PlannerException("Controller patience exceeded"); + throw nav2_core::PatienceExceeded("Controller patience exceeded"); } } else { - throw nav2_core::PlannerException(e.what()); + throw nav2_core::NoValidControl(e.what()); } } diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp new file mode 100644 index 0000000000..bf9c22f4e6 --- /dev/null +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2022. Joshua Wallace +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ +#define NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ + +#include +#include + +namespace nav2_core +{ + +class ControllerException : public std::runtime_error +{ +public: + explicit ControllerException(const std::string & description) + : std::runtime_error(description) {} +}; + +class ControllerTFError : public ControllerException +{ +public: + explicit ControllerTFError(const std::string & description) + : ControllerException(description) {} +}; + +class FailedToMakeProgress : public ControllerException +{ +public: + explicit FailedToMakeProgress(const std::string & description) + : ControllerException(description) {} +}; + +class PatienceExceeded : public ControllerException +{ +public: + explicit PatienceExceeded(const std::string & description) + : ControllerException(description) {} +}; + +class InvalidPath : public ControllerException +{ +public: + explicit InvalidPath(const std::string & description) + : ControllerException(description) {} +}; + +class NoValidControl : public ControllerException +{ +public: + explicit NoValidControl(const std::string & description) + : ControllerException(description) {} +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__CONTROLLER_EXCEPTIONS_HPP_ diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp similarity index 95% rename from nav2_core/include/nav2_core/exceptions.hpp rename to nav2_core/include/nav2_core/planner_exceptions.hpp index 19a113d2e4..622ee0f0db 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -33,8 +33,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV2_CORE__EXCEPTIONS_HPP_ -#define NAV2_CORE__EXCEPTIONS_HPP_ +#ifndef NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ +#define NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ #include #include @@ -101,4 +101,4 @@ class PlannerTFError : public PlannerException } // namespace nav2_core -#endif // NAV2_CORE__EXCEPTIONS_HPP_ +#endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp index 157383a419..66a8bfd9e1 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/exceptions.hpp @@ -34,35 +34,23 @@ #ifndef DWB_CORE__EXCEPTIONS_HPP_ #define DWB_CORE__EXCEPTIONS_HPP_ -#include #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace dwb_core { -/** - * @class PlannerTFException - * @brief Thrown when the planner cannot complete its operation due to TF errors - */ -class PlannerTFException : public nav2_core::PlannerException -{ -public: - explicit PlannerTFException(const std::string description) - : nav2_core::PlannerException(description) {} -}; - /** * @class IllegalTrajectoryException * @brief Thrown when one of the critics encountered a fatal error */ -class IllegalTrajectoryException : public nav2_core::PlannerException +class IllegalTrajectoryException : public nav2_core::ControllerException { public: - IllegalTrajectoryException(const std::string critic_name, const std::string description) - : nav2_core::PlannerException(description), critic_name_(critic_name) {} + IllegalTrajectoryException(const std::string & critic_name, const std::string & description) + : nav2_core::ControllerException(description), critic_name_(critic_name) {} std::string getCriticName() const {return critic_name_;} protected: diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp index 31c306f665..aa6a451a1a 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.hpp @@ -39,7 +39,7 @@ #include #include #include "dwb_core/exceptions.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace dwb_core { @@ -66,11 +66,11 @@ class IllegalTrajectoryTracker * @brief Thrown when all the trajectories explored are illegal */ class NoLegalTrajectoriesException - : public nav2_core::PlannerException + : public nav2_core::ControllerException { public: explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker & tracker) - : PlannerException(tracker.getMessage()), + : ControllerException(tracker.getMessage()), tracker_(tracker) {} IllegalTrajectoryTracker tracker_; }; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 0be00388ca..da8110a1a5 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -44,11 +44,11 @@ #include "dwb_msgs/msg/critic_score.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "nav_2d_utils/conversions.hpp" -#include "nav_2d_utils/parameters.hpp" #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -133,7 +133,9 @@ void DWBLocalPlanner::configure( loadCritics(); } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what()); - throw; + throw nav2_core::ControllerException( + "Couldn't load critics! Caught exception: " + + std::string(e.what())); } } @@ -214,7 +216,9 @@ DWBLocalPlanner::loadCritics() plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_); } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!"); - throw; + throw nav2_core::ControllerException( + "Couldn't initialize critic plugin: " + + std::string(e.what())); } RCLCPP_INFO(logger_, "Critic plugin initialized"); } @@ -253,9 +257,18 @@ DWBLocalPlanner::computeVelocityCommands( geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.twist = nav_2d_utils::twist2Dto3D(cmd_vel2d.velocity); return cmd_vel; - } catch (const nav2_core::PlannerException & e) { + } catch (const nav2_core::ControllerTFError & e) { pub_->publishEvaluation(results); - throw; + throw e; + } catch (const nav2_core::InvalidPath & e) { + pub_->publishEvaluation(results); + throw e; + } catch (const nav2_core::NoValidControl & e) { + pub_->publishEvaluation(results); + throw e; + } catch (const nav2_core::ControllerException & e) { + pub_->publishEvaluation(results); + throw e; } } @@ -333,7 +346,9 @@ DWBLocalPlanner::computeVelocityCommands( pub_->publishLocalPlan(pose.header, empty_traj); pub_->publishCostGrid(costmap_ros_, critics_); - throw; + throw nav2_core::NoValidControl( + "Could not find a legal trajectory: " + + std::string(e.what())); } } @@ -441,7 +456,7 @@ DWBLocalPlanner::transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::InvalidPath("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan @@ -450,8 +465,8 @@ DWBLocalPlanner::transformGlobalPlan( tf_, global_plan_.header.frame_id, pose, robot_pose, transform_tolerance_)) { - throw dwb_core:: - PlannerTFException("Unable to transform robot pose into global plan's frame"); + throw nav2_core:: + ControllerTFError("Unable to transform robot pose into global plan's frame"); } // we'll discard points on the plan that are outside the local costmap @@ -535,7 +550,7 @@ DWBLocalPlanner::transformGlobalPlan( } if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } return transformed_plan; } diff --git a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp index a7764d4b7a..b89957f34e 100644 --- a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp +++ b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp @@ -49,7 +49,7 @@ const geometry_msgs::msg::Pose2D & getClosestPose( rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset); const unsigned int num_poses = trajectory.poses.size(); if (num_poses == 0) { - throw nav2_core::PlannerException("Cannot call getClosestPose on empty trajectory."); + throw nav2_core::InvalidPath("Cannot call getClosestPose on empty trajectory."); } unsigned int closest_index = num_poses; double closest_diff = 0.0; @@ -73,7 +73,7 @@ geometry_msgs::msg::Pose2D projectPose( rclcpp::Duration goal_time = rclcpp::Duration::from_seconds(time_offset); const unsigned int num_poses = trajectory.poses.size(); if (num_poses == 0) { - throw nav2_core::PlannerException("Cannot call projectPose on empty trajectory."); + throw nav2_core::InvalidPath("Cannot call projectPose on empty trajectory."); } if (goal_time <= (trajectory.time_offsets[0])) { return trajectory.poses[0]; diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 5462faa239..9ab82ff4d7 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,3 +1,13 @@ +# 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 INVALID_PATH=3 +int16 PATIENCE_EXCEEDED=4 +int16 FAILED_TO_MAKE_PROGRESS=5 +int16 NO_VALID_CONTROL=6 + #goal definition nav_msgs/Path path string controller_id @@ -5,6 +15,7 @@ string goal_checker_id --- #result definition std_msgs/Empty result +int16 error_code --- #feedback definition float32 distance_to_goal diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 454dd44858..2330a5165b 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -25,7 +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 "nav2_core/planner_exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_navfn_planner/navfn.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 4048775001..ad4f71c3b1 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,7 +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" +#include "nav2_core/planner_exceptions.hpp" namespace nav2_planner { @@ -253,7 +253,7 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - // Service to deterime if the path is valid + // Service to determine if the path is valid rclcpp::Service::SharedPtr is_path_valid_service_; }; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4b52dd3616..d5c648bd1a 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -21,7 +21,7 @@ #include #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -46,7 +46,7 @@ void RegulatedPurePursuitController::configure( auto node = parent.lock(); node_ = parent; if (!node) { - throw nav2_core::PlannerException("Unable to lock node!"); + throw nav2_core::ControllerException("Unable to lock node!"); } costmap_ros_ = costmap_ros; @@ -353,7 +353,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { - throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); + throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); } // populate and return message @@ -579,7 +579,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double logger_, "The dimensions of the costmap is too small to fully include your robot's footprint, " "thusly the robot cannot proceed further"); - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( "RegulatedPurePursuitController: Dimensions of the costmap are too small " "to encapsulate the robot footprint at current speeds!"); } @@ -690,13 +690,13 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::InvalidPath("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } // We'll discard points on the plan that are outside the local costmap @@ -729,7 +729,9 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( stamped_pose.header.frame_id = global_plan_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } transformed_pose.pose.position.z = 0.0; return transformed_pose; }; @@ -749,7 +751,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( global_path_pub_->publish(transformed_plan); if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); } return transformed_plan; diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 83d814dda0..51bb277fed 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -25,7 +25,7 @@ #include "path_utils/path_utils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" class RclCppFixture { @@ -849,7 +849,7 @@ TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) ctrl_->setPlan(global_plan); // Transform the plan - EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::PlannerException); + EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::ControllerException); } // Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3f96dd25e..c3812ec1d3 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -27,7 +27,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_core/controller.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "angles/angles.h" diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bc86900f64..7d32283db7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -175,7 +174,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() { if (current_path_.poses.size() < 2) { - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( "Path is too short to find a valid sampled path point for rotation."); } @@ -193,7 +192,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - throw nav2_core::PlannerException( + throw nav2_core::ControllerException( std::string( "Unable to find a sampling point at least %0.2f from the robot," "passing off to primary controller plugin.", forward_sampling_distance_)); @@ -204,7 +203,7 @@ RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseS { geometry_msgs::msg::PoseStamped pt_base; if (!nav2_util::transformPoseInTargetFrame(pt, pt_base, *tf_, costmap_ros_->getBaseFrameID())) { - throw nav2_core::PlannerException("Failed to transform pose to base frame!"); + throw nav2_core::ControllerTFError("Failed to transform pose to base frame!"); } return pt_base.pose; } @@ -259,11 +258,12 @@ void RotationShimController::isCollisionFree( if (footprint_cost == static_cast(NO_INFORMATION) && costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { - throw std::runtime_error("RotationShimController detected a potential collision ahead!"); + throw nav2_core::NoValidControl( + "RotationShimController detected a potential collision ahead!"); } if (footprint_cost >= static_cast(LETHAL_OBSTACLE)) { - throw std::runtime_error("RotationShimController detected collision ahead!"); + throw nav2_core::NoValidControl("RotationShimController detected collision ahead!"); } } } diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 7b2ee31c0e..01aeca975d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,7 +25,7 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 1fdf154d6f..34f4343190 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -15,15 +15,13 @@ // limitations under the License. #include -#include #include #include #include #include -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_smoother/nav2_smoother.hpp" -#include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 8dbc5416db..d7afe9b55d 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -16,9 +16,7 @@ #include #include #include -#include #include -#include #include #include "gtest/gtest.h" @@ -26,10 +24,9 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_core/smoother.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav2_msgs/action/smooth_path.hpp" #include "nav2_smoother/nav2_smoother.hpp" -#include "tf2_ros/create_timer_ros.h" using SmoothAction = nav2_msgs::action::SmoothPath; using ClientGoalHandle = rclcpp_action::ClientGoalHandle; diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 7b124f9764..bcbff2ae7c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -25,7 +25,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/planner_exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp"