From fdbb76123f5082addd5179bc4cc8b4567ef4014a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 23 Aug 2022 16:22:15 -0400 Subject: [PATCH 01/38] added result codes for global planner --- .../action/compute_path_to_pose_action.hpp | 2 + .../action/compute_path_to_pose_action.cpp | 9 +++ nav2_core/include/nav2_core/exceptions.hpp | 31 +++++++++- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/action/ComputePathToPose.action | 1 + nav2_msgs/msg/GlobalPlannerResultCode.msg | 7 +++ .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/src/planner_server.cpp | 56 ++++++++++++++----- 8 files changed, 91 insertions(+), 17 deletions(-) create mode 100644 nav2_msgs/msg/GlobalPlannerResultCode.msg 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 f87435995c..711e1e613b 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 @@ -70,6 +70,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), + BT::OutputPort("global_planner_result_code", + "The global planner result code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), 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 e6d315e43a..2fcd1ae542 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 @@ -40,6 +40,7 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); + setOutput("global_planner_result_code", result_.result); return BT::NodeStatus::SUCCESS; } @@ -47,6 +48,10 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + + nav2_msgs::msg::GlobalPlannerResultCode result_code; + setOutput("global_planner_result_code", result_code); + return BT::NodeStatus::FAILURE; } @@ -54,6 +59,10 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + + nav2_msgs::msg::GlobalPlannerResultCode result_code; + setOutput("global_planner_result_code", result_code); + return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index be23a91d42..b33df7fa5a 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -46,9 +46,36 @@ 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; +}; + +class GlobalPlannerGoalOccupiedException: public PlannerException +{ +public: + explicit GlobalPlannerGoalOccupiedException(const std::string &description) + : PlannerException(description) {}; +}; + +class GlobalPlannerStartOccupiedException : public PlannerException +{ +public: + explicit GlobalPlannerStartOccupiedException(const std::string &description) + : PlannerException(description) {}; +}; + +class GlobalPlannerNoValidPathException : public PlannerException +{ +public: + explicit GlobalPlannerNoValidPathException(const std::string &description) + : PlannerException(description) {}; +}; + +class GlobalPlannerTimeOutException : public PlannerException +{ +public: + explicit GlobalPlannerTimeOutException(const std::string &description) + : PlannerException(description) {}; }; } // namespace nav2_core diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index ce7376cebb..e1eccfe173 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/GlobalPlannerResultCode.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index a052c552ab..e4515ab1cb 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -7,5 +7,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 +GlobalPlannerResultCode result_code --- #feedback definition diff --git a/nav2_msgs/msg/GlobalPlannerResultCode.msg b/nav2_msgs/msg/GlobalPlannerResultCode.msg new file mode 100644 index 0000000000..f354192d3b --- /dev/null +++ b/nav2_msgs/msg/GlobalPlannerResultCode.msg @@ -0,0 +1,7 @@ +int32 START_OCCUPIED=0 +int32 GOAL_OCCUPIED=1 +int32 NO_VALID_PATH=2 +int32 TIMEOUT=3 + +int32 result_code +string message \ No newline at end of file diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 54e740e528..7811c74989 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/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 { diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9abc461d30..be7ed94552 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -461,26 +461,52 @@ PlannerServer::computePlan() return; } - result->path = getPlan(start, goal_pose, goal->planner_id); + try + { + result->path = getPlan(start, goal_pose, goal->planner_id); - if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { - return; - } + if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { + return; + } - // Publish the plan for visualization purposes - publishPlan(result->path); + // Publish the plan for visualization purposes + publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; - result->planning_time = cycle_duration; + auto cycle_duration = steady_clock_.now() - start_time; + result->planning_time = cycle_duration; - if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { - RCLCPP_WARN( - get_logger(), - "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); - } + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { + RCLCPP_WARN( + get_logger(), + "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); + action_server_pose_->succeeded_current(result); + } + catch (nav2_core::GlobalPlannerStartOccupiedException &e) + { + result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::GOAL_OCCUPIED; + action_server_pose_->terminate_current(result); + } + catch (nav2_core::GlobalPlannerGoalOccupiedException &e) + { + result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::GOAL_OCCUPIED; + result->result_code.message = e.what(); + action_server_pose_->terminate_current(result); + } + catch (nav2_core::GlobalPlannerNoValidPathException &e) + { + result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::NO_VALID_PATH; + result->result_code.message = e.what(); + action_server_pose_->terminate_current(result); + } + catch (nav2_core::GlobalPlannerTimeOutException &e) + { + result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::TIMEOUT; + result->result_code.message = e.what(); + action_server_pose_->terminate_current(result); + } } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", From 6f92768c8a93352828ad9e898629e241f5269fd6 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 11:35:00 -0400 Subject: [PATCH 02/38] code review --- .../action/compute_path_to_pose_action.hpp | 6 +- .../action/compute_path_to_pose_action.cpp | 12 +- nav2_msgs/CMakeLists.txt | 1 - nav2_msgs/action/ComputePathToPose.action | 10 +- nav2_msgs/msg/GlobalPlannerResultCode.msg | 7 - nav2_planner/src/planner_server.cpp | 343 +++++++++--------- 6 files changed, 184 insertions(+), 195 deletions(-) delete mode 100644 nav2_msgs/msg/GlobalPlannerResultCode.msg 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 711e1e613b..97ba57e852 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 @@ -20,6 +20,7 @@ #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav_msgs/msg/path.h" #include "nav2_behavior_tree/bt_action_node.hpp" +#include "std_msgs/msg/int16.hpp" namespace nav2_behavior_tree { @@ -70,8 +71,9 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort("global_planner_result_code", - "The global planner result code"), + BT::OutputPort + ("global_planner_error_code", + "The global planner error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), 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 2fcd1ae542..afbca41591 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 @@ -40,7 +40,7 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); - setOutput("global_planner_result_code", result_.result); + setOutput("global_planner_result_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -49,8 +49,9 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - nav2_msgs::msg::GlobalPlannerResultCode result_code; - setOutput("global_planner_result_code", result_code); + nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code = + nav2_msgs::action::ComputePathToPose::Goal::INVALID; + setOutput("global_planner_error_code", error_code); return BT::NodeStatus::FAILURE; } @@ -60,8 +61,9 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - nav2_msgs::msg::GlobalPlannerResultCode result_code; - setOutput("global_planner_result_code", result_code); + nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code = + nav2_msgs::action::ComputePathToPose::Goal::INVALID; + setOutput("global_planner_error_code", error_code); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e1eccfe173..ce7376cebb 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,7 +22,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" - "msg/GlobalPlannerResultCode.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index e4515ab1cb..427eaf939b 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,3 +1,11 @@ +# Error codes +int16 INVALID=0 +int16 UNKNOWN=1 +int16 START_OCCUPIED=2 +int16 GOAL_OCCUPIED=3 +int16 NO_VALID_PATH=4 +int16 TIMEOUT=5 + #goal definition geometry_msgs/PoseStamped goal geometry_msgs/PoseStamped start @@ -7,6 +15,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 -GlobalPlannerResultCode result_code +int16 error_code --- #feedback definition diff --git a/nav2_msgs/msg/GlobalPlannerResultCode.msg b/nav2_msgs/msg/GlobalPlannerResultCode.msg deleted file mode 100644 index f354192d3b..0000000000 --- a/nav2_msgs/msg/GlobalPlannerResultCode.msg +++ /dev/null @@ -1,7 +0,0 @@ -int32 START_OCCUPIED=0 -int32 GOAL_OCCUPIED=1 -int32 NO_VALID_PATH=2 -int32 TIMEOUT=3 - -int32 result_code -string message \ No newline at end of file diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index be7ed94552..50babe2e24 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -36,16 +36,14 @@ using namespace std::chrono_literals; using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; -namespace nav2_planner -{ - -PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("planner_server", "", options), - gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), - default_ids_{"GridBased"}, - default_types_{"nav2_navfn_planner/NavfnPlanner"}, - costmap_(nullptr) -{ +namespace nav2_planner { + +PlannerServer::PlannerServer(const rclcpp::NodeOptions &options) + : nav2_util::LifecycleNode("planner_server", "", options), + gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), + default_ids_{"GridBased"}, + default_types_{"nav2_navfn_planner/NavfnPlanner"}, + costmap_(nullptr) { RCLCPP_INFO(get_logger(), "Creating"); // Declare this node's parameters @@ -61,29 +59,27 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Setup the global costmap costmap_ros_ = std::make_shared( - "global_costmap", std::string{get_namespace()}, "global_costmap"); + "global_costmap", std::string{get_namespace()}, "global_costmap"); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); } -PlannerServer::~PlannerServer() -{ +PlannerServer::~PlannerServer() { planners_.clear(); costmap_thread_.reset(); } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) -{ +PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); RCLCPP_DEBUG( - get_logger(), "Costmap size: %d,%d", - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + get_logger(), "Costmap size: %d,%d", + costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); tf_ = costmap_ros_->getTfBuffer(); @@ -94,18 +90,18 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) for (size_t i = 0; i != planner_ids_.size(); i++) { try { planner_types_[i] = nav2_util::get_plugin_type_param( - node, planner_ids_[i]); + node, planner_ids_[i]); nav2_core::GlobalPlanner::Ptr planner = - gp_loader_.createUniqueInstance(planner_types_[i]); + gp_loader_.createUniqueInstance(planner_types_[i]); RCLCPP_INFO( - get_logger(), "Created global planner plugin %s of type %s", - planner_ids_[i].c_str(), planner_types_[i].c_str()); + get_logger(), "Created global planner plugin %s of type %s", + planner_ids_[i].c_str(), planner_types_[i].c_str()); planner->configure(node, planner_ids_[i], tf_, costmap_ros_); planners_.insert({planner_ids_[i], planner}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const pluginlib::PluginlibException &ex) { RCLCPP_FATAL( - get_logger(), "Failed to create global planner. Exception: %s", - ex.what()); + get_logger(), "Failed to create global planner. Exception: %s", + ex.what()); return nav2_util::CallbackReturn::FAILURE; } } @@ -115,8 +111,8 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } RCLCPP_INFO( - get_logger(), - "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); double expected_planner_frequency; get_parameter("expected_planner_frequency", expected_planner_frequency); @@ -124,9 +120,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) max_planner_duration_ = 1 / expected_planner_frequency; } else { RCLCPP_WARN( - get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } @@ -135,27 +131,26 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( - shared_from_this(), - "compute_path_to_pose", - std::bind(&PlannerServer::computePlan, this), - nullptr, - std::chrono::milliseconds(500), - true); + shared_from_this(), + "compute_path_to_pose", + std::bind(&PlannerServer::computePlan, this), + nullptr, + std::chrono::milliseconds(500), + true); action_server_poses_ = std::make_unique( - shared_from_this(), - "compute_path_through_poses", - std::bind(&PlannerServer::computePlanThroughPoses, this), - nullptr, - std::chrono::milliseconds(500), - true); + shared_from_this(), + "compute_path_through_poses", + std::bind(&PlannerServer::computePlanThroughPoses, this), + nullptr, + std::chrono::milliseconds(500), + true); return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) -{ +PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); plan_publisher_->on_activate(); @@ -171,14 +166,14 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); is_path_valid_service_ = node->create_service( - "is_path_valid", - std::bind( - &PlannerServer::isPathValid, this, - std::placeholders::_1, std::placeholders::_2)); + "is_path_valid", + std::bind( + &PlannerServer::isPathValid, this, + std::placeholders::_1, std::placeholders::_2)); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PlannerServer::dynamicParametersCallback, this, _1)); + std::bind(&PlannerServer::dynamicParametersCallback, this, _1)); // create bond connection createBond(); @@ -187,8 +182,7 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) -{ +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); action_server_pose_->deactivate(); @@ -210,8 +204,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) -{ +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); action_server_pose_.reset(); @@ -230,16 +223,14 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) -{ +PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Shutting down"); return nav2_util::CallbackReturn::SUCCESS; } template bool PlannerServer::isServerInactive( - std::unique_ptr> & action_server) -{ + std::unique_ptr> &action_server) { if (action_server == nullptr || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return true; @@ -248,8 +239,7 @@ bool PlannerServer::isServerInactive( return false; } -void PlannerServer::waitForCostmap() -{ +void PlannerServer::waitForCostmap() { // Don't compute a plan until costmap is valid (after clear costmap) rclcpp::Rate r(100); while (!costmap_ros_->isCurrent()) { @@ -259,8 +249,7 @@ void PlannerServer::waitForCostmap() template bool PlannerServer::isCancelRequested( - std::unique_ptr> & action_server) -{ + std::unique_ptr> &action_server) { if (action_server->is_cancel_requested()) { RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); action_server->terminate_all(); @@ -272,9 +261,8 @@ bool PlannerServer::isCancelRequested( template void PlannerServer::getPreemptedGoalIfRequested( - std::unique_ptr> & action_server, - typename std::shared_ptr goal) -{ + std::unique_ptr> &action_server, + typename std::shared_ptr goal) { if (action_server->is_preempt_requested()) { goal = action_server->accept_pending_goal(); } @@ -282,10 +270,9 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( - std::unique_ptr> & action_server, - typename std::shared_ptr goal, - geometry_msgs::msg::PoseStamped & start) -{ + std::unique_ptr> &action_server, + typename std::shared_ptr goal, + geometry_msgs::msg::PoseStamped &start) { if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { @@ -298,15 +285,13 @@ bool PlannerServer::getStartPose( template bool PlannerServer::transformPosesToGlobalFrame( - std::unique_ptr> & action_server, - geometry_msgs::msg::PoseStamped & curr_start, - geometry_msgs::msg::PoseStamped & curr_goal) -{ + std::unique_ptr> &action_server, + geometry_msgs::msg::PoseStamped &curr_start, + geometry_msgs::msg::PoseStamped &curr_goal) { if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || - !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) - { + !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { RCLCPP_WARN( - get_logger(), "Could not transform the start or goal pose in the costmap frame"); + get_logger(), "Could not transform the start or goal pose in the costmap frame"); action_server->terminate_current(); return false; } @@ -316,32 +301,30 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( - std::unique_ptr> & action_server, - const geometry_msgs::msg::PoseStamped & goal, - const nav_msgs::msg::Path & path, - const std::string & planner_id) -{ + std::unique_ptr> &action_server, + const geometry_msgs::msg::PoseStamped &goal, + const nav_msgs::msg::Path &path, + const std::string &planner_id) { if (path.poses.size() == 0) { RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path to (%.2f, %.2f)", planner_id.c_str(), - goal.pose.position.x, goal.pose.position.y); + get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_id.c_str(), + goal.pose.position.x, goal.pose.position.y); action_server->terminate_current(); return false; } RCLCPP_DEBUG( - get_logger(), - "Found valid path of size %zu to (%.2f, %.2f)", - path.poses.size(), goal.pose.position.x, - goal.pose.position.y); + get_logger(), + "Found valid path of size %zu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); return true; } void -PlannerServer::computePlanThroughPoses() -{ +PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); auto start_time = steady_clock_.now(); @@ -362,8 +345,8 @@ PlannerServer::computePlanThroughPoses() if (goal->goals.size() == 0) { RCLCPP_WARN( - get_logger(), - "Compute path through poses requested a plan with no viapoint poses, returning."); + get_logger(), + "Compute path through poses requested a plan with no viapoint poses, returning."); action_server_poses_->terminate_current(); } @@ -400,7 +383,7 @@ PlannerServer::computePlanThroughPoses() // Concatenate paths together concat_path.poses.insert( - concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); concat_path.header = curr_path.header; } @@ -413,25 +396,24 @@ PlannerServer::computePlanThroughPoses() if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { RCLCPP_WARN( - get_logger(), - "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); + get_logger(), + "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_poses_->succeeded_current(result); - } catch (std::exception & ex) { + } 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()); + 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(); } } void -PlannerServer::computePlan() -{ +PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); auto start_time = steady_clock_.now(); @@ -440,6 +422,8 @@ PlannerServer::computePlan() auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); + geometry_msgs::msg::PoseStamped start; + try { if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) { return; @@ -450,7 +434,6 @@ PlannerServer::computePlan() getPreemptedGoalIfRequested(action_server_pose_, goal); // Use start pose if provided otherwise use current robot pose - geometry_msgs::msg::PoseStamped start; if (!getStartPose(action_server_pose_, goal, start)) { return; } @@ -461,86 +444,92 @@ PlannerServer::computePlan() return; } - try - { - result->path = getPlan(start, goal_pose, goal->planner_id); - - if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { - return; - } + result->path = getPlan(start, goal_pose, goal->planner_id); - // Publish the plan for visualization purposes - publishPlan(result->path); + if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { + return; + } - auto cycle_duration = steady_clock_.now() - start_time; - result->planning_time = cycle_duration; + // Publish the plan for visualization purposes + publishPlan(result->path); - if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { - RCLCPP_WARN( - get_logger(), - "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); - } + auto cycle_duration = steady_clock_.now() - start_time; + result->planning_time = cycle_duration; - action_server_pose_->succeeded_current(result); - } - catch (nav2_core::GlobalPlannerStartOccupiedException &e) - { - result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::GOAL_OCCUPIED; - action_server_pose_->terminate_current(result); - } - catch (nav2_core::GlobalPlannerGoalOccupiedException &e) - { - result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::GOAL_OCCUPIED; - result->result_code.message = e.what(); - action_server_pose_->terminate_current(result); - } - catch (nav2_core::GlobalPlannerNoValidPathException &e) - { - result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::NO_VALID_PATH; - result->result_code.message = e.what(); - action_server_pose_->terminate_current(result); - } - catch (nav2_core::GlobalPlannerTimeOutException &e) - { - result->result_code.result_code = nav2_msgs::msg::GlobalPlannerResultCode::TIMEOUT; - result->result_code.message = e.what(); - action_server_pose_->terminate_current(result); + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { + RCLCPP_WARN( + get_logger(), + "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } - } catch (std::exception & ex) { + + action_server_pose_->succeeded_current(result); + } catch (nav2_core::GlobalPlannerStartOccupiedException &ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goal.pose.position.x, - goal->goal.pose.position.y, ex.what()); + get_logger(), "%s plugin failed to plan path. " + "Start Pose (%.2f, %.2f) was occupied: \"%s\"", + goal->planner_id.c_str(), + start.pose.position.x, + start.pose.position.y, + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerGoalOccupiedException &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. Goal Pose (%.2f, %.2f) was occupied: \"%s\"", + goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerTimeOutException &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerNoValidPathException &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. No Valid Path Could be found: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + action_server_pose_->terminate_current(result); + } catch (std::exception &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", + goal->planner_id.c_str(), goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); action_server_pose_->terminate_current(); } } nav_msgs::msg::Path PlannerServer::getPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id) -{ + const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal, + const std::string &planner_id) { RCLCPP_DEBUG( - get_logger(), "Attempting to a find path from (%.2f, %.2f) to " - "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, - goal.pose.position.x, goal.pose.position.y); + get_logger(), "Attempting to a find path from (%.2f, %.2f) to " + "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y); if (planners_.find(planner_id) != planners_.end()) { return planners_[planner_id]->createPlan(start, goal); } else { if (planners_.size() == 1 && planner_id.empty()) { RCLCPP_WARN_ONCE( - get_logger(), "No planners specified in action call. " - "Server will use only plugin %s in server." - " This warning will appear once.", planner_ids_concat_.c_str()); + get_logger(), "No planners specified in action call. " + "Server will use only plugin %s in server." + " This warning will appear once.", planner_ids_concat_.c_str()); return planners_[planners_.begin()->first]->createPlan(start, goal); } else { RCLCPP_ERROR( - get_logger(), "planner %s is not a valid planner. " - "Planner names are: %s", planner_id.c_str(), - planner_ids_concat_.c_str()); + get_logger(), "planner %s is not a valid planner. " + "Planner names are: %s", planner_id.c_str(), + planner_ids_concat_.c_str()); } } @@ -548,8 +537,7 @@ PlannerServer::getPlan( } void -PlannerServer::publishPlan(const nav_msgs::msg::Path & path) -{ +PlannerServer::publishPlan(const nav_msgs::msg::Path &path) { auto msg = std::make_unique(path); if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) { plan_publisher_->publish(std::move(msg)); @@ -557,9 +545,8 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path & path) } void PlannerServer::isPathValid( - const std::shared_ptr request, - std::shared_ptr response) -{ + const std::shared_ptr request, + std::shared_ptr response) { response->is_valid = true; if (request->path.poses.empty()) { @@ -577,8 +564,8 @@ void PlannerServer::isPathValid( geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position; current_distance = nav2_util::geometry_utils::euclidean_distance( - current_point, - path_point); + current_point, + path_point); if (current_distance < closest_distance) { closest_point_index = i; @@ -594,13 +581,12 @@ void PlannerServer::isPathValid( unsigned int my = 0; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); + request->path.poses[i].pose.position.x, + request->path.poses[i].pose.position.y, mx, my); unsigned int cost = costmap_->getCost(mx, my); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - { + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { response->is_valid = false; } } @@ -608,14 +594,13 @@ void PlannerServer::isPathValid( } rcl_interfaces::msg::SetParametersResult -PlannerServer::dynamicParametersCallback(std::vector parameters) -{ +PlannerServer::dynamicParametersCallback(std::vector parameters) { std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + for (auto parameter: parameters) { + const auto &type = parameter.get_type(); + const auto &name = parameter.get_name(); if (type == ParameterType::PARAMETER_DOUBLE) { if (name == "expected_planner_frequency") { @@ -623,9 +608,9 @@ PlannerServer::dynamicParametersCallback(std::vector paramete max_planner_duration_ = 1 / parameter.as_double(); } else { RCLCPP_WARN( - get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); max_planner_duration_ = 0.0; } } From 4bd5050406ae6243a6875904b64b729dae7570fc Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 11:41:01 -0400 Subject: [PATCH 03/38] code review --- .../plugins/action/compute_path_to_pose_action.cpp | 6 ++---- nav2_planner/src/planner_server.cpp | 3 ++- 2 files changed, 4 insertions(+), 5 deletions(-) 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 afbca41591..d66008468a 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 @@ -49,8 +49,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code = - nav2_msgs::action::ComputePathToPose::Goal::INVALID; + nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code; setOutput("global_planner_error_code", error_code); return BT::NodeStatus::FAILURE; @@ -61,8 +60,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code = - nav2_msgs::action::ComputePathToPose::Goal::INVALID; + nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code; setOutput("global_planner_error_code", error_code); return BT::NodeStatus::SUCCESS; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 50babe2e24..9078616d66 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -502,7 +502,8 @@ PlannerServer::computePlan() { get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", goal->planner_id.c_str(), goal->goal.pose.position.x, goal->goal.pose.position.y, ex.what()); - action_server_pose_->terminate_current(); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; + action_server_pose_->terminate_current(result); } } From 3ccd777fbb5c2ca43ce55041998de3d846a404a3 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 11:46:35 -0400 Subject: [PATCH 04/38] cleanup --- .../plugins/action/compute_path_to_pose_action.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) 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 d66008468a..1c99cb1aff 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 @@ -48,9 +48,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - - nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code; - setOutput("global_planner_error_code", error_code); + setOutput("global_planner_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -59,9 +57,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - - nav2_msgs::action::ComputePathToPose::Result::_error_code_type error_code; - setOutput("global_planner_error_code", error_code); + setOutput("global_planner_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } From 95de2a8d084b1a238e07cf5fa6878ecf66230bcb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 11:46:58 -0400 Subject: [PATCH 05/38] cleanup --- .../plugins/action/compute_path_to_pose_action.cpp | 2 -- 1 file changed, 2 deletions(-) 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 1c99cb1aff..f707e798f7 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 @@ -49,7 +49,6 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); setOutput("global_planner_error_code", result_.result->error_code); - return BT::NodeStatus::FAILURE; } @@ -58,7 +57,6 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); setOutput("global_planner_error_code", result_.result->error_code); - return BT::NodeStatus::SUCCESS; } From f0a86a02a9d2247336d3af8a45f5b91fc4b696e6 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 16:20:33 -0400 Subject: [PATCH 06/38] update smac lattice planner --- nav2_core/include/nav2_core/exceptions.hpp | 4 +-- nav2_planner/src/planner_server.cpp | 9 +++---- .../include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 6 +++-- .../src/smac_planner_lattice.cpp | 25 +++++-------------- 5 files changed, 17 insertions(+), 28 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index b33df7fa5a..9874187a59 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -64,10 +64,10 @@ class GlobalPlannerStartOccupiedException : public PlannerException : PlannerException(description) {}; }; -class GlobalPlannerNoValidPathException : public PlannerException +class GlobalPlannerNoValidPathFoundException : public PlannerException { public: - explicit GlobalPlannerNoValidPathException(const std::string &description) + explicit GlobalPlannerNoValidPathFoundException(const std::string &description) : PlannerException(description) {}; }; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9078616d66..412a9793ce 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -490,18 +490,17 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerNoValidPathException &ex) { + } catch (nav2_core::GlobalPlannerNoValidPathFoundException &ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. No Valid Path Could be found: \"%s\"", + get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (std::exception &ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goal.pose.position.x, - goal->goal.pose.position.y, ex.what()); + get_logger(), "%s plugin failed to plan: \"%s\"", + goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; action_server_pose_->terminate_current(result); } 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 2af454bb53..24c31931ca 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,6 +25,7 @@ #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_core/exceptions.hpp" #include "nav2_smac_planner/analytic_expansion.hpp" #include "nav2_smac_planner/node_2d.hpp" diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index deaa597b92..d2c5d3c5b9 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -204,12 +204,14 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + throw nav2_core::GlobalPlannerGoalOccupiedException("Failed to compute path, " + "goal is occupied with no tolerance."); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + throw nav2_core::GlobalPlannerStartOccupiedException("Starting point in lethal space!" + "Cannot create feasible plan."); } return true; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 09a3591928..e3d762e124 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -260,26 +260,13 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Compute plan NodeLattice::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + } else { + throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + } } // Convert to world coordinates From af8adbc028ed1037cafe2b9b012726c7f10809ea Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 16:36:34 -0400 Subject: [PATCH 07/38] update planner instances --- nav2_smac_planner/src/smac_planner_2d.cpp | 27 +++++-------------- nav2_smac_planner/src/smac_planner_hybrid.cpp | 25 +++++------------ .../src/smac_planner_lattice.cpp | 1 + 3 files changed, 13 insertions(+), 40 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 606f6861ef..03c66af7ff 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -250,28 +250,13 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + if (!_a_star->createPath(path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + } else { + throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 279af1804c..fc781f8d17 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -313,26 +313,13 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; - std::string error; - try { - if (!_a_star->createPath(path, num_iterations, 0.0)) { - if (num_iterations < _a_star->getMaxIterations()) { - error = std::string("no valid path found"); - } else { - error = std::string("exceeded maximum iterations"); - } - } - } catch (const std::runtime_error & e) { - error = "invalid use: "; - error += e.what(); - } - if (!error.empty()) { - RCLCPP_WARN( - _logger, - "%s: failed to create plan, %s.", - _name.c_str(), error.c_str()); - return plan; + if (!_a_star->createPath(path, num_iterations, 0)) { + if (num_iterations < _a_star->getMaxIterations()) { + throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + } else { + throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + } } // Convert to world coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index e3d762e124..d84caf5ec1 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -260,6 +260,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Compute plan NodeLattice::CoordinateVector path; int num_iterations = 0; + std::string error; if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { if (num_iterations < _a_star->getMaxIterations()) { From 63e5f82e863112763440453863a59b641e21a5a9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 24 Aug 2022 16:39:07 -0400 Subject: [PATCH 08/38] cleanup --- .../plugins/action/compute_path_to_pose_action.hpp | 1 - 1 file changed, 1 deletion(-) 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 97ba57e852..5392115fbe 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 @@ -20,7 +20,6 @@ #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav_msgs/msg/path.h" #include "nav2_behavior_tree/bt_action_node.hpp" -#include "std_msgs/msg/int16.hpp" namespace nav2_behavior_tree { From 7f34be339ac37de152cdceeb8102e283b151ce51 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Aug 2022 10:44:22 -0400 Subject: [PATCH 09/38] updates --- .../action/compute_path_to_pose_action.cpp | 2 ++ nav2_core/include/nav2_core/exceptions.hpp | 21 +++++++++++++++ nav2_msgs/action/ComputePathToPose.action | 5 +++- nav2_planner/src/planner_server.cpp | 27 ++++++++++++++++--- 4 files changed, 51 insertions(+), 4 deletions(-) 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 f707e798f7..0c52933b4c 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 @@ -40,6 +40,7 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; setOutput("global_planner_result_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -48,6 +49,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; setOutput("global_planner_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index 9874187a59..465b5e4710 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -78,6 +78,27 @@ class GlobalPlannerTimeOutException : public PlannerException : PlannerException(description) {}; }; +class GlobalPlannerStartOutsideMapBounds : public PlannerException +{ +public: + explicit GlobalPlannerStartOutsideMapBounds(const std::string &description) + : PlannerException(description) {}; +}; + +class GlobalPlannerGoalOutsideMapBounds : public PlannerException +{ +public: + explicit GlobalPlannerGoalOutsideMapBounds(const std::string &description) + : PlannerException(description) {}; +}; + +class GlobalPlannerStartIsEqualToGoal : public PlannerException +{ +public: + explicit GlobalPlannerStartIsEqualToGoal(const std::string &description) + : PlannerException(description) {}; +}; + } // namespace nav2_core #endif // NAV2_CORE__EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 427eaf939b..734fbf3b17 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,10 +1,13 @@ # Error codes -int16 INVALID=0 +int16 NONE=0 int16 UNKNOWN=1 int16 START_OCCUPIED=2 int16 GOAL_OCCUPIED=3 int16 NO_VALID_PATH=4 int16 TIMEOUT=5 +int16 START_OUTSIDE_MAP=6 +int16 GOAL_OUTSIDE_MAP=7 +int16 START_IS_EQUAL_TO_GOAL=8 #goal definition geometry_msgs/PoseStamped goal diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 70887e576d..2b0822a899 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -483,6 +483,13 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerNoValidPathFoundException &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + action_server_pose_->terminate_current(result); } catch (nav2_core::GlobalPlannerTimeOutException &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", @@ -490,12 +497,26 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerNoValidPathFoundException &ex) { + } catch (nav2_core::GlobalPlannerStartOutsideMapBounds &ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", + get_logger(), "%s plugin failed to plan path. Start Outside map: \"%s\"", goal->planner_id.c_str(), ex.what()); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerGoalOutsideMapBounds &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; + action_server_pose_->terminate_current(result); + } catch (nav2_core::GlobalPlannerStartIsEqualToGoal &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; action_server_pose_->terminate_current(result); } catch (std::exception &ex) { RCLCPP_WARN( From c2221a21cfe4ed9ea0adc7643712c07675113abb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Aug 2022 11:09:21 -0400 Subject: [PATCH 10/38] renaming --- nav2_core/include/nav2_core/exceptions.hpp | 35 +++++++++++-------- nav2_planner/src/planner_server.cpp | 12 +++---- nav2_smac_planner/src/a_star.cpp | 4 +-- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 4 +-- .../src/smac_planner_lattice.cpp | 4 +-- 6 files changed, 35 insertions(+), 28 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index 465b5e4710..c0f0f05247 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -50,52 +50,59 @@ class PlannerException : public std::runtime_error : std::runtime_error(description) {} }; -class GlobalPlannerGoalOccupiedException: public PlannerException +class GoalOccupied: public PlannerException { public: - explicit GlobalPlannerGoalOccupiedException(const std::string &description) + explicit GoalOccupied(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerStartOccupiedException : public PlannerException +class StartOccupied : public PlannerException { public: - explicit GlobalPlannerStartOccupiedException(const std::string &description) + explicit StartOccupied(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerNoValidPathFoundException : public PlannerException +class NoValidPathCouldBeFound : public PlannerException { public: - explicit GlobalPlannerNoValidPathFoundException(const std::string &description) + explicit NoValidPathCouldBeFound(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerTimeOutException : public PlannerException +class PlannerTimedOut : public PlannerException { public: - explicit GlobalPlannerTimeOutException(const std::string &description) + explicit PlannerTimedOut(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerStartOutsideMapBounds : public PlannerException +class StartOutsideMapBounds : public PlannerException { public: - explicit GlobalPlannerStartOutsideMapBounds(const std::string &description) + explicit StartOutsideMapBounds(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerGoalOutsideMapBounds : public PlannerException +class GoalOutsideMapBounds : public PlannerException { public: - explicit GlobalPlannerGoalOutsideMapBounds(const std::string &description) + explicit GoalOutsideMapBounds(const std::string &description) : PlannerException(description) {}; }; -class GlobalPlannerStartIsEqualToGoal : public PlannerException +class StartIsEqualToGoal : public PlannerException { public: - explicit GlobalPlannerStartIsEqualToGoal(const std::string &description) + explicit StartIsEqualToGoal(const std::string &description) + : PlannerException(description) {}; +}; + +class PlannerTFError : public PlannerException +{ +public: + explicit PlannerTFError(const std::string &description) : PlannerException(description) {}; }; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 2b0822a899..564fd6c0dd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -464,7 +464,7 @@ PlannerServer::computePlan() { } action_server_pose_->succeeded_current(result); - } catch (nav2_core::GlobalPlannerStartOccupiedException &ex) { + } catch (nav2_core::StartOccupied &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. " "Start Pose (%.2f, %.2f) was occupied: \"%s\"", @@ -474,7 +474,7 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerGoalOccupiedException &ex) { + } catch (nav2_core::GoalOccupied &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. Goal Pose (%.2f, %.2f) was occupied: \"%s\"", goal->planner_id.c_str(), @@ -483,28 +483,28 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerNoValidPathFoundException &ex) { + } catch (nav2_core::NoValidPathCouldBeFound &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerTimeOutException &ex) { + } catch (nav2_core::PlannerTimedOut &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerStartOutsideMapBounds &ex) { + } catch (nav2_core::StartOutsideMapBounds &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. Start Outside map: \"%s\"", goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerGoalOutsideMapBounds &ex) { + } catch (nav2_core::GoalOutsideMapBounds &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", goal->planner_id.c_str(), diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d2c5d3c5b9..2249acdf79 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -204,13 +204,13 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GlobalPlannerGoalOccupiedException("Failed to compute path, " + throw nav2_core::GoalOccupied("Failed to compute path, " "goal is occupied with no tolerance."); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GlobalPlannerStartOccupiedException("Starting point in lethal space!" + throw nav2_core::StartOccupied("Starting point in lethal space!" "Cannot create feasible plan."); } diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 03c66af7ff..0ca029cd90 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -253,9 +253,9 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( if (!_a_star->createPath(path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); } } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index fc781f8d17..cd5043bece 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -316,9 +316,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (!_a_star->createPath(path, num_iterations, 0)) { if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index d84caf5ec1..6de619b879 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -264,9 +264,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { if (num_iterations < _a_star->getMaxIterations()) { - throw nav2_core::GlobalPlannerNoValidPathFoundException("no valid path found"); + throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::GlobalPlannerNoValidPathFoundException("exceeded maximum iterations"); + throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); } } From 915c68f1776bb17e874bd8a078d33ccfae71300b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Aug 2022 11:34:48 -0400 Subject: [PATCH 11/38] fixes --- nav2_msgs/action/ComputePathToPose.action | 1 + nav2_planner/src/planner_server.cpp | 11 +++++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 734fbf3b17..7506c44b7e 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -8,6 +8,7 @@ int16 TIMEOUT=5 int16 START_OUTSIDE_MAP=6 int16 GOAL_OUTSIDE_MAP=7 int16 START_IS_EQUAL_TO_GOAL=8 +int16 TF_ERROR=9 #goal definition geometry_msgs/PoseStamped goal diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 564fd6c0dd..f5e6c861ad 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -511,13 +511,20 @@ PlannerServer::computePlan() { ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); - } catch (nav2_core::GlobalPlannerStartIsEqualToGoal &ex) { + } catch (nav2_core::StartIsEqualToGoal &ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", + get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerTFError &ex) { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", + goal->planner_id.c_str(), + ex.what()); + result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; + action_server_pose_->terminate_current(result); } catch (std::exception &ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan: \"%s\"", From b88ee774c56be7eaa1d84da638897008fef617aa Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 31 Aug 2022 15:50:38 -0400 Subject: [PATCH 12/38] cpplint --- nav2_core/include/nav2_core/exceptions.hpp | 16 ++++++++-------- nav2_planner/src/planner_server.cpp | 3 +-- nav2_smac_planner/src/smac_planner_2d.cpp | 3 ++- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index c0f0f05247..0a74b0f3a5 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -54,56 +54,56 @@ class GoalOccupied: public PlannerException { public: explicit GoalOccupied(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class StartOccupied : public PlannerException { public: explicit StartOccupied(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class NoValidPathCouldBeFound : public PlannerException { public: explicit NoValidPathCouldBeFound(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class PlannerTimedOut : public PlannerException { public: explicit PlannerTimedOut(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class StartOutsideMapBounds : public PlannerException { public: explicit StartOutsideMapBounds(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class GoalOutsideMapBounds : public PlannerException { public: explicit GoalOutsideMapBounds(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class StartIsEqualToGoal : public PlannerException { public: explicit StartIsEqualToGoal(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; class PlannerTFError : public PlannerException { public: explicit PlannerTFError(const std::string &description) - : PlannerException(description) {}; + : PlannerException(description) {} }; } // namespace nav2_core diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f5e6c861ad..79ef4d3fde 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -625,8 +625,7 @@ rcl_interfaces::msg::SetParametersResult PlannerServer::dynamicParametersCallback(std::vector parameters) { std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; - - for (auto parameter: parameters) { + for (auto parameter : parameters) { const auto &type = parameter.get_type(); const auto &name = parameter.get_name(); diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 0ca029cd90..15c6f283ad 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -251,7 +251,8 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( Node2D::CoordinateVector path; int num_iterations = 0; - if (!_a_star->createPath(path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { + if (!_a_star->createPath(path, num_iterations, + _tolerance / static_cast(costmap->getResolution()))) { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { From 8dc55f2e4a08dbff566c330d81a8abe15537fe93 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 31 Aug 2022 16:35:56 -0400 Subject: [PATCH 13/38] uncrusitfy --- .../action/compute_path_to_pose_action.hpp | 5 +- nav2_core/include/nav2_core/exceptions.hpp | 34 +- nav2_planner/src/planner_server.cpp | 326 ++++++++++-------- nav2_smac_planner/src/a_star.cpp | 10 +- nav2_smac_planner/src/smac_planner_2d.cpp | 7 +- 5 files changed, 203 insertions(+), 179 deletions(-) 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 5392115fbe..0306375d51 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 @@ -70,9 +70,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort - ("global_planner_error_code", - "The global planner error code"), + BT::OutputPort( + "global_planner_error_code", "The global planner error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index 0a74b0f3a5..2c37106ea0 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -46,64 +46,64 @@ 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) {} }; -class GoalOccupied: public PlannerException +class GoalOccupied : public PlannerException { public: - explicit GoalOccupied(const std::string &description) + explicit GoalOccupied(const std::string & description) : PlannerException(description) {} }; class StartOccupied : public PlannerException { public: - explicit StartOccupied(const std::string &description) - : PlannerException(description) {} + explicit StartOccupied(const std::string & description) + : PlannerException(description) {} }; class NoValidPathCouldBeFound : public PlannerException { public: - explicit NoValidPathCouldBeFound(const std::string &description) - : PlannerException(description) {} + explicit NoValidPathCouldBeFound(const std::string & description) + : PlannerException(description) {} }; class PlannerTimedOut : public PlannerException { public: - explicit PlannerTimedOut(const std::string &description) - : PlannerException(description) {} + explicit PlannerTimedOut(const std::string & description) + : PlannerException(description) {} }; class StartOutsideMapBounds : public PlannerException { public: - explicit StartOutsideMapBounds(const std::string &description) - : PlannerException(description) {} + explicit StartOutsideMapBounds(const std::string & description) + : PlannerException(description) {} }; class GoalOutsideMapBounds : public PlannerException { public: - explicit GoalOutsideMapBounds(const std::string &description) - : PlannerException(description) {} + explicit GoalOutsideMapBounds(const std::string & description) + : PlannerException(description) {} }; class StartIsEqualToGoal : public PlannerException { public: - explicit StartIsEqualToGoal(const std::string &description) - : PlannerException(description) {} + explicit StartIsEqualToGoal(const std::string & description) + : PlannerException(description) {} }; class PlannerTFError : public PlannerException { public: - explicit PlannerTFError(const std::string &description) - : PlannerException(description) {} + explicit PlannerTFError(const std::string & description) + : PlannerException(description) {} }; } // namespace nav2_core diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 79ef4d3fde..03ccefdb8d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -36,14 +36,16 @@ using namespace std::chrono_literals; using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; -namespace nav2_planner { - -PlannerServer::PlannerServer(const rclcpp::NodeOptions &options) - : nav2_util::LifecycleNode("planner_server", "", options), - gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), - default_ids_{"GridBased"}, - default_types_{"nav2_navfn_planner/NavfnPlanner"}, - costmap_(nullptr) { +namespace nav2_planner +{ + +PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("planner_server", "", options), + gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), + default_ids_{"GridBased"}, + default_types_{"nav2_navfn_planner/NavfnPlanner"}, + costmap_(nullptr) +{ RCLCPP_INFO(get_logger(), "Creating"); // Declare this node's parameters @@ -65,21 +67,23 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions &options) costmap_thread_ = std::make_unique(costmap_ros_); } -PlannerServer::~PlannerServer() { +PlannerServer::~PlannerServer() +{ planners_.clear(); costmap_thread_.reset(); } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { +PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ RCLCPP_INFO(get_logger(), "Configuring"); costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); RCLCPP_DEBUG( - get_logger(), "Costmap size: %d,%d", - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + get_logger(), "Costmap size: %d,%d", + costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); tf_ = costmap_ros_->getTfBuffer(); @@ -90,18 +94,18 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { for (size_t i = 0; i != planner_ids_.size(); i++) { try { planner_types_[i] = nav2_util::get_plugin_type_param( - node, planner_ids_[i]); + node, planner_ids_[i]); nav2_core::GlobalPlanner::Ptr planner = - gp_loader_.createUniqueInstance(planner_types_[i]); + gp_loader_.createUniqueInstance(planner_types_[i]); RCLCPP_INFO( - get_logger(), "Created global planner plugin %s of type %s", - planner_ids_[i].c_str(), planner_types_[i].c_str()); + get_logger(), "Created global planner plugin %s of type %s", + planner_ids_[i].c_str(), planner_types_[i].c_str()); planner->configure(node, planner_ids_[i], tf_, costmap_ros_); planners_.insert({planner_ids_[i], planner}); - } catch (const pluginlib::PluginlibException &ex) { + } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create global planner. Exception: %s", - ex.what()); + get_logger(), "Failed to create global planner. Exception: %s", + ex.what()); return nav2_util::CallbackReturn::FAILURE; } } @@ -111,8 +115,8 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { } RCLCPP_INFO( - get_logger(), - "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); double expected_planner_frequency; get_parameter("expected_planner_frequency", expected_planner_frequency); @@ -120,9 +124,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { max_planner_duration_ = 1 / expected_planner_frequency; } else { RCLCPP_WARN( - get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } @@ -131,26 +135,27 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( - shared_from_this(), - "compute_path_to_pose", - std::bind(&PlannerServer::computePlan, this), - nullptr, - std::chrono::milliseconds(500), - true); + shared_from_this(), + "compute_path_to_pose", + std::bind(&PlannerServer::computePlan, this), + nullptr, + std::chrono::milliseconds(500), + true); action_server_poses_ = std::make_unique( - shared_from_this(), - "compute_path_through_poses", - std::bind(&PlannerServer::computePlanThroughPoses, this), - nullptr, - std::chrono::milliseconds(500), - true); + shared_from_this(), + "compute_path_through_poses", + std::bind(&PlannerServer::computePlanThroughPoses, this), + nullptr, + std::chrono::milliseconds(500), + true); return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { +PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ RCLCPP_INFO(get_logger(), "Activating"); plan_publisher_->on_activate(); @@ -166,14 +171,14 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { auto node = shared_from_this(); is_path_valid_service_ = node->create_service( - "is_path_valid", - std::bind( - &PlannerServer::isPathValid, this, - std::placeholders::_1, std::placeholders::_2)); + "is_path_valid", + std::bind( + &PlannerServer::isPathValid, this, + std::placeholders::_1, std::placeholders::_2)); // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PlannerServer::dynamicParametersCallback, this, _1)); + std::bind(&PlannerServer::dynamicParametersCallback, this, _1)); // create bond connection createBond(); @@ -182,7 +187,8 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { } nav2_util::CallbackReturn -PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ RCLCPP_INFO(get_logger(), "Deactivating"); action_server_pose_->deactivate(); @@ -204,7 +210,8 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { } nav2_util::CallbackReturn -PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ RCLCPP_INFO(get_logger(), "Cleaning up"); action_server_pose_.reset(); @@ -223,14 +230,16 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { } nav2_util::CallbackReturn -PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) { +PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(get_logger(), "Shutting down"); return nav2_util::CallbackReturn::SUCCESS; } template bool PlannerServer::isServerInactive( - std::unique_ptr> &action_server) { + std::unique_ptr> & action_server) +{ if (action_server == nullptr || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return true; @@ -239,7 +248,8 @@ bool PlannerServer::isServerInactive( return false; } -void PlannerServer::waitForCostmap() { +void PlannerServer::waitForCostmap() +{ // Don't compute a plan until costmap is valid (after clear costmap) rclcpp::Rate r(100); while (!costmap_ros_->isCurrent()) { @@ -249,7 +259,8 @@ void PlannerServer::waitForCostmap() { template bool PlannerServer::isCancelRequested( - std::unique_ptr> &action_server) { + std::unique_ptr> & action_server) +{ if (action_server->is_cancel_requested()) { RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); action_server->terminate_all(); @@ -261,8 +272,9 @@ bool PlannerServer::isCancelRequested( template void PlannerServer::getPreemptedGoalIfRequested( - std::unique_ptr> &action_server, - typename std::shared_ptr goal) { + std::unique_ptr> & action_server, + typename std::shared_ptr goal) +{ if (action_server->is_preempt_requested()) { goal = action_server->accept_pending_goal(); } @@ -270,9 +282,10 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( - std::unique_ptr> &action_server, - typename std::shared_ptr goal, - geometry_msgs::msg::PoseStamped &start) { + std::unique_ptr> & action_server, + typename std::shared_ptr goal, + geometry_msgs::msg::PoseStamped & start) +{ if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { @@ -285,13 +298,15 @@ bool PlannerServer::getStartPose( template bool PlannerServer::transformPosesToGlobalFrame( - std::unique_ptr> &action_server, - geometry_msgs::msg::PoseStamped &curr_start, - geometry_msgs::msg::PoseStamped &curr_goal) { + std::unique_ptr> & action_server, + geometry_msgs::msg::PoseStamped & curr_start, + geometry_msgs::msg::PoseStamped & curr_goal) +{ if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || - !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { + !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) + { RCLCPP_WARN( - get_logger(), "Could not transform the start or goal pose in the costmap frame"); + get_logger(), "Could not transform the start or goal pose in the costmap frame"); action_server->terminate_current(); return false; } @@ -301,30 +316,31 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( - std::unique_ptr> &action_server, - const geometry_msgs::msg::PoseStamped &goal, - const nav_msgs::msg::Path &path, - const std::string &planner_id) { + std::unique_ptr> & action_server, + const geometry_msgs::msg::PoseStamped & goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id) +{ if (path.poses.size() == 0) { RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path to (%.2f, %.2f)", planner_id.c_str(), - goal.pose.position.x, goal.pose.position.y); + get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_id.c_str(), + goal.pose.position.x, goal.pose.position.y); action_server->terminate_current(); return false; } RCLCPP_DEBUG( - get_logger(), - "Found valid path of size %zu to (%.2f, %.2f)", - path.poses.size(), goal.pose.position.x, - goal.pose.position.y); + get_logger(), + "Found valid path of size %zu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); return true; } -void -PlannerServer::computePlanThroughPoses() { +void PlannerServer::computePlanThroughPoses() +{ std::lock_guard lock(dynamic_params_lock_); auto start_time = steady_clock_.now(); @@ -345,8 +361,8 @@ PlannerServer::computePlanThroughPoses() { if (goal->goals.size() == 0) { RCLCPP_WARN( - get_logger(), - "Compute path through poses requested a plan with no viapoint poses, returning."); + get_logger(), + "Compute path through poses requested a plan with no viapoint poses, returning."); action_server_poses_->terminate_current(); } @@ -383,7 +399,7 @@ PlannerServer::computePlanThroughPoses() { // Concatenate paths together concat_path.poses.insert( - concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); concat_path.header = curr_path.header; } @@ -396,24 +412,25 @@ PlannerServer::computePlanThroughPoses() { if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { RCLCPP_WARN( - get_logger(), - "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); + get_logger(), + "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_poses_->succeeded_current(result); - } catch (std::exception &ex) { + } 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()); + 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(); } } void -PlannerServer::computePlan() { +PlannerServer::computePlan() +{ std::lock_guard lock(dynamic_params_lock_); auto start_time = steady_clock_.now(); @@ -458,77 +475,77 @@ PlannerServer::computePlan() { if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { RCLCPP_WARN( - get_logger(), - "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); + get_logger(), + "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) { + } catch (nav2_core::StartOccupied & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. " - "Start Pose (%.2f, %.2f) was occupied: \"%s\"", - goal->planner_id.c_str(), - start.pose.position.x, - start.pose.position.y, - ex.what()); + get_logger(), "%s plugin failed to plan path. " + "Start Pose (%.2f, %.2f) was occupied: \"%s\"", + goal->planner_id.c_str(), + start.pose.position.x, + start.pose.position.y, + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; action_server_pose_->terminate_current(result); - } catch (nav2_core::GoalOccupied &ex) { + } catch (nav2_core::GoalOccupied & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Pose (%.2f, %.2f) was occupied: \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + get_logger(), "%s plugin failed to plan path. Goal Pose (%.2f, %.2f) was occupied: \"%s\"", + goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); - } catch (nav2_core::NoValidPathCouldBeFound &ex) { + } catch (nav2_core::NoValidPathCouldBeFound & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; action_server_pose_->terminate_current(result); - } catch (nav2_core::PlannerTimedOut &ex) { + } catch (nav2_core::PlannerTimedOut & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); - } catch (nav2_core::StartOutsideMapBounds &ex) { + } catch (nav2_core::StartOutsideMapBounds & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start Outside map: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. Start Outside map: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); - } catch (nav2_core::GoalOutsideMapBounds &ex) { + } catch (nav2_core::GoalOutsideMapBounds & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); - } catch (nav2_core::StartIsEqualToGoal &ex) { + } catch (nav2_core::StartIsEqualToGoal & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; action_server_pose_->terminate_current(result); - } catch (nav2_core::PlannerTFError &ex) { + } catch (nav2_core::PlannerTFError & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", - goal->planner_id.c_str(), - ex.what()); + get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", + goal->planner_id.c_str(), + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; action_server_pose_->terminate_current(result); - } catch (std::exception &ex) { + } catch (std::exception & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan: \"%s\"", - goal->planner_id.c_str(), ex.what()); + get_logger(), "%s plugin failed to plan: \"%s\"", + goal->planner_id.c_str(), ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; action_server_pose_->terminate_current(result); } @@ -536,28 +553,29 @@ PlannerServer::computePlan() { nav_msgs::msg::Path PlannerServer::getPlan( - const geometry_msgs::msg::PoseStamped &start, - const geometry_msgs::msg::PoseStamped &goal, - const std::string &planner_id) { + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id) +{ RCLCPP_DEBUG( - get_logger(), "Attempting to a find path from (%.2f, %.2f) to " - "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, - goal.pose.position.x, goal.pose.position.y); + get_logger(), "Attempting to a find path from (%.2f, %.2f) to " + "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y); if (planners_.find(planner_id) != planners_.end()) { return planners_[planner_id]->createPlan(start, goal); } else { if (planners_.size() == 1 && planner_id.empty()) { RCLCPP_WARN_ONCE( - get_logger(), "No planners specified in action call. " - "Server will use only plugin %s in server." - " This warning will appear once.", planner_ids_concat_.c_str()); + get_logger(), "No planners specified in action call. " + "Server will use only plugin %s in server." + " This warning will appear once.", planner_ids_concat_.c_str()); return planners_[planners_.begin()->first]->createPlan(start, goal); } else { RCLCPP_ERROR( - get_logger(), "planner %s is not a valid planner. " - "Planner names are: %s", planner_id.c_str(), - planner_ids_concat_.c_str()); + get_logger(), "planner %s is not a valid planner. " + "Planner names are: %s", planner_id.c_str(), + planner_ids_concat_.c_str()); } } @@ -565,7 +583,8 @@ PlannerServer::getPlan( } void -PlannerServer::publishPlan(const nav_msgs::msg::Path &path) { +PlannerServer::publishPlan(const nav_msgs::msg::Path & path) +{ auto msg = std::make_unique(path); if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) { plan_publisher_->publish(std::move(msg)); @@ -573,8 +592,9 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path &path) { } void PlannerServer::isPathValid( - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request, + std::shared_ptr response) +{ response->is_valid = true; if (request->path.poses.empty()) { @@ -592,8 +612,8 @@ void PlannerServer::isPathValid( geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position; current_distance = nav2_util::geometry_utils::euclidean_distance( - current_point, - path_point); + current_point, + path_point); if (current_distance < closest_distance) { closest_point_index = i; @@ -609,12 +629,13 @@ void PlannerServer::isPathValid( unsigned int my = 0; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); + request->path.poses[i].pose.position.x, + request->path.poses[i].pose.position.y, mx, my); unsigned int cost = costmap_->getCost(mx, my); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { response->is_valid = false; } } @@ -622,12 +643,13 @@ void PlannerServer::isPathValid( } rcl_interfaces::msg::SetParametersResult -PlannerServer::dynamicParametersCallback(std::vector parameters) { +PlannerServer::dynamicParametersCallback(std::vector parameters) +{ std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto &type = parameter.get_type(); - const auto &name = parameter.get_name(); + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); if (type == ParameterType::PARAMETER_DOUBLE) { if (name == "expected_planner_frequency") { @@ -635,9 +657,9 @@ PlannerServer::dynamicParametersCallback(std::vector paramete max_planner_duration_ = 1 / parameter.as_double(); } else { RCLCPP_WARN( - get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); max_planner_duration_ = 0.0; } } diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 2249acdf79..030527fee6 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -204,14 +204,16 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GoalOccupied("Failed to compute path, " - "goal is occupied with no tolerance."); + throw nav2_core::GoalOccupied( + "Failed to compute path, " + "goal is occupied with no tolerance."); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Starting point in lethal space!" - "Cannot create feasible plan."); + throw nav2_core::StartOccupied( + "Starting point in lethal space!" + "Cannot create feasible plan."); } return true; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 15c6f283ad..5c0b37252e 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -250,9 +250,10 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; - - if (!_a_star->createPath(path, num_iterations, - _tolerance / static_cast(costmap->getResolution()))) { + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { From 85493f9a2fa4e33441fb7303fe93ebf9641c9790 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 9 Sep 2022 15:38:08 -0400 Subject: [PATCH 14/38] code review --- .../action/compute_path_to_pose_action.hpp | 2 +- .../action/compute_path_to_pose_action.cpp | 6 ++-- nav2_planner/src/planner_server.cpp | 33 ++++++++++++++----- .../nav2_smac_planner/smac_planner_hybrid.hpp | 5 +++ nav2_smac_planner/src/a_star.cpp | 8 ++--- 5 files changed, 36 insertions(+), 18 deletions(-) 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 0306375d51..5db45a7d75 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 @@ -71,7 +71,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( - "global_planner_error_code", "The global planner error code"), + "compute_path_to_pose_error_code", "The global planner error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), 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 0c52933b4c..a255da4b91 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 @@ -41,7 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("global_planner_result_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } @@ -50,7 +50,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("global_planner_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("global_planner_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 03ccefdb8d..03e7add019 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -502,50 +502,67 @@ PlannerServer::computePlan() action_server_pose_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. No Valid Path found: \"%s\"", + get_logger(), "%s plugin failed to plan path to goal pose (%.2f, %.2f). " + "No Valid Path found: \"%s\"", goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Timeout occurred: \"%s\"", + get_logger(), "%s plugin failed to plan path to goal pose (%.2f, %.2f). " + "Timeout occurred: \"%s\"", goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start Outside map: \"%s\"", + get_logger(), "%s plugin failed to plan path. Start Outside map (%.2f, %.2f): \"%s\"", goal->planner_id.c_str(), + start.pose.position.x, + start.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Outside map: \"%s\"", + get_logger(), "%s plugin failed to plan path. Goal Outside map (%.2f, %.2f): \"%s\"", goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::StartIsEqualToGoal & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", + get_logger(), "%s plugin failed to plan path. Start equal to goal (%0.2f, %0.2f): \"%s\"", goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start equal to goal: \"%s\"", + get_logger(), "%s plugin failed to plan path to (%0.2f, %0.2f). Transform error: \"%s\"", goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; action_server_pose_->terminate_current(result); } catch (std::exception & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan: \"%s\"", - goal->planner_id.c_str(), ex.what()); + get_logger(), "%s plugin failed to plan to (%0.2f, %0.2f): \"%s\"", + goal->planner_id.c_str(), + goal->goal.pose.position.x, + goal->goal.pose.position.y, + ex.what()); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; action_server_pose_->terminate_current(result); } diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 51f6608b5f..993858f6af 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -36,6 +36,11 @@ namespace nav2_smac_planner { +/** + * Note: All exceptions thrown are handled by the planner server and returned to the action + * requester + */ + class SmacPlannerHybrid : public nav2_core::GlobalPlanner { public: diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 030527fee6..859cd46758 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -204,16 +204,12 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GoalOccupied( - "Failed to compute path, " - "goal is occupied with no tolerance."); + throw nav2_core::GoalOccupied("Failed to compute path, goal is occupied with no tolerance."); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied( - "Starting point in lethal space!" - "Cannot create feasible plan."); + throw nav2_core::StartOccupied("Starting point in lethal space! Cannot create feasible plan."); } return true; From 6612d5833ab3e3941282326e14cdf72c7e87e78e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 9 Sep 2022 16:07:14 -0400 Subject: [PATCH 15/38] navfn exceptions --- .../nav2_navfn_planner/navfn_planner.hpp | 1 + nav2_navfn_planner/src/navfn_planner.cpp | 26 +++++-------------- 2 files changed, 7 insertions(+), 20 deletions(-) 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 8090d817ef..454dd44858 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/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" diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ede7f38b8..32d35d831b 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -151,8 +151,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( 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; + throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); } path.header.stamp = clock_->now(); path.header.frame_id = global_frame_; @@ -172,9 +171,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_) ); } @@ -222,12 +220,7 @@ NavfnPlanner::makePlan( 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; + throw nav2_core::StartOutsideMapBounds("Robot's start position is off the global costmap"); } // clear the starting cell within the costmap because we know it can't be an obstacle @@ -252,11 +245,7 @@ NavfnPlanner::makePlan( 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; + throw nav2_core::GoalOutsideMapBounds("Goal is off the global costmap"); } int map_goal[2]; @@ -386,10 +375,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."); + throw nav2_core::GoalOutsideMapBounds("Goal is off the global costmap"); return false; } From bb5be0abb436437c9bd2e6f39675e9db33cb3c9e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 9 Sep 2022 17:07:07 -0400 Subject: [PATCH 16/38] theta_star_planner --- .../include/nav2_theta_star_planner/theta_star_planner.hpp | 1 + nav2_theta_star_planner/src/theta_star_planner.cpp | 7 +++---- 2 files changed, 4 insertions(+), 4 deletions(-) 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 211962090d..7b124f9764 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,6 +25,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_core/exceptions.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index cc98649ccc..dc441f548f 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -93,8 +93,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); if (mx_start == mx_goal && my_start == my_goal) { if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); - return global_path; + throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; @@ -154,13 +153,13 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; if (planner_->isUnsafeToPlan()) { - RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); global_path.poses.clear(); + throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); } else if (planner_->generatePath(path)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { - RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); + throw nav2_core::NoValidPathCouldBeFound("Could not generate path between the given poses"); } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; From ea413622dedc25911c0df10b907ad5262fa869a1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 13 Sep 2022 18:17:13 -0400 Subject: [PATCH 17/38] fix code review --- .../plugins/action/compute_path_to_pose_action.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 879f8f6bcf..b5e0e9084c 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 @@ -40,6 +40,7 @@ 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; @@ -49,7 +50,6 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; setOutput("compute_path_to_pose_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -58,6 +58,8 @@ 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; } From 5b01c2a808c8a287dac5987c2dcac8e275a2d67b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 14 Sep 2022 14:49:00 -0400 Subject: [PATCH 18/38] wrote timeout exception --- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_lattice.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 5c0b37252e..7846492be6 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -257,7 +257,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); } } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index cd5043bece..45932a5c70 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -318,7 +318,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 6de619b879..dd90e7abb4 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -266,7 +266,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { - throw nav2_core::NoValidPathCouldBeFound("exceeded maximum iterations"); + throw nav2_core::PlannerTimedOut("exceeded maximum iterations"); } } From d615e2d2d9556811b8f17c5d30576830f8b46cfe Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 10:36:57 -0400 Subject: [PATCH 19/38] consistent exception throwing across planners --- nav2_core/include/nav2_core/exceptions.hpp | 36 ++++++------ nav2_navfn_planner/src/navfn_planner.cpp | 35 ++++++------ .../include/nav2_planner/planner_server.hpp | 19 ------- nav2_planner/src/planner_server.cpp | 57 ++++--------------- nav2_smac_planner/src/smac_planner_2d.cpp | 8 ++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 9 ++- .../src/smac_planner_lattice.cpp | 8 ++- .../src/theta_star_planner.cpp | 23 ++++++-- 8 files changed, 86 insertions(+), 109 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index 2c37106ea0..fd923abde6 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -50,52 +50,52 @@ class PlannerException : public std::runtime_error : std::runtime_error(description) {} }; -class GoalOccupied : public PlannerException +class StartOccupied : public PlannerException { public: - explicit GoalOccupied(const std::string & description) - : PlannerException(description) {} + explicit StartOccupied(const std::string & description) + : PlannerException(description) {} }; -class StartOccupied : public PlannerException +class GoalOccupied : public PlannerException { public: - explicit StartOccupied(const std::string & description) + explicit GoalOccupied(const std::string & description) : PlannerException(description) {} }; -class NoValidPathCouldBeFound : public PlannerException +class StartOutsideMapBounds : public PlannerException { public: - explicit NoValidPathCouldBeFound(const std::string & description) - : PlannerException(description) {} + explicit StartOutsideMapBounds(const std::string & description) + : PlannerException(description) {} }; -class PlannerTimedOut : public PlannerException +class GoalOutsideMapBounds : public PlannerException { public: - explicit PlannerTimedOut(const std::string & description) - : PlannerException(description) {} + explicit GoalOutsideMapBounds(const std::string & description) + : PlannerException(description) {} }; -class StartOutsideMapBounds : public PlannerException +class StartIsEqualToGoal : public PlannerException { public: - explicit StartOutsideMapBounds(const std::string & description) - : PlannerException(description) {} + explicit StartIsEqualToGoal(const std::string & description) + : PlannerException(description) {} }; -class GoalOutsideMapBounds : public PlannerException +class NoValidPathCouldBeFound : public PlannerException { public: - explicit GoalOutsideMapBounds(const std::string & description) + explicit NoValidPathCouldBeFound(const std::string & description) : PlannerException(description) {} }; -class StartIsEqualToGoal : public PlannerException +class PlannerTimedOut : public PlannerException { public: - explicit StartIsEqualToGoal(const std::string & description) + explicit PlannerTimedOut(const std::string & description) : PlannerException(description) {} }; diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 32d35d831b..9bf1f23119 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -134,6 +134,22 @@ 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 outside map bounds"); + } + + if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } + + if (costmap_->getCost(mx_start, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); + } + + if (costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied("Failed to create a unique pose path because of obstacles"); + } // Update planner based on the new costmap size if (isPlannerOutOfDate()) { @@ -148,11 +164,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) { - throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); - } path.header.stamp = clock_->now(); path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; @@ -219,9 +230,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)) { - throw nav2_core::StartOutsideMapBounds("Robot's start position is off the global costmap"); - } + worldToMap(wx, wy, mx, my); // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); @@ -244,10 +253,7 @@ NavfnPlanner::makePlan( wx = goal.position.x; wy = goal.position.y; - if (!worldToMap(wx, wy, mx, my)) { - throw nav2_core::GoalOutsideMapBounds("Goal is off the global costmap"); - } - + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; @@ -374,10 +380,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)) { - throw nav2_core::GoalOutsideMapBounds("Goal is off the global costmap"); - return false; - } + worldToMap(wx, wy, mx, my); int map_goal[2]; map_goal[0] = mx; diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 7811c74989..1ca064ab71 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -153,39 +153,20 @@ class PlannerServer : public nav2_util::LifecycleNode */ template bool getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr 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 bool transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal); - /** - * @brief Validate that the path contains a meaningful path - * @param action_server Action server to terminate if required - * @param goal Goal Current goal - * @param path Current path - * @param planner_id The planner ID used to generate the path - * @return bool If path is valid - */ - template - bool validatePath( - std::unique_ptr> & 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 action_server_pose_; std::unique_ptr action_server_poses_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 03e7add019..1941e97207 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -282,63 +282,31 @@ void PlannerServer::getPreemptedGoalIfRequested( template bool PlannerServer::getStartPose( - std::unique_ptr> & action_server, typename std::shared_ptr goal, geometry_msgs::msg::PoseStamped & start) { if (goal->use_start) { start = goal->start; } else if (!costmap_ros_->getRobotPose(start)) { - action_server->terminate_current(); return false; } return true; } -template bool PlannerServer::transformPosesToGlobalFrame( - std::unique_ptr> & action_server, geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal) { if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) { - RCLCPP_WARN( - get_logger(), "Could not transform the start or goal pose in the costmap frame"); - action_server->terminate_current(); return false; } return true; } -template -bool PlannerServer::validatePath( - std::unique_ptr> & action_server, - const geometry_msgs::msg::PoseStamped & goal, - const nav_msgs::msg::Path & path, - const std::string & planner_id) -{ - if (path.poses.size() == 0) { - RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path to (%.2f, %.2f)", planner_id.c_str(), - goal.pose.position.x, goal.pose.position.y); - action_server->terminate_current(); - return false; - } - - RCLCPP_DEBUG( - get_logger(), - "Found valid path of size %zu to (%.2f, %.2f)", - path.poses.size(), goal.pose.position.x, - goal.pose.position.y); - - return true; -} - void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); @@ -368,8 +336,8 @@ void PlannerServer::computePlanThroughPoses() // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!getStartPose(action_server_poses_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Get consecutive paths through these points @@ -385,16 +353,15 @@ void PlannerServer::computePlanThroughPoses() curr_goal = goal->goals[i]; // Transform them into the global frame - if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) { - return; + if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - // check path for validity - if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) { - return; + if (curr_path.poses.empty()) { + throw nav2_core::PlannerException(goal->planner_id + "generated a empty path"); } // Concatenate paths together @@ -451,20 +418,20 @@ PlannerServer::computePlan() getPreemptedGoalIfRequested(action_server_pose_, goal); // Use start pose if provided otherwise use current robot pose - if (!getStartPose(action_server_pose_, goal, start)) { - return; + if (!getStartPose(goal, start)) { + throw nav2_core::PlannerTFError("Unable to get start pose"); } // Transform them into the global frame geometry_msgs::msg::PoseStamped goal_pose = goal->goal; - if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) { - return; + if (!transformPosesToGlobalFrame(start, goal_pose)) { + throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } result->path = getPlan(start, goal_pose, goal->planner_id); - if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { - return; + if (result->path.poses.empty()) { + throw nav2_core::PlannerException(goal->planner_id + "generated a empty path"); } // Publish the plan for visualization purposes diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 7846492be6..0afbb85520 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -211,11 +211,15 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point unsigned int mx_start, my_start, mx_goal, my_goal; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + } _a_star->setStart(mx_start, my_start, 0); // Set goal point - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } _a_star->setGoal(mx_goal, my_goal, 0); // Setup message diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 45932a5c70..9180ba5417 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -273,7 +273,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + } + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); @@ -286,7 +289,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index dd90e7abb4..39b496d744 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -234,13 +234,17 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; - _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + } _a_star->setStart( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } _a_star->setGoal( mx, my, NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index dc441f548f..76b35ef8c5 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -89,12 +89,25 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (!planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, + my_start)) { + throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + } + + if (!planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, + my_goal)) { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } + + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); + } + + if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::GoalOccupied("Failed to create a unique pose path because of obstacles"); + } + if (mx_start == mx_goal && my_start == my_goal) { - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); - } global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; geometry_msgs::msg::PoseStamped pose; From 63774317f5a816924c54d7837ae03f968000fa69 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 11:37:31 -0400 Subject: [PATCH 20/38] code review --- .../action/compute_path_to_pose_action.hpp | 2 +- nav2_msgs/action/ComputePathToPose.action | 10 +-- .../include/nav2_planner/planner_server.hpp | 5 ++ nav2_planner/src/planner_server.cpp | 79 ++++++------------- nav2_smac_planner/src/smac_planner_2d.cpp | 1 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 + .../src/smac_planner_lattice.cpp | 1 + 7 files changed, 36 insertions(+), 63 deletions(-) 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 9feaa250d6..e980012cc4 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 @@ -76,7 +76,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( - "compute_path_to_pose_error_code", "The global planner error code"), + "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"), diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 7506c44b7e..59ce0cd022 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -3,11 +3,11 @@ int16 NONE=0 int16 UNKNOWN=1 int16 START_OCCUPIED=2 int16 GOAL_OCCUPIED=3 -int16 NO_VALID_PATH=4 -int16 TIMEOUT=5 -int16 START_OUTSIDE_MAP=6 -int16 GOAL_OUTSIDE_MAP=7 -int16 START_IS_EQUAL_TO_GOAL=8 +int16 START_OUTSIDE_MAP=4 +int16 GOAL_OUTSIDE_MAP=5 +int16 START_IS_EQUAL_TO_GOAL=6 +int16 NO_VALID_PATH=7 +int16 TIMEOUT=8 int16 TF_ERROR=9 #goal definition diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 1ca064ab71..6edd6298a1 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -198,6 +198,11 @@ 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 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 1941e97207..e5c35274c2 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -449,87 +449,39 @@ PlannerServer::computePlan() action_server_pose_->succeeded_current(result); } catch (nav2_core::StartOccupied & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. " - "Start Pose (%.2f, %.2f) was occupied: \"%s\"", - goal->planner_id.c_str(), - start.pose.position.x, - start.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOccupied & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Pose (%.2f, %.2f) was occupied: \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path to goal pose (%.2f, %.2f). " - "No Valid Path found: \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path to goal pose (%.2f, %.2f). " - "Timeout occurred: \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start Outside map (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), - start.pose.position.x, - start.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Goal Outside map (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::StartIsEqualToGoal & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path. Start equal to goal (%0.2f, %0.2f): \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan path to (%0.2f, %0.2f). Transform error: \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; action_server_pose_->terminate_current(result); } catch (std::exception & ex) { - RCLCPP_WARN( - get_logger(), "%s plugin failed to plan to (%0.2f, %0.2f): \"%s\"", - goal->planner_id.c_str(), - goal->goal.pose.position.x, - goal->goal.pose.position.y, - ex.what()); + exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; action_server_pose_->terminate_current(result); } @@ -654,6 +606,19 @@ PlannerServer::dynamicParametersCallback(std::vector paramete return result; } +void PlannerServer::exceptionWarning(const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal, + const std::string &planner_id, + const std::exception &ex) +{ + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", + planner_id.c_str(), + start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y, + ex.what()); +} + } // namespace nav2_planner #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 0afbb85520..b614ef0b9a 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -254,6 +254,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; + // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 9180ba5417..ee40a6fcbe 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -319,6 +319,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( NodeHybrid::CoordinateVector path; int num_iterations = 0; + // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath(path, num_iterations, 0)) { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 39b496d744..27f8415a86 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -266,6 +266,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( int num_iterations = 0; std::string error; + // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); From 71ae1a8010636195cf3c6de800d1f00d71eae4a1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 11:43:40 -0400 Subject: [PATCH 21/38] remove --- .../include/nav2_smac_planner/smac_planner_hybrid.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 993858f6af..51f6608b5f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -36,11 +36,6 @@ namespace nav2_smac_planner { -/** - * Note: All exceptions thrown are handled by the planner server and returned to the action - * requester - */ - class SmacPlannerHybrid : public nav2_core::GlobalPlanner { public: From 1a73968c9a6171d68c1bf648fd897929c6f5f123 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 13:47:41 -0400 Subject: [PATCH 22/38] uncrusitfy --- .../include/nav2_planner/planner_server.hpp | 9 +++++---- nav2_planner/src/planner_server.cpp | 19 ++++++++++--------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 6edd6298a1..a42bca0209 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -198,10 +198,11 @@ 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); + 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 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e5c35274c2..03bb114924 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -606,17 +606,18 @@ PlannerServer::dynamicParametersCallback(std::vector paramete return result; } -void PlannerServer::exceptionWarning(const geometry_msgs::msg::PoseStamped &start, - const geometry_msgs::msg::PoseStamped &goal, - const std::string &planner_id, - const std::exception &ex) +void PlannerServer::exceptionWarning( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id, + const std::exception & ex) { RCLCPP_WARN( - get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", - planner_id.c_str(), - start.pose.position.x, start.pose.position.y, - goal.pose.position.x, goal.pose.position.y, - ex.what()); + get_logger(), "%s plugin failed to plan from (%.2f, %.2f) to (%0.2f, %.2f): \"%s\"", + planner_id.c_str(), + start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y, + ex.what()); } } // namespace nav2_planner From a97fa2c3a16f71e0c73f38ed549501085999f700 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 13:51:41 -0400 Subject: [PATCH 23/38] uncrusify --- nav2_core/include/nav2_core/exceptions.hpp | 8 ++++---- nav2_navfn_planner/src/navfn_planner.cpp | 2 +- nav2_theta_star_planner/src/theta_star_planner.cpp | 10 ++++++---- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index fd923abde6..dd440e6544 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -54,7 +54,7 @@ class StartOccupied : public PlannerException { public: explicit StartOccupied(const std::string & description) - : PlannerException(description) {} + : PlannerException(description) {} }; class GoalOccupied : public PlannerException @@ -68,21 +68,21 @@ class StartOutsideMapBounds : public PlannerException { public: explicit StartOutsideMapBounds(const std::string & description) - : PlannerException(description) {} + : PlannerException(description) {} }; class GoalOutsideMapBounds : public PlannerException { public: explicit GoalOutsideMapBounds(const std::string & description) - : PlannerException(description) {} + : PlannerException(description) {} }; class StartIsEqualToGoal : public PlannerException { public: explicit StartIsEqualToGoal(const std::string & description) - : PlannerException(description) {} + : PlannerException(description) {} }; class NoValidPathCouldBeFound : public PlannerException diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 9bf1f23119..b4ff78d4a9 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -183,7 +183,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (!makePlan(start.pose, goal.pose, tolerance_, path)) { throw nav2_core::NoValidPathCouldBeFound( - "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); + "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 76b35ef8c5..71c8d3211c 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -89,13 +89,15 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; - if (!planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, - my_start)) { + if (!planner_->costmap_->worldToMap( + start.pose.position.x, start.pose.position.y, mx_start, my_start)) + { throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); } - if (!planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, - my_goal)) { + if (!planner_->costmap_->worldToMap( + goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) + { throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); } From 7f1b03e299ee41282172c151ec0a5d861c00da17 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 14:06:50 -0400 Subject: [PATCH 24/38] catch exception --- nav2_theta_star_planner/test/test_theta_star.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 7776179963..efd41a6c88 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -174,8 +174,12 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { } goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - path = planner_2d->createPlan(start, goal); - EXPECT_EQ(static_cast(path.poses.size()), 0); + + try { + path = planner_2d->createPlan(start, goal); + } catch (std::exception & ex) { + EXPECT_STREQ("Failed to create a unique pose path because of obstacles", ex.what()); + } planner_2d->deactivate(); planner_2d->cleanup(); From 8eb398f77df3a342f14f6f7f0d1293e7d471127c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 14:28:39 -0400 Subject: [PATCH 25/38] expect throw --- nav2_theta_star_planner/test/test_theta_star.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index efd41a6c88..264f737de4 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -175,11 +175,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - try { - path = planner_2d->createPlan(start, goal); - } catch (std::exception & ex) { - EXPECT_STREQ("Failed to create a unique pose path because of obstacles", ex.what()); - } + EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); From 15867f2c1522e9f721f1f5b14a3d6564d7e278d7 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 15:21:13 -0400 Subject: [PATCH 26/38] update string of exceptions --- nav2_navfn_planner/src/navfn_planner.cpp | 18 +++++++++++++----- nav2_smac_planner/src/a_star.cpp | 4 ++-- nav2_smac_planner/src/smac_planner_2d.cpp | 8 ++++++-- .../src/theta_star_planner.cpp | 16 ++++++++++++---- 4 files changed, 33 insertions(+), 13 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index b4ff78d4a9..09d0e18466 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -136,19 +136,27 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( #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 outside map bounds"); + 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 outside map bounds"); + 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_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); + 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 (costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::GoalOccupied("Failed to create a unique pose path because of obstacles"); + 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 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 859cd46758..da588240c7 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -204,12 +204,12 @@ bool AStarAlgorithm::areInputsValid() if (getToleranceHeuristic() < 0.001 && !_goal->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::GoalOccupied("Failed to compute path, goal is occupied with no tolerance."); + throw nav2_core::GoalOccupied("Goal was in lethal cost"); } // Check if starting point is valid if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Starting point in lethal space! Cannot create feasible plan."); + throw nav2_core::StartOccupied("Start was in lethal cost"); } return true; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index b614ef0b9a..56451e7e06 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -212,13 +212,17 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Set starting point 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 outside map bounds"); + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); } _a_star->setStart(mx_start, my_start, 0); // Set goal point if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { - throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); } _a_star->setGoal(mx_goal, my_goal, 0); diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 71c8d3211c..9f4930da72 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -92,21 +92,29 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( if (!planner_->costmap_->worldToMap( start.pose.position.x, start.pose.position.y, mx_start, my_start)) { - throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + 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 (!planner_->costmap_->worldToMap( goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { - throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + 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 (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Failed to create a unique pose path because of obstacles"); + 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 (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::GoalOccupied("Failed to create a unique pose path because of obstacles"); + 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"); } if (mx_start == mx_goal && my_start == my_goal) { From ebf717f4cc86547d26e32c8d3c34a12432ed15e9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 21 Sep 2022 15:48:39 -0400 Subject: [PATCH 27/38] throw with coords --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 8 ++++++-- nav2_smac_planner/src/smac_planner_lattice.cpp | 8 ++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ee40a6fcbe..e1c76178f7 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -274,7 +274,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { - throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); } double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; @@ -290,7 +292,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Set goal point, in A* bin search coordinates if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { - throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); } orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; while (orientation_bin < 0.0) { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 27f8415a86..b43bb87332 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -235,7 +235,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set starting point, in A* bin search coordinates unsigned int mx, my; if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { - throw nav2_core::StartOutsideMapBounds("Start outside map bounds"); + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); } _a_star->setStart( mx, my, @@ -243,7 +245,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Set goal point, in A* bin search coordinates if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { - throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + throw nav2_core::GoalOutsideMapBounds( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); } _a_star->setGoal( mx, my, From 6d8bb50cc9b0e17b1b203dd9a2c7e29d28255988 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 22 Sep 2022 21:11:44 -0400 Subject: [PATCH 28/38] removed start == goal error code --- nav2_core/include/nav2_core/exceptions.hpp | 7 ------- nav2_msgs/action/ComputePathToPose.action | 7 +++---- nav2_planner/src/planner_server.cpp | 4 ---- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/nav2_core/include/nav2_core/exceptions.hpp b/nav2_core/include/nav2_core/exceptions.hpp index dd440e6544..19a113d2e4 100644 --- a/nav2_core/include/nav2_core/exceptions.hpp +++ b/nav2_core/include/nav2_core/exceptions.hpp @@ -78,13 +78,6 @@ class GoalOutsideMapBounds : public PlannerException : PlannerException(description) {} }; -class StartIsEqualToGoal : public PlannerException -{ -public: - explicit StartIsEqualToGoal(const std::string & description) - : PlannerException(description) {} -}; - class NoValidPathCouldBeFound : public PlannerException { public: diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 59ce0cd022..71b5e8afc9 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -5,10 +5,9 @@ int16 START_OCCUPIED=2 int16 GOAL_OCCUPIED=3 int16 START_OUTSIDE_MAP=4 int16 GOAL_OUTSIDE_MAP=5 -int16 START_IS_EQUAL_TO_GOAL=6 -int16 NO_VALID_PATH=7 -int16 TIMEOUT=8 -int16 TF_ERROR=9 +int16 NO_VALID_PATH=6 +int16 TIMEOUT=7 +int16 TF_ERROR=8 #goal definition geometry_msgs/PoseStamped goal diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 03bb114924..5c011622dd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -472,10 +472,6 @@ PlannerServer::computePlan() exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); - } catch (nav2_core::StartIsEqualToGoal & ex) { - exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_IS_EQUAL_TO_GOAL; - 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; From 8b236951ad2fb9a80e30a3a46d04b6b7da9e60c0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 26 Sep 2022 10:02:25 -0400 Subject: [PATCH 29/38] code review --- nav2_smac_planner/src/smac_planner_2d.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 56451e7e06..46dfa98fb2 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -241,8 +241,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); - return plan; + throw nav2_core::StartOccupied("Start was in lethal cost"); } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal From 9d0484e790f9c5c71780de64dc723565bafef819 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 26 Sep 2022 10:31:45 -0400 Subject: [PATCH 30/38] code review --- .../include/nav2_planner/planner_server.hpp | 14 +++++++++ nav2_planner/src/planner_server.cpp | 31 +++++++++++++++---- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index a42bca0209..61b9ad4eb6 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -171,6 +171,20 @@ class PlannerServer : public nav2_util::LifecycleNode std::unique_ptr action_server_pose_; std::unique_ptr action_server_poses_; + /** + * @brief Validate that the path contains a meaningful path + * @param action_server Action server to terminate if required + * @param goal Goal Current goal + * @param path Current path + * @param planner_id The planner ID used to generate the path + * @return bool If path is valid + */ + template + bool validatePath( + const geometry_msgs::msg::PoseStamped & curr_goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id); + /** * @brief The action server callback which calls planner to get the path * ComputePathToPose diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 5c011622dd..77a57d3c3c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -307,6 +307,29 @@ bool PlannerServer::transformPosesToGlobalFrame( return true; } +template +bool PlannerServer::validatePath( + const geometry_msgs::msg::PoseStamped & goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id) +{ + if (path.poses.empty()) { + RCLCPP_WARN( + get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_id.c_str(), + goal.pose.position.x, goal.pose.position.y); + throw nav2_core::PlannerException(planner_id + "generated a empty path"); + } + + RCLCPP_DEBUG( + get_logger(), + "Found valid path of size %zu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); + + return true; +} + void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); @@ -360,9 +383,7 @@ void PlannerServer::computePlanThroughPoses() // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - if (curr_path.poses.empty()) { - throw nav2_core::PlannerException(goal->planner_id + "generated a empty path"); - } + validatePath(curr_goal, curr_path, goal->planner_id); // Concatenate paths together concat_path.poses.insert( @@ -430,9 +451,7 @@ PlannerServer::computePlan() result->path = getPlan(start, goal_pose, goal->planner_id); - if (result->path.poses.empty()) { - throw nav2_core::PlannerException(goal->planner_id + "generated a empty path"); - } + validatePath(goal_pose, result->path, goal->planner_id); // Publish the plan for visualization purposes publishPlan(result->path); From 93d98eb988794fa691309c34a4c072e66d827efb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 26 Sep 2022 10:47:01 -0400 Subject: [PATCH 31/38] uncrustify --- .../include/nav2_planner/planner_server.hpp | 10 +++++----- nav2_planner/src/planner_server.cpp | 20 +++++++++---------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 61b9ad4eb6..c3389f8a07 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -179,11 +179,11 @@ class PlannerServer : public nav2_util::LifecycleNode * @param planner_id The planner ID used to generate the path * @return bool If path is valid */ - template - bool validatePath( - const geometry_msgs::msg::PoseStamped & curr_goal, - const nav_msgs::msg::Path & path, - const std::string & planner_id); + template + bool validatePath( + const geometry_msgs::msg::PoseStamped & curr_goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id); /** * @brief The action server callback which calls planner to get the path diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 77a57d3c3c..354fb2ab79 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -309,23 +309,23 @@ bool PlannerServer::transformPosesToGlobalFrame( template bool PlannerServer::validatePath( - const geometry_msgs::msg::PoseStamped & goal, - const nav_msgs::msg::Path & path, - const std::string & planner_id) + const geometry_msgs::msg::PoseStamped & goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id) { if (path.poses.empty()) { RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path to (%.2f, %.2f)", planner_id.c_str(), - goal.pose.position.x, goal.pose.position.y); + get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_id.c_str(), + goal.pose.position.x, goal.pose.position.y); throw nav2_core::PlannerException(planner_id + "generated a empty path"); } RCLCPP_DEBUG( - get_logger(), - "Found valid path of size %zu to (%.2f, %.2f)", - path.poses.size(), goal.pose.position.x, - goal.pose.position.y); + get_logger(), + "Found valid path of size %zu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); return true; } From bf970252f9115c0ece578e1553f099011c2535c1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 27 Sep 2022 21:05:47 -0400 Subject: [PATCH 32/38] code review --- nav2_planner/include/nav2_planner/planner_server.hpp | 8 ++++---- nav2_planner/src/planner_server.cpp | 10 +++++++--- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c3389f8a07..4048775001 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -167,10 +167,6 @@ class PlannerServer : public nav2_util::LifecycleNode geometry_msgs::msg::PoseStamped & curr_start, geometry_msgs::msg::PoseStamped & curr_goal); - // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_pose_; - std::unique_ptr action_server_poses_; - /** * @brief Validate that the path contains a meaningful path * @param action_server Action server to terminate if required @@ -225,6 +221,10 @@ class PlannerServer : public nav2_util::LifecycleNode rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + // Our action server implements the ComputePathToPose action + std::unique_ptr action_server_pose_; + std::unique_ptr action_server_poses_; + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 354fb2ab79..c7a90bcbbe 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -318,7 +318,7 @@ bool PlannerServer::validatePath( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", planner_id.c_str(), goal.pose.position.x, goal.pose.position.y); - throw nav2_core::PlannerException(planner_id + "generated a empty path"); + return false; } RCLCPP_DEBUG( @@ -383,7 +383,9 @@ void PlannerServer::computePlanThroughPoses() // Get plan from start -> goal nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); - validatePath(curr_goal, curr_path, goal->planner_id); + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + } // Concatenate paths together concat_path.poses.insert( @@ -451,7 +453,9 @@ PlannerServer::computePlan() result->path = getPlan(start, goal_pose, goal->planner_id); - validatePath(goal_pose, result->path, goal->planner_id); + if (!validatePath(goal_pose, result->path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + } // Publish the plan for visualization purposes publishPlan(result->path); From 0234c00529bb94af4f26727a233258e060a4064a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 28 Sep 2022 08:44:15 -0400 Subject: [PATCH 33/38] message order --- nav2_msgs/action/ComputePathToPose.action | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 71b5e8afc9..8a2ffb0089 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,13 +1,14 @@ # Error codes +# Note: The expected priority order of the errors should match the message order int16 NONE=0 int16 UNKNOWN=1 -int16 START_OCCUPIED=2 -int16 GOAL_OCCUPIED=3 -int16 START_OUTSIDE_MAP=4 -int16 GOAL_OUTSIDE_MAP=5 -int16 NO_VALID_PATH=6 -int16 TIMEOUT=7 -int16 TF_ERROR=8 +int16 TF_ERROR=2 #system failure +int16 START_OUTSIDE_MAP=3 #request failure +int16 GOAL_OUTSIDE_MAP=4 #request failure +int16 START_OCCUPIED=5 #run-time failure +int16 GOAL_OCCUPIED=6 #run-time failure +int16 TIMEOUT=7 #run-time failure +int16 NO_VALID_PATH=8 #run-time failure #goal definition geometry_msgs/PoseStamped goal From 5ce061f35607ee917da41050843956116ab28a50 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 28 Sep 2022 14:05:00 -0400 Subject: [PATCH 34/38] remove remarks --- nav2_msgs/action/ComputePathToPose.action | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 8a2ffb0089..7e727088a1 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -2,13 +2,13 @@ # Note: The expected priority order of the errors should match the message order int16 NONE=0 int16 UNKNOWN=1 -int16 TF_ERROR=2 #system failure -int16 START_OUTSIDE_MAP=3 #request failure -int16 GOAL_OUTSIDE_MAP=4 #request failure -int16 START_OCCUPIED=5 #run-time failure -int16 GOAL_OCCUPIED=6 #run-time failure -int16 TIMEOUT=7 #run-time failure -int16 NO_VALID_PATH=8 #run-time failure +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 From 9e2c0562c2bf42a3436cca390f889ee86d797ca8 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 28 Sep 2022 14:12:21 -0400 Subject: [PATCH 35/38] update xml --- nav2_behavior_tree/nav2_tree_nodes.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 62a5f1f482..16e5dca3b1 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,6 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node + "The compute path to pose error code" From 50e891cbc783fcce93cdecb4a0f831e299008f96 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 28 Sep 2022 14:13:10 -0400 Subject: [PATCH 36/38] update xml --- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 16e5dca3b1..29ad929615 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" + "The compute path to pose error code" From 91a6253a2f14835d93c637d546ae966687aa99f3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 28 Sep 2022 11:22:06 -0700 Subject: [PATCH 37/38] Update nav2_behavior_tree/nav2_tree_nodes.xml --- nav2_behavior_tree/nav2_tree_nodes.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 29ad929615..636392929a 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" + "The compute path to pose error code" From 72a8f36848f320e6caadde7f78072194459b0eba Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 11 Oct 2022 20:10:12 -0400 Subject: [PATCH 38/38] fix --- nav2_navfn_planner/src/navfn_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 09d0e18466..53d7276036 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -147,7 +147,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (costmap_->getCost(mx_start, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { + 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");