improvement based on feedback

Signed-off-by: stevedan <[email protected]>

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
@@ -195,6 +195,13 @@ class AStarAlgorithm
    */
   unsigned int & getSizeDim3();
 
+  /**
+   * @brief Return the first goal coordinate defined by the user
+   * before applying the heading mode
+   * @return Coordinate to the first goal
+   */
+  Coordinates getInitialGoalCoordinate();
+
 protected:
   /**
    * @brief Get pointer to next goal in open set
@@ -258,7 +265,6 @@ class AStarAlgorithm
    */
   void clearStart();
 
-
   bool _traverse_unknown;
   int _max_iterations;
   int _max_on_approach_iterations;
@@ -272,9 +278,8 @@ class AStarAlgorithm
 
   CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodeVector _goals;
   NodeSet _goalsSet;
-  GoalHeadingMode _current_goal_heading_mode;
+  GoalHeadingMode _goal_heading_mode;
 
   Graph _graph;
   NodeQueue _queue;

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 & initial_goal_coords,
     const NodeGetter & getter, int & iterations, int & best_cost);
 
   /**

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -463,7 +463,7 @@ class NodeHybrid
    * \brief Sets the goal mode for the current search
    * \param goal_heading_mode The goal heading mode to use
    */
-  static void setGoalHeadingMode(const GoalHeadingMode & goal_heading_mode);
+  static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode);
 
   NodeHybrid * parent;
   Coordinates pose;
@@ -482,7 +482,7 @@ class NodeHybrid
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
   static float size_lookup;
-  static GoalHeadingMode current_goal_heading_mode;
+  static GoalHeadingMode goal_heading_mode;
 
 private:
   float _cell_cost;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -419,15 +419,15 @@ class NodeLattice
    * \brief Sets the goal mode for the current search
    * \param goal_heading_mode The goal heading mode to use
    */
-  static void setGoalHeadingMode(const GoalHeadingMode & goal_heading_mode);
+  static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode);
 
   NodeLattice * parent;
   Coordinates pose;
   static LatticeMotionTable motion_table;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
   static float size_lookup;
-  static GoalHeadingMode current_goal_heading_mode;
+  static GoalHeadingMode goal_heading_mode;
 
 private:
   float _cell_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);
@@ -71,8 +71,8 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  _current_goal_heading_mode = goal_heading_mode;
-  NodeT::setGoalHeadingMode(_current_goal_heading_mode);
+  _goal_heading_mode = goal_heading_mode;
+  NodeT::setGoalHeadingMode(_goal_heading_mode);
   NodeT::precomputeDistanceHeuristic(
     lookup_table_size, _motion_model, dim_3_size, _search_info);
   _dim3_size = dim_3_size;
@@ -96,7 +96,7 @@ void AStarAlgorithm<Node2D>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  _current_goal_heading_mode = goal_heading_mode;
+  _goal_heading_mode = goal_heading_mode;
 
   if (dim_3_size != 1) {
     throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization.");
@@ -195,10 +195,10 @@ 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));
 }
 
@@ -208,15 +208,14 @@ void AStarAlgorithm<NodeT>::setGoal(
   const unsigned int & my,
   const unsigned int & dim_3)
 {
-  _goals.clear();
   _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 (_current_goal_heading_mode) {
+  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),
@@ -225,10 +224,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),
@@ -242,8 +241,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),
@@ -266,9 +275,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]);
   }
 }
 
@@ -281,13 +290,15 @@ 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)) {
+    // if any goal is valid, then all goals are valid
+    auto goal = *_goalsSet.begin();
+    if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) {
       throw nav2_core::GoalOccupied("Goal was in lethal cost");
     }
   }
@@ -375,8 +386,10 @@ bool AStarAlgorithm<NodeT>::createPath(
     current_node->visited();
 
     // 2.1) Use an analytic expansion (if available) to generate a path
+    expansion_result = nullptr;
     expansion_result = _expander->tryAnalyticExpansion(
-      current_node, getGoals(), neighborGetter, analytic_iterations, closest_distance);
+      current_node, getGoals(),
+      getInitialGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
@@ -436,9 +449,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>
@@ -465,7 +478,7 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
     NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
   // Heurittic cost for more than one goal is already cotemplated in the
   // precomputation stage
-  float heuristic = NodeT::getHeuristicCost(node_coords, _goals_coordinates[0]);
+  float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate());
   if (heuristic < _best_heuristic_node.first) {
     _best_heuristic_node = {heuristic, node->getIndex()};
   }
@@ -538,6 +551,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>::getInitialGoalCoordinate()
+{
+  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 & initial_goal_coords,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {
@@ -63,31 +63,30 @@ 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;
-    for (unsigned int i = 0; i < goals_node.size(); i++) {
-      closest_distance = std::min(
-        closest_distance,
-        static_cast<int>(NodeT::getHeuristicCost(node_coords, goals_node[0]->pose)));
-      // 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)
-      int desired_iterations = std::max(
-        static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
-        static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
-
-      // 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) {
+    closest_distance = std::min(
+      closest_distance,
+      static_cast<int>(NodeT::getHeuristicCost(node_coords, initial_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)
+    int desired_iterations = std::max(
+      static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
+      static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
+    // 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 (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[i], 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
@@ -103,9 +102,10 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
               test_node->parent->parent->parent->parent->parent)
             {
               test_node = test_node->parent->parent->parent->parent->parent;
+              // print the goals pose
               refined_analytic_nodes =
                 getAnalyticPath(
-                test_node, goals_node[i], 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[i], 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 = i;
+            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 & initial_goal_coords,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {