cleanup code after optimization

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

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -18,7 +18,6 @@
 
 #include <vector>
 #include <iostream>
-#include <unordered_map>
 #include <unordered_set>
 #include <functional>
 #include <memory>
@@ -113,16 +112,12 @@ class AStarAlgorithm
    * @param tolerance Reference to tolerance in costmap nodes
    * @param cancel_checker Function to check if the task has been canceled
    * @param expansions_log Optional expansions logged for debug
-   * @param goal_heading_mode The goal heading mode to use
-   * @param coarse_search_resolution The resolution to search for goal heading
    * @return if plan was successful
    */
   bool createPath(
     CoordinateVector & path, int & num_iterations, const float & tolerance,
     std::function<bool()> cancel_checker,
-    std::vector<std::tuple<float, float, float>> * expansions_log = nullptr,
-    const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT,
-    const int & coarse_search_resolution = 0);
+    std::vector<std::tuple<float, float, float>> * expansions_log = nullptr);
 
   /**
    * @brief Sets the collision checker to use
@@ -136,12 +131,14 @@ class AStarAlgorithm
    * @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 GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT,
+    const int & coarse_search_resolution = 0);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -214,6 +211,15 @@ class AStarAlgorithm
    */
   unsigned int & getSizeDim3();
 
+  /**
+   * @brief Prepare goals for expansion
+   * @param goals_to_expand Vector of goals to expand
+   * @param coarse_search_goal_size Size of the coarse search goal
+   */
+  void prepareGoalsForExpansion(
+    NodeVector & goals_to_expand,
+    unsigned int & coarse_search_goal_size);
+
 protected:
   /**
    * @brief Get pointer to next goal in open set
@@ -264,6 +270,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);
 
   /**
@@ -284,12 +295,13 @@ class AStarAlgorithm
   unsigned int _x_size;
   unsigned int _y_size;
   unsigned int _dim3_size;
+  unsigned int _coarse_search_resolution;
   SearchInfo _search_info;
 
   CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodeSet _goalsSet;
-  NodeVector _goalsVector;
+  NodeSet _goals_set;
+  NodeVector _goals_vector;
 
   Graph _graph;
   NodeQueue _queue;

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -18,7 +18,6 @@
 #include <functional>
 #include <list>
 #include <memory>
-#include <unordered_set>
 #include <string>
 #include <vector>
 #include <queue>
@@ -38,7 +37,6 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
-  typedef std::unordered_set<NodePtr> NodeSet;
   typedef std::vector<NodePtr> NodeVector;
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
@@ -86,17 +84,22 @@ class AnalyticExpansion
    * @brief Attempt an analytic path completion
    * @param node The node to start the analytic path from
    * @param goals_node set of goals node to plan to
+   * @param goals_to_expand set of goals node to plan to expand 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 closest_distance Closest distance to goal
+   * @param coarse_search_goal_size Coarse search goal size
    * @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 NodeVector & goals_to_expand,
     const CoordinateVector & goals_coords,
     const NodeGetter & getter, int & iterations,
-    int & closest_distance);
+    int & closest_distance,
+    const unsigned int & coarse_search_goal_size);
 
   /**
    * @brief Perform an analytic path expansion to the goal
@@ -136,13 +139,6 @@ class AnalyticExpansion
   NodePtr setAnalyticPath(
     const NodePtr & node, const NodePtr & goal,
     const AnalyticExpansionNodes & expanded_nodes);
-
-  /**
-   * @brief Sets the goals to expand to in the analytic expansion
-   * @param goals_node set of goals node to plan to
-   * @param coarse_search_resolution The resolution to expand to
-   */
-  void setGoalsToExpand(const NodeVector & goals_node, const int & coarse_search_resolution);
 
   /**
    * @brief Takes an expanded nodes to clean up, if necessary, of any state
@@ -158,8 +154,6 @@ class AnalyticExpansion
   unsigned int _dim_3_size;
   GridCollisionChecker * _collision_checker;
   std::list<std::unique_ptr<NodeT>> _detached_nodes;
-  std::vector<NodePtr> _goals_to_expand;
-  unsigned int _coarse_search_goal_size;
 };
 
 }  // namespace nav2_smac_planner