[Smac Planner] Enable goal orientation non-specificity

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
@@ -262,9 +263,9 @@ class AStarAlgorithm
   unsigned int _dim3_size;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
+  std::vector<Coordinates> _goals_coordinates;
   NodePtr _start;
-  NodePtr _goal;
+  std::vector<NodePtr> _goals;
 
   Graph _graph;
   NodeQueue _queue;

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, 
+  ANY_HEADING = 3,
+};
+
 inline std::string toString(const MotionModel & n)
 {
   switch (n) {
@@ -59,6 +67,38 @@ 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::ANY_HEADING:
+      return "ANY_HEADING";
+    default:
+      return "Unknown";
+  }
+}
+
+// TODO(@stevedanomodolor) find a better way to do this
+
+inline GoalHeading fromStringToGH(const std::string & n)
+{
+  if(n == "DEFAULT") {
+    return GoalHeading::DEFAULT;
+  } else if(n == "BIDIRECTIONAL") {
+    return GoalHeading::BIDIRECTIONAL;
+  } else if (n == "ANY_HEADING") {
+    return GoalHeading::ANY_HEADING;
+  } 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

@@ -110,6 +110,8 @@ struct HybridMotionTable
    */
   float getAngleFromBin(const unsigned int & bin_idx);
 
+  unsigned int getNumOfBins();
+
   MotionModel motion_model = MotionModel::UNKNOWN;
   MotionPoses projections;
   unsigned int size_x;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -94,6 +94,8 @@ struct LatticeMotionTable
    */
   float & getAngleFromBin(const unsigned int & bin_idx);
 
+  unsigned int getNumOfBins();
+
   unsigned int size_x;
   unsigned int num_angle_quantization;
   float change_penalty;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -118,6 +118,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   double _lookup_table_size;
   double _minimum_turning_radius_global_coords;
   bool _debug_visualizations;
+  std::string _goal_heading_type;
+  GoalHeading _goal_heading;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -111,6 +111,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   double _max_planning_time;
   double _lookup_table_size;
+  std::string _goal_heading_type;
+  GoalHeading _goal_heading;
   std::mutex _mutex;
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 

nav2_smac_planner/src/a_star.cpp

@@ -179,39 +179,98 @@ 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.");
   }
 
-  _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
-  _goal_coordinates = Node2D::Coordinates(mx, my);
+  _goal.clear();
+  _goal_coordinates.clear();
+  _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));
+  _goals.clear();
+  // _goals_coordinates.clear();
+  std::vector<Coordinates> goal_coordinates;
+  unsigned int number_of_bins = NodeT::motion_table.getNumOfBins();
+  if(goal_heading == GoalHeading::DEFAULT){
+    _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+    goal_coordinates.push_back(typename NodeT::Coordinates(
+      static_cast<float>(mx),
+      static_cast<float>(my),
+      static_cast<float>(dim_3)));
+  }else if(goal_heading == GoalHeading::BIDIRECTIONAL)
+  {
+    _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+    // add angle -180
+    _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3 + number_of_bins/2)));
+    goal_coordinates.push_back(typename NodeT::Coordinates(
+      static_cast<float>(mx),
+      static_cast<float>(my),
+      static_cast<float>(dim_3)));
+    goal_coordinates.push_back(typename NodeT::Coordinates(
+      static_cast<float>(mx),
+      static_cast<float>(my),
+      static_cast<float>(dim_3 + number_of_bins/2)));
+
+  }else if (goal_heading == GoalHeading::ANY_HEADING)
+  {
+    for (unsigned int i = 0; i < number_of_bins; i++)
+    {
+      _goals.push_back(addToGraph(NodeT::getIndex(mx, my, i)));
+      goal_coordinates.push_back(typename NodeT::Coordinates(
+        static_cast<float>(mx),
+        static_cast<float>(my),
+        static_cast<float>(i)));
+    }
+  }
+  else
+  {
+    throw std::runtime_error("Goal Heading not supported");
+  }
 
-  typename NodeT::Coordinates goal_coords(
-    static_cast<float>(mx),
-    static_cast<float>(my),
-    static_cast<float>(dim_3));
+  auto has_goals_changed = [](const std::vector<Coordinates> & current_goal_coordinates,const std::vector<Coordinates> & previous_goal_coordinates) -> bool 
+  {
+    if(current_goal_coordinates.size() != previous_goal_coordinates.size())
+    {
+      return true;
+    }
+    else 
+    {
+      for(unsigned int i = 0; i < goal_coordinates.size() - 1; i++)
+    {
+      if(goal_coordinates[i] != goal_coordinates[i+1])
+      {
+        return true;
+      }
+    }
+    }    
+  }
 
