improvement

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;
 

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -19,6 +19,7 @@
 #include <vector>
 #include <list>
 #include <memory>
+#include <unordered_set>
 
 #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<NodePtr> NodeVector;
+  typedef std::unordered_set<NodePtr> NodeSet;
   typedef typename NodeT::Coordinates Coordinates;
   typedef std::function<bool (const unsigned int &, NodeT * &)> 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);
 
   /**

nav2_smac_planner/src/a_star.cpp

@@ -44,7 +44,7 @@ AStarAlgorithm<NodeT>::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<Node2D>::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<typename NodeT>
@@ -210,14 +208,15 @@ void AStarAlgorithm<NodeT>::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<float>(mx),
@@ -226,10 +225,10 @@ void AStarAlgorithm<NodeT>::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<float>(mx),
@@ -243,8 +242,18 @@ void AStarAlgorithm<NodeT>::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<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(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<float>(mx),
@@ -267,9 +276,9 @@ void AStarAlgorithm<NodeT>::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<NodeT>::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<NodeT>::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<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
 }
 
 template<typename NodeT>
-typename AStarAlgorithm<NodeT>::NodeVector & AStarAlgorithm<NodeT>::getGoals()
+typename AStarAlgorithm<NodeT>::NodeSet & AStarAlgorithm<NodeT>::getGoals()
 {
-  return _goals;
+  return _goalsSet;
 }
 
 template<typename NodeT>
@@ -539,6 +550,13 @@ void AStarAlgorithm<NodeT>::clearStart()
   _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE);
 }
 
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::Coordinates AStarAlgorithm<NodeT>::getOriginalGoalCoordinate()
+{
+  return _goals_coordinates[0];
+}
+
 // Instantiate algorithm for the supported template types
 template class AStarAlgorithm<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;

nav2_smac_planner/src/analytic_expansion.cpp

@@ -48,7 +48,7 @@ void AnalyticExpansion<NodeT>::setCollisionChecker(
 
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     float current_best_score = std::numeric_limits<float>::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<int>(NodeT::getHeuristicCost(node_coords, goals_node[0]->pose)));
+      closest_distance,
+      static_cast<int>(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<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<NodeT>::NodePtr AnalyticExpansion<NodeT>::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<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             } else {
               state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(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,15 +163,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::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;
           }
         }
       }
     }
     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<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::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)
 {

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);
 }