Include goal heading mode to allow bidirectional and all direction goals

Signed-off-by: stevedan <[email protected]>

nav2_smac_planner/CMakeLists.txt

@@ -1,7 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_smac_planner)
 
-set(CMAKE_BUILD_TYPE Release) #Debug, Release
+set(CMAKE_BUILD_TYPE Debug) #Debug, Release
+
 
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
@@ -155,7 +156,8 @@ 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)
-  add_subdirectory(test)
+  # TODO: uncomment when tests are modified
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include ${OMPL_INCLUDE_DIRS})

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -126,7 +126,8 @@ class AStarAlgorithm
   void setGoal(
     const unsigned int & mx,
     const unsigned int & my,
-    const unsigned int & dim_3);
+    const unsigned int & dim_3,
+    const GoalHeading & goal_heading = GoalHeading::DEFAULT);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -151,11 +152,11 @@ class AStarAlgorithm
    */
   NodePtr & getStart();
 
-  /**
-   * @brief Get pointer reference to goal node
-   * @return Node pointer reference to goal node
-   */
-  NodePtr & getGoal();
+  // /**
+  //  * @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
@@ -262,9 +263,9 @@ class AStarAlgorithm
   unsigned int _dim3_size;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
+  CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodePtr _goal;
+  NodeVector _goals;
 
   Graph _graph;
   NodeQueue _queue;

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -34,6 +34,7 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
+  typedef std::vector<NodePtr> NodeVector;
   typedef typename NodeT::Coordinates Coordinates;
   typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
 
@@ -87,7 +88,7 @@ class AnalyticExpansion
    */
   NodePtr tryAnalyticExpansion(
     const NodePtr & current_node,
-    const NodePtr & goal_node,
+    const NodeVector & goal_nodes,
     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 GoalHeading
+{
+  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 GoalHeading & n)
+{
+  switch (n) {
+    case GoalHeading::DEFAULT:
+      return "DEFAULT";
+    case GoalHeading::BIDIRECTIONAL:
+      return "BIDIRECTIONAL";
+    case GoalHeading::ALL_DIRECTION:
+      return "ALL_DIRECTION";
+    default:
+      return "Unknown";
+  }
+}
+
+inline GoalHeading fromStringToGH(const std::string & n)
+{
+  if (n == "DEFAULT") {
+    return GoalHeading::DEFAULT;
+  } else if (n == "BIDIRECTIONAL") {
+    return GoalHeading::BIDIRECTIONAL;
+  } else if (n == "ALL_DIRECTION") {
+    return GoalHeading::ALL_DIRECTION;
+  } else {
+    return GoalHeading::UNKNOWN;
+  }
+}
+
 const float UNKNOWN = 255.0;
 const float OCCUPIED = 254.0;
 const float INSCRIBED = 253.0;

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -165,12 +165,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);
     }

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -120,6 +120,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
+  GoalHeading _goal_heading;
   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,6 +117,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
     _planned_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
+  GoalHeading _goal_heading;
   std::mutex _mutex;
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 

nav2_smac_planner/src/a_star.cpp

@@ -41,9 +41,9 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   _x_size(0),
   _y_size(0),
   _search_info(search_info),
-  _goal_coordinates(Coordinates()),
+  _goals_coordinates(CoordinateVector()),
   _start(nullptr),
-  _goal(nullptr),
+  _goals(NodeVector()),
   _motion_model(motion_model)
 {
   _graph.reserve(100000);
@@ -179,30 +179,72 @@ template<>
 void AStarAlgorithm<Node2D>::setGoal(
   const unsigned int & mx,
   const unsigned int & my,
-  const unsigned int & dim_3)
+  const unsigned int & dim_3,
+  const GoalHeading & goal_heading)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
   }
+  _goals.clear();
+  _goals_coordinates.clear();
 
-  _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
-  _goal_coordinates = Node2D::Coordinates(mx, my);
+  _goals.push_back(addToGraph(Node2D::getIndex(mx, my, getSizeX())));
+  _goals_coordinates.push_back(Node2D::Coordinates(mx, my));
 }
 
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::setGoal(
   const unsigned int & mx,
   const unsigned int & my,
-  const unsigned int & dim_3)
+  const unsigned int & dim_3,
+  const GoalHeading & goal_heading)
 {
-  _goal = addToGraph(NodeT::getIndex(mx, my, dim_3));
-
-  typename NodeT::Coordinates goal_coords(
-    static_cast<float>(mx),
-    static_cast<float>(my),
-    static_cast<float>(dim_3));
+  _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)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3)));
+      break;
+    case GoalHeading::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_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3 + num_bins / 2)));
+      break;
+    case GoalHeading::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_coordinates.push_back(
+          typename NodeT::Coordinates(
+            static_cast<float>(mx),
+            static_cast<float>(my),
+            static_cast<float>(i)));
+      }
+      break;
+    case GoalHeading::UNKNOWN:
+      throw std::runtime_error("Goal heading is UNKNOWN.");
+      break;
+  }
 
-  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
+  if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
@@ -211,8 +253,11 @@ void AStarAlgorithm<NodeT>::setGoal(
       _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my);
   }
 
-  _goal_coordinates = goal_coords;
-  _goal->setPose(_goal_coordinates);
+  _goals_coordinates = goals_coordinates;
+  _goals = goals;
+  for (unsigned int i = 0; i < goals.size(); ++i) {
+    _goals[i]->setPose(_goals_coordinates[i]);
+  }
 }
 
 template<typename NodeT>
@@ -224,15 +269,21 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
   }
 
   // Check if points were filled in
-  if (!_start || !_goal) {
-    throw std::runtime_error("Failed to compute path, no valid start or goal given.");
+  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 &&
-    !_goal->isNodeValid(_traverse_unknown, _collision_checker))
-  {
-    throw nav2_core::GoalOccupied("Goal was in lethal cost");
+  if (getToleranceHeuristic() < 0.001) {
+    for (auto & goal : _goals) {
+      if (!goal->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
@@ -314,9 +365,8 @@ bool AStarAlgorithm<NodeT>::createPath(
     current_node->visited();
 
     // 2.1) Use an analytic expansion (if available) to generate a path
-    expansion_result = nullptr;
     expansion_result = _expander->tryAnalyticExpansion(
-      current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
+      current_node, _goals, neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
@@ -366,7 +416,7 @@ bool AStarAlgorithm<NodeT>::createPath(
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
 {
-  return node == getGoal();
+  return std::find(_goals.begin(), _goals.end(), node) != _goals.end();
 }
 
 template<typename NodeT>
@@ -375,11 +425,11 @@ typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
   return _start;
 }
 
-template<typename NodeT>
-typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
-{
-  return _goal;
-}
+// template<typename NodeT>
+// typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
+// {
+//   return _goals[0];
+// }
 
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode()
@@ -403,9 +453,11 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
 {
   const Coordinates node_coords =
     NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
-  float heuristic = NodeT::getHeuristicCost(
-    node_coords, _goal_coordinates);
-
+  // we want to get the shortest heuristic cost from all of the goals
+  float heuristic = std::numeric_limits<float>::max();
+  for (auto & goal : _goals_coordinates) {
+    heuristic = std::min(heuristic, NodeT::getHeuristicCost(node_coords, goal));
+  }
   if (heuristic < _best_heuristic_node.first) {
     _best_heuristic_node = {heuristic, node->getIndex()};
   }