feat(freespace_planning_algorithms): implement support for multiple g…

…oal candidates in A star planner (autowarefoundation#8092) * refactor freespace planning algorithms Signed-off-by: mohammad alqudah <[email protected]> * fix error Signed-off-by: mohammad alqudah <[email protected]> * use vector instead of map for a-star node graph Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary parameters Signed-off-by: mohammad alqudah <[email protected]> * precompute average turning radius Signed-off-by: mohammad alqudah <[email protected]> * add threshold for minimum distance between direction changes Signed-off-by: mohammad alqudah <[email protected]> * apply curvature weight and change in curvature weight Signed-off-by: mohammad alqudah <[email protected]> * store total cost instead of heuristic cost Signed-off-by: mohammad alqudah <[email protected]> * fix reverse weight application Signed-off-by: mohammad alqudah <[email protected]> * fix parameter description in README Signed-off-by: mohammad alqudah <[email protected]> * implement edt map to store distance to nearest obstacle for each grid cell Signed-off-by: mohammad alqudah <[email protected]> * use obstacle edt in collision check Signed-off-by: mohammad alqudah <[email protected]> * add cost for distance to obstacle Signed-off-by: mohammad alqudah <[email protected]> * fix formats Signed-off-by: mohammad alqudah <[email protected]> * add missing include Signed-off-by: mohammad alqudah <[email protected]> * refactor functions Signed-off-by: mohammad alqudah <[email protected]> * add missing include Signed-off-by: mohammad alqudah <[email protected]> * implement backward search option Signed-off-by: mohammad alqudah <[email protected]> * precompute number of margin cells to reduce out of range vertices check necessity Signed-off-by: mohammad alqudah <[email protected]> * add reset data function Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah <[email protected]> * fix is_back flag logic Signed-off-by: mohammad alqudah <[email protected]> * add member function set() to AstarNode struct Signed-off-by: mohammad alqudah <[email protected]> * implement adaptive expansion distance Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * interpolate nodes with large expansion distance Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> * fix interpolation for backward search Signed-off-by: mohammad alqudah <[email protected]> * ensure expansion distance is larger than grid cell diagonal Signed-off-by: mohammad alqudah <[email protected]> * compute collision free distance to goal map Signed-off-by: mohammad alqudah <[email protected]> * use obstacle edt when computing collision free distance map Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> * fix expansion cost function Signed-off-by: mohammad alqudah <[email protected]> * set distance map before setting start node Signed-off-by: mohammad alqudah <[email protected]> * refactor detect collision function Signed-off-by: mohammad alqudah <[email protected]> * use flag instead of enum Signed-off-by: mohammad alqudah <[email protected]> * add missing variable initialization Signed-off-by: mohammad alqudah <[email protected]> * remove declared but undefined function Signed-off-by: mohammad alqudah <[email protected]> * refactor makePlan() function Signed-off-by: mohammad alqudah <[email protected]> * remove bool return statement for void function Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary checks Signed-off-by: mohammad alqudah <[email protected]> * minor fix Signed-off-by: mohammad alqudah <[email protected]> * refactor computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * enable both forward and backward search options for multiple goal candidates Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * set min and max expansion distance after setting costmap Signed-off-by: mohammad alqudah <[email protected]> * refactor detectCollision function Signed-off-by: mohammad alqudah <[email protected]> * remove unused function Signed-off-by: mohammad alqudah <[email protected]> * change default parameter values Signed-off-by: mohammad alqudah <[email protected]> * add missing last waypoint Signed-off-by: mohammad alqudah <[email protected]> * fix computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * rename parameter Signed-off-by: mohammad alqudah <[email protected]> * use linear function for obstacle distance cost Signed-off-by: mohammad alqudah <[email protected]> * fix rrtstar obstacle check Signed-off-by: mohammad alqudah <[email protected]> * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah <[email protected]> * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah <[email protected]> * append shifted goal pose to waypoints for more accurate arrival Signed-off-by: mohammad alqudah <[email protected]> * remove redundant return statements Signed-off-by: mohammad alqudah <[email protected]> * check goal pose validity before setting collision free distance map Signed-off-by: mohammad alqudah <[email protected]> * declare variables as const where necessary Signed-off-by: mohammad alqudah <[email protected]> * initialize vectors using assign function Signed-off-by: mohammad alqudah <[email protected]> * compare front and back lengths when setting min and max dimension Signed-off-by: mohammad alqudah <[email protected]> * add docstring and citation for computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah <[email protected]> * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah <[email protected]> * add cost for lateral distance near goal Signed-off-by: mohammad alqudah <[email protected]> * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * refactor setPath function Signed-off-by: mohammad alqudah <[email protected]> * fix function setPath Signed-off-by: mohammad alqudah <[email protected]> * declare bool var as constant Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary includes Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
a-maumau · Sep 2, 2024 · 9c04530 · 9c04530
1 parent 34e69f0
commit 9c04530
Show file tree

Hide file tree

Showing 13 changed files with 290 additions and 72 deletions.
diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md
@@ -82,9 +82,11 @@ None
 | `use_back`                  | bool   | whether using backward trajectory                       |
 | `adapt_expansion_distance`  | bool   | if true, adapt expansion distance based on environment  |
 | `expansion_distance`        | double | length of expansion for node transitions                |
+| `near_goal_distance`        | double | near goal distance threshold                            |
 | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost             |
 | `smoothness_weight`         | double | cost factor for change in curvature                     |
 | `obstacle_distance_weight`  | double | cost factor for distance to obstacle                    |
+| `goal_lat_distance_weight`  | double | cost factor for lateral distance from goal              |
 
 #### RRT\* search parameters
 

diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
@@ -22,7 +22,7 @@
     theta_size: 144
     angle_goal_range: 6.0
     lateral_goal_range: 0.5
-    longitudinal_goal_range: 2.0
+    longitudinal_goal_range: 1.0
     curve_weight: 0.5
     reverse_weight: 1.0
     direction_change_weight: 1.5
@@ -36,9 +36,11 @@
       use_back: true
       adapt_expansion_distance: true
       expansion_distance: 0.5
+      near_goal_distance: 4.0
       distance_heuristic_weight: 1.5
       smoothness_weight: 0.5
       obstacle_distance_weight: 1.5
+      goal_lat_distance_weight: 5.0
 
     # -- RRT* search Configurations --
     rrtstar:

diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json
@@ -146,6 +146,11 @@
               "default": 0.5,
               "description": "Distance for expanding nodes in A* search."
             },
