improvement

nav2_smac_planner/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_smac_planner)
 
-set(CMAKE_BUILD_TYPE Debug) #Debug, Release
+set(CMAKE_BUILD_TYPE Release) #Debug, Release
 
 
 find_package(ament_cmake REQUIRED)
@@ -156,8 +156,7 @@ if(BUILD_TESTING)
   set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  # TODO: uncomment when tests are modified
-  #add_subdirectory(test)
+  add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include ${OMPL_INCLUDE_DIRS})

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -127,7 +127,7 @@ class AStarAlgorithm
     const unsigned int & mx,
     const unsigned int & my,
     const unsigned int & dim_3,
-    const GoalHeading & goal_heading = GoalHeading::DEFAULT);
+    const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -156,7 +156,7 @@ class AStarAlgorithm
   //  * @brief Get pointer reference to goal node
   //  * @return Node pointer reference to goal node
   //  */
-  // NodePtr & getGoal();
+  NodeVector & getGoals();
 
   /**
    * @brief Get maximum number of on-approach iterations after within threshold

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -88,7 +88,7 @@ class AnalyticExpansion
    */
   NodePtr tryAnalyticExpansion(
     const NodePtr & current_node,
-    const NodeVector & goal_nodes,
+    const NodeVector & goals_node,
     const NodeGetter & getter, int & iterations, int & best_cost);
 
   /**

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -28,7 +28,7 @@ enum class MotionModel
   STATE_LATTICE = 4,
 };
 
-enum class GoalHeading
+enum class GoalHeadingMode
 {
   UNKNOWN = 0,
   DEFAULT = 1,
@@ -67,30 +67,30 @@ inline MotionModel fromString(const std::string & n)
   }
 }
 
-inline std::string toString(const GoalHeading & n)
+inline std::string toString(const GoalHeadingMode & n)
 {
   switch (n) {
-    case GoalHeading::DEFAULT:
+    case GoalHeadingMode::DEFAULT:
       return "DEFAULT";
-    case GoalHeading::BIDIRECTIONAL:
+    case GoalHeadingMode::BIDIRECTIONAL:
       return "BIDIRECTIONAL";
-    case GoalHeading::ALL_DIRECTION:
+    case GoalHeadingMode::ALL_DIRECTION:
       return "ALL_DIRECTION";
     default:
       return "Unknown";
   }
 }
 
-inline GoalHeading fromStringToGH(const std::string & n)
+inline GoalHeadingMode fromStringToGH(const std::string & n)
 {
   if (n == "DEFAULT") {
-    return GoalHeading::DEFAULT;
+    return GoalHeadingMode::DEFAULT;
   } else if (n == "BIDIRECTIONAL") {
-    return GoalHeading::BIDIRECTIONAL;
+    return GoalHeadingMode::BIDIRECTIONAL;
   } else if (n == "ALL_DIRECTION") {
-    return GoalHeading::ALL_DIRECTION;
+    return GoalHeadingMode::ALL_DIRECTION;
   } else {
-    return GoalHeading::UNKNOWN;
+    return GoalHeadingMode::UNKNOWN;
   }
 }
 

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -120,7 +120,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
-  GoalHeading _goal_heading;
+  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

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

nav2_smac_planner/src/a_star.cpp

@@ -180,7 +180,7 @@ void AStarAlgorithm<Node2D>::setGoal(
   const unsigned int & mx,
   const unsigned int & my,
   const unsigned int & dim_3,
-  const GoalHeading & goal_heading)
+  const GoalHeadingMode & goal_heading_mode)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
@@ -197,26 +197,28 @@ void AStarAlgorithm<NodeT>::setGoal(
   const unsigned int & mx,
   const unsigned int & my,
   const unsigned int & dim_3,
-  const GoalHeading & goal_heading)
+  const GoalHeadingMode & goal_heading_mode)
 {
   _goals.clear();
-  _goals_coordinates.clear();
   NodeVector goals;
   CoordinateVector goals_coordinates;
   unsigned int num_bins = NodeT::motion_table.num_angle_quantization;
-  switch (goal_heading) {
-    case GoalHeading::DEFAULT:
-      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+  unsigned int dim_3_half_bin = 0;
+  switch (goal_heading_mode) {
+    case GoalHeadingMode::DEFAULT:
+      _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
       goals_coordinates.push_back(
         typename NodeT::Coordinates(
           static_cast<float>(mx),
           static_cast<float>(my),
           static_cast<float>(dim_3)));
       break;
-    case GoalHeading::BIDIRECTIONAL:
+    case GoalHeadingMode::BIDIRECTIONAL:
       // Add two goals, one for each direction
-      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
-      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3 + num_bins / 2)));
+      _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+      // 180 degrees
+      dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins;
+      _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin)));
       goals_coordinates.push_back(
         typename NodeT::Coordinates(
           static_cast<float>(mx),
@@ -226,20 +228,20 @@ void AStarAlgorithm<NodeT>::setGoal(
         typename NodeT::Coordinates(
           static_cast<float>(mx),
           static_cast<float>(my),
-          static_cast<float>(dim_3 + num_bins / 2)));
+          static_cast<float>(dim_3_half_bin)));
       break;
-    case GoalHeading::ALL_DIRECTION:
+    case GoalHeadingMode::ALL_DIRECTION:
       // Add all possible headings as goals
       for (unsigned int i = 0; i < num_bins; ++i) {
-        goals.push_back(addToGraph(NodeT::getIndex(mx, my, i)));
+        _goals.push_back(addToGraph(NodeT::getIndex(mx, my, i)));
         goals_coordinates.push_back(
           typename NodeT::Coordinates(
             static_cast<float>(mx),
             static_cast<float>(my),
             static_cast<float>(i)));
       }
       break;
-    case GoalHeading::UNKNOWN:
+    case GoalHeadingMode::UNKNOWN:
       throw std::runtime_error("Goal heading is UNKNOWN.");
       break;
   }
@@ -254,7 +256,6 @@ void AStarAlgorithm<NodeT>::setGoal(
   }
 
   _goals_coordinates = goals_coordinates;
-  _goals = goals;
   for (unsigned int i = 0; i < goals.size(); ++i) {
     _goals[i]->setPose(_goals_coordinates[i]);
   }
@@ -270,20 +271,14 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
 
   // Check if points were filled in
   if (!_start || !_goals.empty()) {
-    for (auto & goal : _goals) {
-      if (!goal) {
         throw std::runtime_error("Failed to compute path, no valid start or goal given.");
-      }
-    }
   }
 
   // Check if ending point is valid
   if (getToleranceHeuristic() < 0.001) {
-    for (auto & goal : _goals) {
-      if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) {
+      if (!_goals[0]->isNodeValid(_traverse_unknown, _collision_checker)) {
         throw nav2_core::GoalOccupied("Goal was in lethal cost");
       }
-    }
   }
 
   // Note: We do not check the if the start is valid because it is cleared
@@ -366,7 +361,7 @@ bool AStarAlgorithm<NodeT>::createPath(
 
     // 2.1) Use an analytic expansion (if available) to generate a path
     expansion_result = _expander->tryAnalyticExpansion(
-      current_node, _goals, neighborGetter, analytic_iterations, closest_distance);
+      current_node, getGoals(), neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
@@ -425,11 +420,11 @@ typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
   return _start;
 }
 
-// template<typename NodeT>
-// typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
-// {
-//   return _goals[0];
-// }
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodeVector & AStarAlgorithm<NodeT>::getGoals()
+{
+  return _goals;
+}
 
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode()

nav2_smac_planner/src/analytic_expansion.cpp

@@ -48,7 +48,7 @@ void AnalyticExpansion<NodeT>::setCollisionChecker(
 
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
-  const NodePtr & current_node, const NodeVector & goals_nodes,
+  const NodePtr & current_node, const NodeVector & goals_node,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {
@@ -61,40 +61,32 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
       NodeT::getCoords(
       current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
 
-    std::vector<std::pair<NodePtr, int>> goal_distance_pairs;
-    goal_distance_pairs.reserve(goals_nodes.size());
-    for (size_t i = 0; i < goals_nodes.size(); i++) {
-      closest_distance =
-        static_cast<int>(NodeT::getHeuristicCost(node_coords, goals_nodes[i]->pose));
-      goal_distance_pairs.push_back(std::make_pair(goals_nodes[i], closest_distance));
-    }
-    std::sort(
-      goal_distance_pairs.begin(), goal_distance_pairs.end(),
-      [](const std::pair<NodePtr, int> & a, const std::pair<NodePtr, int> & b) {
-        return a.second < b.second;
-      });
-    // we try the closest goal first and if we fail, we try the next closest
-
-    for (const auto & goal_distance_pair : goal_distance_pairs) {
-      closest_distance = goal_distance_pair.second;
-      NodePtr goal_node = goal_distance_pair.first;
+    float current_best_score = std::numeric_limits<float>::max();
+    AnalyticExpansionNodes current_best_analytic_nodes = {};
+    NodePtr current_best_goal_node = nullptr;
+    NodePtr current_best_node = nullptr;
+    for (unsigned int i = 0; i < goals_node.size(); i++) {
+      closest_distance = std::min(
+      closest_distance,
+      static_cast<int>(NodeT::getHeuristicCost(node_coords, goals_node[i]->pose)));
       // We want to expand at a rate of d/expansion_ratio,
       // but check to see if we are so close that we would be expanding every iteration
       // If so, limit it to the expansion ratio (rounded up)
       int desired_iterations = std::max(
         static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
         static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
+
       // If we are closer now, we should update the target number of iterations to go
       analytic_iterations =
         std::min(analytic_iterations, desired_iterations);
 
       // Always run the expansion on the first run in case there is a
       // trivial path to be found
       if (analytic_iterations <= 0) {
-        // Reset the counter and try the analytic path expansion
-        analytic_iterations = desired_iterations;
-        AnalyticExpansionNodes analytic_nodes =
-          getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space);
+      // Reset the counter and try the analytic path expansion
+      analytic_iterations = desired_iterations;
+      AnalyticExpansionNodes analytic_nodes =
+        getAnalyticPath(current_node, goals_node[i], getter, current_node->motion_table.state_space);
         if (!analytic_nodes.empty()) {
           // If we have a valid path, attempt to refine it
           NodePtr node = current_node;
@@ -103,14 +95,13 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           for (int i = 0; i < 8; i++) {
             // Attempt to create better paths in 5 node increments, need to make sure
             // they exist for each in order to do so (maximum of 40 points back).
-            if (test_node->parent && test_node->parent->parent &&
-              test_node->parent->parent->parent &&
+            if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent &&
               test_node->parent->parent->parent->parent &&
               test_node->parent->parent->parent->parent->parent)
             {
               test_node = test_node->parent->parent->parent->parent->parent;
               refined_analytic_nodes =
-                getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space);
+                getAnalyticPath(test_node, goals_node[i], getter, test_node->motion_table.state_space);
               if (refined_analytic_nodes.empty()) {
                 break;
               }
@@ -157,21 +148,31 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             } else {
               state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(min_turn_rad);
             }
-            refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space);
+            refined_analytic_nodes = getAnalyticPath(node, goals_node[i], getter, state_space);
             score = scoringFn(refined_analytic_nodes);
             if (score <= best_score) {
               analytic_nodes = refined_analytic_nodes;
               best_score = score;
             }
           }
+          if(best_score < current_best_score)
+          {
+            current_best_score = best_score;
+            current_best_node = node;
+            current_best_goal_node = goals_node[i];
+            current_best_analytic_nodes = analytic_nodes;
+          }
 
-          return setAnalyticPath(node, goal_node, analytic_nodes);
         }
       }
-
-      analytic_iterations--;
     }
+    if(!current_best_analytic_nodes.empty())
+    {
+      return setAnalyticPath(current_best_node, current_best_goal_node, current_best_analytic_nodes);
+    }
+    analytic_iterations--;   
   }
+
   // No valid motion model - return nullptr
   return NodePtr(nullptr);
 }
@@ -368,7 +369,7 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
-  const NodePtr & current_node, const NodeVector & goals_nodes,
+  const NodePtr & current_node, const NodeVector & goals_node,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {