Revert recent smac changes causing regressions (ros-navigation#5221)

* Revert "Prototype solving ros-navigation#5192 Issue 2: Reeds-Shepp reduce small reverse expansions (ros-navigation#5207)"

This reverts commit c32873d.

* Revert "include bug fix for nav2_smac_planner (ros-navigation#5198)"

This reverts commit 6a74ba6.

* Revert "Feat/smac planner include orientation flexibility (ros-navigation#4127)"

This reverts commit f5543c3.

Author: SteveMacenski

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -34,7 +34,6 @@
 #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"
 
@@ -57,9 +56,6 @@ 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
@@ -96,8 +92,6 @@ 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,
@@ -133,15 +127,11 @@ 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 GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT,
-    const int & coarse_search_resolution = 1);
+    const unsigned int & dim_3);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -166,6 +156,12 @@ 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
@@ -196,18 +192,6 @@ 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
@@ -228,6 +212,13 @@ 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
@@ -251,11 +242,6 @@ 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);
 
   /**
@@ -276,11 +262,12 @@ 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;
-  GoalManagerT _goal_manager;
+  NodePtr _goal;
+
   Graph _graph;
   NodeQueue _queue;
 

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -15,14 +15,11 @@
 #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 <string>
-#include <vector>
+#include <functional>
 #include <list>
 #include <memory>
+#include <string>
+#include <vector>
 
 #include "nav2_smac_planner/node_2d.hpp"
 #include "nav2_smac_planner/node_hybrid.hpp"
@@ -38,10 +35,8 @@ 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
@@ -64,33 +59,7 @@ class AnalyticExpansion
     Coordinates proposed_coords;
   };
 
-  /**
-   * @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};
-  };
+  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
 
   /**
    * @brief Constructor for analytic expansion object
@@ -110,22 +79,17 @@ class AnalyticExpansion
   /**
    * @brief Attempt an analytic path completion
    * @param node The node to start the analytic path from
-   * @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 goal The goal node to plan to
    * @param getter Gets a node at a set of coordinates
    * @param iterations Iterations to run over
-   * @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
+   * @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
    */
   NodePtr tryAnalyticExpansion(
     const NodePtr & current_node,
-    const NodeVector & coarse_check_goals,
-    const NodeVector & fine_check_goals,
-    const CoordinateVector & goals_coords,
-    const NodeGetter & getter, int & iterations,
-    int & closest_distance);
+    const NodePtr & goal_node,
+    const NodeGetter & getter, int & iterations, int & best_cost);
 
   /**
    * @brief Perform an analytic path expansion to the goal
@@ -139,21 +103,6 @@ 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 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
@@ -165,13 +114,6 @@ class AnalyticExpansion
     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,14 +28,6 @@ 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) {
@@ -67,33 +59,6 @@ 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;