+            "near_goal_distance": {
+              "type": "number",
+              "default": 4.0,
+              "description": "Distance threshold to consider near goal."
+            },
             "distance_heuristic_weight": {
               "type": "number",
               "default": 1.0,
@@ -160,6 +165,11 @@
               "type": "number",
               "default": 0.5,
               "description": "Weight for distance to obstacle in A* search."
+            },
+            "goal_lat_distance_weight": {
+              "type": "number",
+              "default": 0.5,
+              "description": "Weight for lateral distance from original goal."
             }
           },
           "required": [

diff --git a/...ning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/...ning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp
@@ -377,8 +377,7 @@ void FreespacePlannerNode::updateTargetIndex()
     } else {
       // Switch to next partial trajectory
       prev_target_index_ = target_index_;
-      target_index_ =
-        getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_);
+      target_index_ = new_target_index;
     }
   }
 }

diff --git a/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -16,6 +16,7 @@
 #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/math/normalization.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
 #include <geometry_msgs/msg/pose_array.hpp>
@@ -29,13 +30,22 @@
 
 namespace autoware::freespace_planning_algorithms
 {
+using autoware::universe_utils::normalizeRadian;
+
+geometry_msgs::msg::Pose transformPose(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform);
 int discretizeAngle(const double theta, const int theta_size);
 
 struct IndexXYT
 {
   int x;
   int y;
   int theta;
+
+  bool operator==(const IndexXYT & index) const
+  {
+    return (x == index.x && y == index.y && theta == index.theta);
+  }
 };
 
 struct IndexXY
@@ -138,6 +148,12 @@ struct PlannerWaypoints
   double compute_length() const;
 };
 
