From a7fc6bf0e992616c432ac9d057b3a4c57c567da4 Mon Sep 17 00:00:00 2001 From: stevedan Date: Sat, 23 Mar 2024 05:31:11 +0100 Subject: [PATCH] improvement --- .../include/nav2_smac_planner/a_star.hpp | 10 +++- .../nav2_smac_planner/analytic_expansion.hpp | 6 ++- nav2_smac_planner/src/a_star.cpp | 52 +++++++++++++------ nav2_smac_planner/src/analytic_expansion.cpp | 24 ++++----- nav2_smac_planner/test/test_a_star.cpp | 21 ++++++++ 5 files changed, 80 insertions(+), 33 deletions(-) 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 cd405330ca9..81cc1509eb9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -163,7 +163,7 @@ class AStarAlgorithm * @brief Get pointer reference to goal node * @return Node pointer reference to goal node */ - NodeVector & getGoals(); + NodeSet & getGoals(); /** * @brief Get maximum number of on-approach iterations after within threshold @@ -258,6 +258,13 @@ class AStarAlgorithm */ void clearStart(); + /** + * @brief Return the first goal coordinate defined by the user + * before applying the heading mode + * @return Coordinate to the first goal + */ + Coordinates getOriginalGoalCoordinate(); + bool _traverse_unknown; int _max_iterations; @@ -272,7 +279,6 @@ class AStarAlgorithm CoordinateVector _goals_coordinates; NodePtr _start; - NodeVector _goals; NodeSet _goalsSet; GoalHeadingMode _goal_heading_mode; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index cc8b7d66112..ad36d9791bc 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" @@ -34,7 +35,7 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; - typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; @@ -88,7 +89,8 @@ class AnalyticExpansion */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodeVector & goals_node, + const NodeSet & goals_node, + const Coordinates & first_goal, const NodeGetter & getter, int & iterations, int & best_cost); /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 6dd37a64ce7..08d40caee46 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -44,7 +44,7 @@ AStarAlgorithm::AStarAlgorithm( _search_info(search_info), _goals_coordinates(CoordinateVector()), _start(nullptr), - _goals(NodeVector()), + _goalsSet(NodeSet()), _motion_model(motion_model) { _graph.reserve(100000); @@ -195,13 +195,11 @@ void AStarAlgorithm::setGoal( if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - _goals.clear(); _goals_coordinates.clear(); _goalsSet.clear(); - _goals.push_back(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); + _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); - _goalsSet.insert(_goals[0]); } template @@ -210,14 +208,15 @@ void AStarAlgorithm::setGoal( const unsigned int & my, const unsigned int & dim_3) { - _goals.clear(); + // nodeset look for better logic _goalsSet.clear(); + NodeVector goals; CoordinateVector goals_coordinates; unsigned int num_bins = NodeT::motion_table.num_angle_quantization; unsigned int dim_3_half_bin = 0; switch (_goal_heading_mode) { case GoalHeadingMode::DEFAULT: - _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); goals_coordinates.push_back( typename NodeT::Coordinates( static_cast(mx), @@ -226,10 +225,10 @@ void AStarAlgorithm::setGoal( break; case GoalHeadingMode::BIDIRECTIONAL: // Add two goals, one for each direction - _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); // 180 degrees dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins; - _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); + _goalsSet.insert(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); goals_coordinates.push_back( typename NodeT::Coordinates( static_cast(mx), @@ -243,8 +242,18 @@ void AStarAlgorithm::setGoal( break; case GoalHeadingMode::ALL_DIRECTION: // Add all possible headings as goals + // set first goal to be the same as the input + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); for (unsigned int i = 0; i < num_bins; ++i) { - _goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); + if (i == dim_3) { + continue; + } + goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); goals_coordinates.push_back( typename NodeT::Coordinates( static_cast(mx), @@ -267,9 +276,9 @@ void AStarAlgorithm::setGoal( } _goals_coordinates = goals_coordinates; - for (unsigned int i = 0; i < _goals.size(); ++i) { - _goals[i]->setPose(_goals_coordinates[i]); - _goalsSet.insert(_goals[i]); + for (unsigned int i = 0; i < goals.size(); i++) { + goals[i]->setPose(_goals_coordinates[i]); + _goalsSet.insert(goals[i]); } } @@ -282,13 +291,14 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || _goals.empty()) { + if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { - if (!_goals[0]->isNodeValid(_traverse_unknown, _collision_checker)) { + auto first_goal = *_goalsSet.begin(); // Get the first goal from _goalsSet + if (!first_goal->isNodeValid(_traverse_unknown, _collision_checker)) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } } @@ -377,7 +387,8 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoals(), neighborGetter, analytic_iterations, closest_distance); + current_node, getGoals(), + getOriginalGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -437,9 +448,9 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() } template -typename AStarAlgorithm::NodeVector & AStarAlgorithm::getGoals() +typename AStarAlgorithm::NodeSet & AStarAlgorithm::getGoals() { - return _goals; + return _goalsSet; } template @@ -539,6 +550,13 @@ void AStarAlgorithm::clearStart() _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); } + +template +typename AStarAlgorithm::Coordinates AStarAlgorithm::getOriginalGoalCoordinate() +{ + return _goals_coordinates[0]; +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 976412d97e1..1772c7e60fd 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeVector & goals_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -63,11 +63,11 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic float current_best_score = std::numeric_limits::max(); AnalyticExpansionNodes current_best_analytic_nodes = {}; - unsigned int current_best_goal_index = 0; + NodePtr current_best_goal = nullptr; NodePtr current_best_node = nullptr; closest_distance = std::min( - closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goals_node[0]->pose))); + closest_distance, + static_cast(NodeT::getHeuristicCost(node_coords, goal_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) @@ -77,16 +77,16 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); - + // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - for (unsigned int g = 0; g < goals_node.size(); g++) { + for (auto goal_node : goals_node) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; AnalyticExpansionNodes analytic_nodes = getAnalyticPath( - current_node, goals_node[g], getter, + current_node, goal_node, getter, current_node->motion_table.state_space); if (!analytic_nodes.empty()) { // If we have a valid path, attempt to refine it @@ -105,7 +105,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // print the goals pose refined_analytic_nodes = getAnalyticPath( - test_node, goals_node[g], getter, + test_node, goal_node, getter, test_node->motion_table.state_space); if (refined_analytic_nodes.empty()) { break; @@ -153,7 +153,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } else { state_space = std::make_shared(min_turn_rad); } - refined_analytic_nodes = getAnalyticPath(node, goals_node[g], getter, state_space); + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); score = scoringFn(refined_analytic_nodes); if (score <= best_score) { analytic_nodes = refined_analytic_nodes; @@ -163,7 +163,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (best_score < current_best_score) { current_best_score = best_score; current_best_node = node; - current_best_goal_index = g; + current_best_goal = goal_node; current_best_analytic_nodes = analytic_nodes; } } @@ -171,7 +171,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } if (!current_best_analytic_nodes.empty()) { return setAnalyticPath( - current_best_node, goals_node[current_best_goal_index], + current_best_node, current_best_goal, current_best_analytic_nodes); } analytic_iterations--; @@ -375,7 +375,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeVector & goals_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index a70b6b34a1c..c99d114e99b 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -361,6 +361,15 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); + gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); + gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); + gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); + EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -369,4 +378,16 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); + + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); }