-  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
+  if (!_search_info.cache_obstacle_heuristic || has_goals_changed(goal_coordinates)) {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
 
     NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my);
   }
 
-  _goal_coordinates = goal_coords;
-  _goal->setPose(_goal_coordinates);
+  _goals_coordinates.clear();
+  _goals_coordinates = goal_coordinates;
+  for(int i = 0; i < _goals_coordinates.size(); i++)
+  {
+    _goals[i]->setPose(_goals_coordinates[i]);
+  }
 }
 
 template<typename NodeT>
@@ -368,6 +427,7 @@ bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
   return node == getGoal();
 }
 
+
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
 {

nav2_smac_planner/src/node_hybrid.cpp

@@ -337,6 +337,11 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return bin_idx * bin_size;
 }
 
+unsigned int getNumOfBins()
+{
+  return num_angle_quantization;
+}
+
 NodeHybrid::NodeHybrid(const unsigned int index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),

nav2_smac_planner/src/node_lattice.cpp

@@ -172,6 +172,11 @@ float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return lattice_metadata.heading_angles[bin_idx];
 }
 
+unsigned int getNumOfBins()
+{
+  return lattice_metadata.heading_angles.size();
+}
+
 NodeLattice::NodeLattice(const unsigned int index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -90,7 +90,7 @@ void SmacPlanner2D::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".max_planning_time", rclcpp::ParameterValue(2.0));
   node->get_parameter(name + ".max_planning_time", _max_planning_time);
-
+  
   _motion_model = MotionModel::TWOD;
 
   if (_max_on_approach_iterations <= 0) {

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -154,6 +154,20 @@ void SmacPlannerHybrid::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
   node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
+
+  nav2_util::declare_parameter_if_not_declared(
+  node, name + ".goal_heading", rclcpp::ParameterValue("DEFAULT"));
+  node->get_parameter(name + ".goal_heading", _goal_heading_type);
+
+  _goal_heading = fromStringToGH(_goal_heading_type);
+  if(_goal_heading == GoalHeading::UNKNOWN)
+  {
+    RCLCPP_WARN(
+      _logger,
+      "Unable to get GoalHeader type. Given '%s', "
+      "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
+      _goal_heading_type.c_str());
+  }
   _motion_model = fromString(_motion_model_for_search);
   if (_motion_model == MotionModel::UNKNOWN) {
     RCLCPP_WARN(
@@ -359,7 +373,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
   orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
-  _a_star->setGoal(mx, my, orientation_bin_id);
+  _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -600,6 +614,17 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
             _motion_model_for_search.c_str());
         }
+      }else if (name == _name + ".goal_heading") {
+        _goal_heading_type = parameter.as_string();
+        _goal_heading = fromStringToGH(_goal_heading_type);
+        if(_goal_heading == GoalHeading::UNKNOWN)
+        {
+          RCLCPP_WARN(
+            _logger,
+            "Unable to get GoalHeader type. Given '%s', "
+            "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
+            _goal_heading_type.c_str());
+        }
       }
     }
   }

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -127,6 +127,20 @@ void SmacPlannerLattice::configure(
     node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false));
   node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion);
 
+  nav2_util::declare_parameter_if_not_declared(
+  node, name + ".goal_heading", rclcpp::ParameterValue("DEFAULT"));
+  node->get_parameter(name + ".goal_heading", _goal_heading_type);
+
+  _goal_heading = fromStringToGH(_goal_heading_type);
+  if(_goal_heading == GoalHeading::UNKNOWN)
+  {
+    RCLCPP_WARN(
+      _logger,
+      "Unable to get GoalHeader type. Given '%s', "
+      "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
+      _goal_heading_type.c_str());
+  }
+
   _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
   _search_info.minimum_turning_radius =
     _metadata.min_turning_radius / (_costmap->getResolution());
@@ -264,7 +278,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   }
   _a_star->setGoal(
     mx, my,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)),
+    _goal_heading);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -442,6 +457,19 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         _search_info.minimum_turning_radius =
           _metadata.min_turning_radius / (_costmap->getResolution());
       }
+      else if (name == _name + ".goal_heading") {
+        _goal_heading_type = parameter.as_string();
+        _goal_heading = fromStringToGH(_goal_heading_type);
+        if(_goal_heading == GoalHeading::UNKNOWN)
+        {
+          RCLCPP_WARN(
+            _logger,
+            "Unable to get GoalHeader type. Given '%s', "
+            "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
+            _goal_heading_type.c_str());
+        }
+      }
+
     }
   }