+struct EDTData
+{
+  double distance;
+  double angle;
+};
+
 class AbstractPlanningAlgorithm
 {
 public:
@@ -162,6 +178,9 @@ class AbstractPlanningAlgorithm
   virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
   virtual bool makePlan(
     const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0;
+  virtual bool makePlan(
+    const geometry_msgs::msg::Pose & start_pose,
+    const std::vector<geometry_msgs::msg::Pose> & goal_candidates) = 0;
   virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const;
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
   double getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const;
@@ -223,7 +242,7 @@ class AbstractPlanningAlgorithm
   }
 
   template <typename IndexType>
-  inline double getObstacleEDT(const IndexType & index) const
+  inline EDTData getObstacleEDT(const IndexType & index) const
   {
     return edt_map_[indexToId(index)];
   }
@@ -235,6 +254,19 @@ class AbstractPlanningAlgorithm
     return index.y * costmap_.info.width + index.x;
   }
 
+  inline double getVehicleBaseToFrameDistance(const double angle) const
+  {
+    const double normalized_angle = std::abs(normalizeRadian(angle));
+    const double w = 0.5 * collision_vehicle_shape_.width;
+    const double l_b = collision_vehicle_shape_.base2back;
+    const double l_f = collision_vehicle_shape_.length - l_b;
+
+    if (normalized_angle < atan(w / l_f)) return l_f / cos(normalized_angle);
+    if (normalized_angle < M_PI_2) return w / sin(normalized_angle);
+    if (normalized_angle < M_PI_2 + atan(l_b / w)) return w / cos(normalized_angle - M_PI_2);
+    return l_b / cos(M_PI - normalized_angle);
+  }
+
   PlannerCommonParam planner_common_param_;
   VehicleShape collision_vehicle_shape_;
 
@@ -250,8 +282,8 @@ class AbstractPlanningAlgorithm
   // is_obstacle's table
   std::vector<bool> is_obstacle_table_;
 
-  // Euclidean distance transform map (distance to nearest obstacle cell)
-  std::vector<double> edt_map_;
+  // Euclidean distance transform map (distance & angle info to nearest obstacle cell)
+  std::vector<EDTData> edt_map_;
 
   // pose in costmap frame
   geometry_msgs::msg::Pose start_pose_;

diff --git a/...space_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/...space_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
@@ -23,6 +23,8 @@
 #include <nav_msgs/msg/path.hpp>
 #include <std_msgs/msg/header.hpp>
 
+#include <boost/optional/optional.hpp>
+
 #include <cmath>
 #include <functional>
 #include <iostream>
@@ -46,11 +48,13 @@ struct AstarParam
   bool use_back;               // backward search
   bool adapt_expansion_distance;
   double expansion_distance;
+  double near_goal_distance;
 
   // search configs
   double distance_heuristic_weight;  // obstacle threshold on grid [0,255]
   double smoothness_weight;
   double obstacle_distance_weight;
+  double goal_lat_distance_weight;
 };
 
 struct AstarNode
@@ -105,15 +109,19 @@ class AstarSearch : public AbstractPlanningAlgorithm
         node.declare_parameter<bool>("astar.use_back"),
         node.declare_parameter<bool>("astar.adapt_expansion_distance"),
         node.declare_parameter<double>("astar.expansion_distance"),
+        node.declare_parameter<double>("astar.near_goal_distance"),
         node.declare_parameter<double>("astar.distance_heuristic_weight"),
         node.declare_parameter<double>("astar.smoothness_weight"),
-        node.declare_parameter<double>("astar.obstacle_distance_weight")})
+        node.declare_parameter<double>("astar.obstacle_distance_weight"),
+        node.declare_parameter<double>("astar.goal_lat_distance_weight")})
   {
   }
 
   void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override;
   bool makePlan(const Pose & start_pose, const Pose & goal_pose) override;
 
+  bool makePlan(const Pose & start_pose, const std::vector<Pose> & goal_candidates) override;
+
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
 
   inline int getKey(const IndexXYT & index)
@@ -127,16 +135,18 @@ class AstarSearch : public AbstractPlanningAlgorithm
   void expandNodes(AstarNode & current_node, const bool is_back = false);
   void resetData();
   void setPath(const AstarNode & goal);
