Feat/smac planner include orientation flexibility

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -16,12 +16,15 @@
 #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
 #define NAV2_SMAC_PLANNER__A_STAR_HPP_
 
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <unordered_set>
 #include <functional>
 #include <memory>
 #include <queue>
 #include <tuple>
 #include <utility>
-#include <vector>
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_core/planner_exceptions.hpp"
@@ -49,6 +52,7 @@ class AStarAlgorithm
   typedef NodeT * NodePtr;
   typedef robin_hood::unordered_node_map<uint64_t, NodeT> Graph;
   typedef std::vector<NodePtr> NodeVector;
+  typedef std::unordered_set<NodePtr> NodeSet;
   typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
@@ -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,
@@ -129,7 +135,8 @@ class AStarAlgorithm
   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);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -155,10 +162,16 @@ class AStarAlgorithm
   NodePtr & getStart();
 
   /**
-   * @brief Get pointer reference to goal node
-   * @return Node pointer reference to goal node
+   * @brief Get pointer reference to goals node
+   * @return unordered_set of node pointers reference to the goals nodes
+   */
+  NodeSet & getGoals();
+
+  /**
+   * @brief Get pointer reference to goals coordinates
+   * @return vector of goals coordinates reference to the goals coordinates
    */
-  NodePtr & getGoal();
+  CoordinateVector & getGoalsCoordinates();
 
   /**
    * @brief Get maximum number of on-approach iterations after within threshold
@@ -260,9 +273,9 @@ class AStarAlgorithm
   unsigned int _dim3_size;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
+  CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodePtr _goal;
+  NodeSet _goalsSet;
 
   Graph _graph;
   NodeQueue _queue;

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -18,6 +18,7 @@
 #include <functional>
 #include <list>
 #include <memory>
+#include <unordered_set>
 #include <string>
 #include <vector>
 
@@ -35,7 +36,9 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
+  typedef std::unordered_set<NodePtr> NodeSet;
   typedef typename NodeT::Coordinates Coordinates;
+  typedef typename NodeT::CoordinateVector CoordinateVector;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
 
   /**
@@ -79,16 +82,18 @@ 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 goals_node set of goals node 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
+   * @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 NodeSet & goals_node,
+    const CoordinateVector & goals_coords,
     const NodeGetter & getter, int & iterations, int & best_cost);
 
   /**

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;

nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -50,6 +50,16 @@ class Node2D
     : x(x_in), y(y_in)
     {}
 
+    inline bool operator==(const Coordinates & rhs) const
+    {
+      return this->x == rhs.x && this->y == rhs.y;
+    }
+
+    inline bool operator!=(const Coordinates & rhs) const
+    {
+      return !(*this == rhs);
+    }
+
     float x, y;
   };
   typedef std::vector<Coordinates> CoordinateVector;
@@ -75,6 +85,15 @@ class Node2D
     return this->_index == rhs._index;
   }
 
+  /**
+   * @brief setting continuous coordinate search poses (in partial-cells)
+   * @param Pose pose
+   */
+  inline void setPose(const Coordinates & pose_in)
+  {
+    pose = pose_in;
+  }
+
   /**
    * @brief Reset method for new search
    */
@@ -224,7 +243,7 @@ class Node2D
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize the neighborhood to be used in A*
@@ -264,6 +283,7 @@ class Node2D
   bool backtracePath(CoordinateVector & path);
 
   Node2D * parent;
+  Coordinates pose;
   static float cost_travel_multiplier;
   static std::vector<int> _neighbors_grid_offsets;
 

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -167,12 +167,12 @@ class NodeHybrid
     : x(x_in), y(y_in), theta(theta_in)
     {}
 
-    inline bool operator==(const Coordinates & rhs)
+    inline bool operator==(const Coordinates & rhs) const
     {
       return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
     }
 
-    inline bool operator!=(const Coordinates & rhs)
+    inline bool operator!=(const Coordinates & rhs) const
     {
       return !(*this == rhs);
     }
@@ -371,7 +371,7 @@ class NodeHybrid
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize motion models

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -314,7 +314,7 @@ class NodeLattice
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize motion models
@@ -408,8 +408,8 @@ class NodeLattice
   bool backtracePath(CoordinateVector & path);
 
   /**
-   * \brief add node to the path
-   * \param current_node
+   * @brief add node to the path
+   * @param current_node
    */
   void addNodeToPath(NodePtr current_node, CoordinateVector & path);
 

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -123,6 +123,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
+  GoalHeadingMode _goal_heading_mode;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     _planned_footprints_publisher;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -120,6 +120,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
     _planned_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
+  GoalHeadingMode _goal_heading_mode;
   std::mutex _mutex;
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;