Fix/smac planner orientation goals

.devcontainer/devcontainer.json

@@ -14,6 +14,8 @@
         // "--pid=host", // DDS discovery with host, without --network=host
         // "--privileged", // device access to host peripherals, e.g. USB
         // "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb
+        // "--volume=/tmp/.X11-unix:/tmp/.X11-unix", // X11 socket for GUI applications
+        // "--gpus=all" // access to all GPUs, e.g. for GPU-accelerated applications
     ],
     "workspaceFolder": "/opt/overlay_ws/src/navigation2",
     "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind",
@@ -23,6 +25,8 @@
     "remoteEnv": {
         "OVERLAY_MIXINS": "release ccache lld",
         "CCACHE_DIR": "/tmp/.ccache"
+        // "QT_X11_NO_MITSHM": "1", // disable MIT-SHM for X11 forwarding
+        // "DISPLAY": "${localEnv:DISPLAY}", // X11 forwarding
     },
     "mounts": [
         {

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,8 @@ 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;
+
 
   /**
    * @struct nav2_smac_planner::NodeComparator
@@ -90,6 +93,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 +130,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 +163,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 +193,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 +225,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 +248,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 +273,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

@@ -15,6 +15,10 @@
 #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/base/spaces/ReedsSheppStateSpace.h>
+
 #include <functional>
 #include <list>
 #include <memory>
@@ -35,8 +39,10 @@
 {
 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
@@ -59,7 +65,33 @@
     Coordinates proposed_coords;
   };
 
-  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
+  /**
+   * @struct AnalyticExpansionNodes
+   * @brief Analytic expansion nodes and associated metadata
+   *
+   * This structure holds a collection of analytic expansion nodes and the number of direction
+   * changes encountered during the expansion.
+   */
+  struct AnalyticExpansionNodes
+  {
+    AnalyticExpansionNodes() = default;
+
+    void add(
+      NodePtr & node,
+      Coordinates & initial_coords,
+      Coordinates & proposed_coords)
+    {
+      nodes.emplace_back(node, initial_coords, proposed_coords);
+    }
+
+    void setDirectionChanges(int changes)
+    {
+      direction_changes = changes;
+    }
+
+    std::vector<AnalyticExpansionNode> nodes;
+    int direction_changes{0};
+  };
 
   /**
    * @brief Constructor for analytic expansion object
@@ -79,17 +111,22 @@
   /**
    * @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 +140,21 @@
     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 node The node to start the analytic path from. Node head may
+   * change as a result of refinement
+   * @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(
+    NodePtr & 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
@@ -114,6 +166,13 @@
     const NodePtr & node, const NodePtr & goal,
     const AnalyticExpansionNodes & expanded_nodes);
 
+  /**
+    * @brief Counts the number of direction changes in a Reeds-Shepp path
+    * @param path The Reeds-Shepp path to count direction changes in
+    * @return The number of direction changes in the path
+    */
+  int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path);
+
   /**
    * @brief Takes an expanded nodes to clean up, if necessary, of any state
    * information that may be polluting it from a prior search iteration

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;