-  void setStartNode();
+  void setStartNode(const double cost_offset = 0.0);
   double estimateCost(const Pose & pose, const IndexXYT & index) const;
   bool isGoal(const AstarNode & node) const;
+  void setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const;
   Pose node2pose(const AstarNode & node) const;
 
   double getExpansionDistance(const AstarNode & current_node) const;
   double getSteeringCost(const int steering_index) const;
   double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const;
   double getDirectionChangeCost(const double dir_distance) const;
-  double getObsDistanceCost(const double obs_distance) const;
+  double getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const;
+  double getLatDistanceCost(const Pose & pose) const;
 
   // Algorithm specific param
   AstarParam astar_param_;
@@ -150,6 +160,11 @@ class AstarSearch : public AbstractPlanningAlgorithm
   // goal node, which may helpful in testing and debugging
   AstarNode * goal_node_;
 
+  mutable boost::optional<Pose> shifted_goal_pose_;
+
+  // alternate goals for when multiple goal candidates are given
+  std::vector<Pose> alternate_goals_;
+
   // distance metric option (removed when the reeds_shepp gets stable)
   bool use_reeds_shepp_;
 
@@ -158,7 +173,9 @@ class AstarSearch : public AbstractPlanningAlgorithm
   double avg_turning_radius_;
   double min_expansion_dist_;
   double max_expansion_dist_;
+  double near_goal_dist_;
   bool is_backward_search_;
+  bool is_multiple_goals_;
 
   // the following constexpr values were found to be best by trial and error, through multiple
   // tests, and are not expected to be changed regularly, therefore they were not made into ros
@@ -169,8 +186,11 @@ class AstarSearch : public AbstractPlanningAlgorithm
   static constexpr double dist_to_goal_expansion_factor_ = 0.15;
   static constexpr double dist_to_obs_expansion_factor_ = 0.3;
 
+  // initial cost offset for multi goal backward search
+  static constexpr double multi_goal_backward_cost_offset = 5.0;
+
   // cost free obstacle distance
-  static constexpr double cost_free_obs_dist = 5.0;
+  static constexpr double cost_free_obs_dist = 1.0;
 };
 }  // namespace autoware::freespace_planning_algorithms
 

diff --git a/..._freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp b/..._freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp
@@ -58,6 +58,9 @@ class RRTStar : public AbstractPlanningAlgorithm
   bool makePlan(
     const geometry_msgs::msg::Pose & start_pose,
     const geometry_msgs::msg::Pose & goal_pose) override;
+  bool makePlan(
+    const geometry_msgs::msg::Pose & start_pose,
+    const std::vector<geometry_msgs::msg::Pose> & goal_candidates) override;
   bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const override;
 
 private:

diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -120,14 +120,19 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
         &freespace_planning_algorithms::AstarParam::adapt_expansion_distance)
       .def_readwrite(
         "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance)
+      .def_readwrite(
+        "near_goal_distance", &freespace_planning_algorithms::AstarParam::near_goal_distance)
       .def_readwrite(
         "distance_heuristic_weight",
         &freespace_planning_algorithms::AstarParam::distance_heuristic_weight)
       .def_readwrite(
         "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight)
       .def_readwrite(
         "obstacle_distance_weight",
-        &freespace_planning_algorithms::AstarParam::obstacle_distance_weight);
+        &freespace_planning_algorithms::AstarParam::obstacle_distance_weight)
+      .def_readwrite(
+        "goal_lat_distance_weight",
+        &freespace_planning_algorithms::AstarParam::goal_lat_distance_weight);
   auto pyPlannerCommonParam =
     py::class_<freespace_planning_algorithms::PlannerCommonParam>(
       p, "PlannerCommonParam", py::dynamic_attr())

diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
@@ -49,9 +49,11 @@
 astar_param.use_back = True
 astar_param.adapt_expansion_distance = True
 astar_param.expansion_distance = 0.4
+astar_param.near_goal_distance = 3.0
 astar_param.distance_heuristic_weight = 1.0
 astar_param.smoothness_weight = 1.0
 astar_param.obstacle_distance_weight = 1.0
+astar_param.goal_lat_distance_weight = 1.0
 
 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)