Feat/smac planner include orientation flexibility (#4127)

* include functionality to allow multiple goal heading for smac planner

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

* include missing parameter inclusion

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

* increase test coverage

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

---------

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

Author: stevedanomodolor

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -32,6 +32,7 @@
 #include "nav2_smac_planner/node_hybrid.hpp"
 #include "nav2_smac_planner/node_lattice.hpp"
 #include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/goal_manager.hpp"
 #include "nav2_smac_planner/types.hpp"
 #include "nav2_smac_planner/constants.hpp"
 
@@ -54,6 +55,9 @@ class AStarAlgorithm
   typedef typename NodeT::CoordinateVector CoordinateVector;
   typedef typename NodeVector::iterator NeighborIterator;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
+  typedef GoalManager<NodeT> GoalManagerT;
+  typedef std::vector<GoalState<NodeT>> GoalStateVector;
+
 
   /**
    * @struct nav2_smac_planner::NodeComparator
@@ -90,6 +94,8 @@ class AStarAlgorithm
    * or planning time exceeded
    * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns
    * false after this timeout
+   * @param lookup_table_size Size of the lookup table to store heuristic values
+   * @param dim_3_size Number of quantization bins
    */
   void initialize(
     const bool & allow_unknown,
@@ -125,11 +131,15 @@ class AStarAlgorithm
    * @param mx The node X index of the goal
    * @param my The node Y index of the goal
    * @param dim_3 The node dim_3 index of the goal
+   * @param goal_heading_mode The goal heading mode to use
+   * @param coarse_search_resolution The resolution to search for goal heading
    */
   void setGoal(
     const float & mx,
     const float & my,
-    const unsigned int & dim_3);
+    const unsigned int & dim_3,
+    const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT,
+    const int & coarse_search_resolution = 1);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -154,12 +164,6 @@ class AStarAlgorithm
    */
   NodePtr & getStart();
 
-  /**
-   * @brief Get pointer reference to goal node
-   * @return Node pointer reference to goal node
-   */
-  NodePtr & getGoal();
-
   /**
    * @brief Get maximum number of on-approach iterations after within threshold
    * @return Reference to Maximum on-approach iterations parameter
@@ -190,6 +194,18 @@ class AStarAlgorithm
    */
   unsigned int & getSizeDim3();
 
+  /**
+   * @brief Get the resolution of the coarse search
+   * @return Size of the goals to expand
+   */
+  unsigned int getCoarseSearchResolution();
+
+  /**
+   * @brief Get the goals manager class
+   * @return Goal manager class
+   */
+  GoalManagerT getGoalManager();
+
 protected:
   /**
    * @brief Get pointer to next goal in open set
@@ -210,13 +226,6 @@ class AStarAlgorithm
    */
   inline NodePtr addToGraph(const uint64_t & index);
 
-  /**
-   * @brief Check if this node is the goal node
-   * @param node Node pointer to check if its the goal node
-   * @return if node is goal
-   */
-  inline bool isGoal(NodePtr & node);
-
   /**
    * @brief Get cost of heuristic of node
    * @param node Node pointer to get heuristic for
@@ -240,6 +249,11 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
+  /**
+   * @brief Check if node has been visited
+   * @param current_node Node to check if visited
+   * @return if node has been visited
+   */
   inline bool onVisitationCheckNode(const NodePtr & node);
 
   /**
@@ -260,12 +274,11 @@ class AStarAlgorithm
   unsigned int _x_size;
   unsigned int _y_size;
   unsigned int _dim3_size;
+  unsigned int _coarse_search_resolution;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
   NodePtr _start;
-  NodePtr _goal;
-
+  GoalManagerT _goal_manager;
   Graph _graph;
   NodeQueue _queue;
 

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -35,8 +35,10 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
+  typedef std::vector<NodePtr> NodeVector;
   typedef typename NodeT::Coordinates Coordinates;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
+  typedef typename NodeT::CoordinateVector CoordinateVector;
 
   /**
    * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
@@ -79,17 +81,22 @@ class AnalyticExpansion
   /**
    * @brief Attempt an analytic path completion
    * @param node The node to start the analytic path from
-   * @param goal The goal node to plan to
+   * @param coarse_check_goals Coarse list of goals nodes to plan to
+   * @param fine_check_goals Fine list of goals nodes to plan to
+   * @param goals_coords vector of goal coordinates to plan to
    * @param getter Gets a node at a set of coordinates
    * @param iterations Iterations to run over
-   * @param best_cost Best heuristic cost to propertionally expand more closer to the goal
-   * @return Node pointer reference to goal node if successful, else
-   * return nullptr
+   * @param closest_distance Closest distance to goal
+   * @return Node pointer reference to goal node with the best score out of the goals node if
+   * successful, else return nullptr
    */
   NodePtr tryAnalyticExpansion(
     const NodePtr & current_node,
-    const NodePtr & goal_node,
-    const NodeGetter & getter, int & iterations, int & best_cost);
+    const NodeVector & coarse_check_goals,
+    const NodeVector & fine_check_goals,
+    const CoordinateVector & goals_coords,
+    const NodeGetter & getter, int & iterations,
+    int & closest_distance);
 
   /**
    * @brief Perform an analytic path expansion to the goal
@@ -103,6 +110,20 @@ class AnalyticExpansion
     const NodePtr & node, const NodePtr & goal,
     const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space);
 
+  /**
+   * @brief Refined analytic path from the current node to the goal
+   * @param current_node The node to start the analytic path from
+   * @param goal_node The goal node to plan to
+   * @param getter The function object that gets valid nodes from the graph
+   * @param analytic_nodes The set of analytic nodes to refine
+   * @return The score of the refined path
+   */
+  float refineAnalyticPath(
+    const NodePtr & current_node,
+    const NodePtr & goal_node,
+    const NodeGetter & getter,
+    AnalyticExpansionNodes & analytic_nodes);
+
   /**
    * @brief Takes final analytic expansion and appends to current expanded node
    * @param node The node to start the analytic path from

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -28,6 +28,14 @@ enum class MotionModel
   STATE_LATTICE = 4,
 };
 
+enum class GoalHeadingMode
+{
+  UNKNOWN = 0,
+  DEFAULT = 1,
+  BIDIRECTIONAL = 2,
+  ALL_DIRECTION = 3,
+};
+
 inline std::string toString(const MotionModel & n)
 {
   switch (n) {
@@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n)
   }
 }
 
+inline std::string toString(const GoalHeadingMode & n)
+{
+  switch (n) {
+    case GoalHeadingMode::DEFAULT:
+      return "DEFAULT";
+    case GoalHeadingMode::BIDIRECTIONAL:
+      return "BIDIRECTIONAL";
+    case GoalHeadingMode::ALL_DIRECTION:
+      return "ALL_DIRECTION";
+    default:
+      return "Unknown";
+  }
+}
+
+inline GoalHeadingMode fromStringToGH(const std::string & n)
+{
+  if (n == "DEFAULT") {
+    return GoalHeadingMode::DEFAULT;
+  } else if (n == "BIDIRECTIONAL") {
+    return GoalHeadingMode::BIDIRECTIONAL;
+  } else if (n == "ALL_DIRECTION") {
+    return GoalHeadingMode::ALL_DIRECTION;
+  } else {
+    return GoalHeadingMode::UNKNOWN;
+  }
+}
+
 const float UNKNOWN_COST = 255.0;
 const float OCCUPIED_COST = 254.0;
 const float INSCRIBED_COST = 253.0;