feat(freespace_planning_algorithms): use distance to nearest obstacle…

… to improve path planning (autowarefoundation#8089)

* refactor freespace planning algorithms

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix error

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* use vector instead of map for a-star node graph

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove unnecessary parameters

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* precompute average turning radius

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add threshold for minimum distance between direction changes

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* apply curvature weight and change in curvature weight

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* store total cost instead of heuristic cost

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix reverse weight application

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix parameter description in README

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* implement edt map to store distance to nearest obstacle for each grid cell

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* use obstacle edt in collision check

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add cost for distance to obstacle

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix formats

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add missing include

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* refactor functions

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add missing include

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* precompute number of margin cells to reduce out of range vertices check necessity

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add reset data function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add member function set() to AstarNode struct

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* implement adaptive expansion distance

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove unnecessary code

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* interpolate nodes with large expansion distance

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* minor refactor

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* ensure expansion distance is larger than grid cell diagonal

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* compute collision free distance to goal map

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* use obstacle edt when computing collision free distance map

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* minor refactor

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix expansion cost function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* set distance map before setting start node

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* refactor detect collision function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add missing variable initialization

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove declared but undefined function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove unnecessary checks

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* minor fix

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* refactor computeEDTMap function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove unnecessary code

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* set min and max expansion distance after setting costmap

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* refactor detectCollision function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove unused function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* change default parameter values

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix computeEDTMap function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* rename parameter

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* use linear function for obstacle distance cost

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix rrtstar obstacle check

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* remove redundant return statements

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* check goal pose validity before setting collision free distance map

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* declare variables as const where necessary

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* compare front and back lengths when setting min and max dimension

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add docstring and citation for computeEDTMap function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* suppress spell check

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

planning/autoware_freespace_planner/README.md

@@ -76,9 +76,11 @@ None
 | --------------------------- | ------ | ------------------------------------------------------- |
 | `only_behind_solutions`     | bool   | whether restricting the solutions to be behind the goal |
 | `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                |
 | `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                    |
 
 #### RRT\* search parameters
 

planning/autoware_freespace_planner/config/freespace_planner.param.yaml

@@ -32,9 +32,11 @@
     astar:
       only_behind_solutions: false
       use_back: true
+      adapt_expansion_distance: true
       expansion_distance: 0.5
-      distance_heuristic_weight: 1.0
+      distance_heuristic_weight: 1.5
       smoothness_weight: 0.5
+      obstacle_distance_weight: 1.5
 
     # -- RRT* search Configurations --
     rrtstar:

planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp

@@ -573,6 +573,7 @@ void FreespacePlannerNode::initializePlanningAlgorithm()
   extended_vehicle_shape.length += margin;
   extended_vehicle_shape.width += margin;
   extended_vehicle_shape.base2back += margin / 2;
+  extended_vehicle_shape.setMinMaxDimension();
 
   const auto planner_common_param = getPlannerCommonParam();
 

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp

@@ -24,6 +24,7 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
+#include <limits>
 #include <vector>
 
 namespace autoware::freespace_planning_algorithms
@@ -63,6 +64,8 @@ struct VehicleShape
   double base_length;
   double max_steering;
   double base2back;  // base_link to rear [m]
+  double min_dimension;
+  double max_dimension;
 
   VehicleShape() = default;
 
@@ -74,6 +77,7 @@ struct VehicleShape
     max_steering(max_steering),
     base2back(base2back)
   {
+    setMinMaxDimension();
   }
 
   explicit VehicleShape(
@@ -84,6 +88,19 @@ struct VehicleShape
     max_steering(vehicle_info.max_steer_angle_rad),
     base2back(vehicle_info.rear_overhang_m + margin / 2.0)
   {
+    setMinMaxDimension();
+  }
+
+  void setMinMaxDimension()
+  {
+    const auto base2front = length - base2back;
+    if (base2back <= base2front) {
+      min_dimension = std::min(0.5 * width, base2back);
+      max_dimension = std::hypot(base2front, 0.5 * width);
+    } else {
+      min_dimension = std::min(0.5 * width, base2front);
+      max_dimension = std::hypot(base2back, 0.5 * width);
+    }
   }
 };
 
@@ -156,6 +173,16 @@ class AbstractPlanningAlgorithm
     std::vector<IndexXY> & vertex_indexes_2d) const;
   bool detectBoundaryExit(const IndexXYT & base_index) const;
   bool detectCollision(const IndexXYT & base_index) const;
+  bool detectCollision(const geometry_msgs::msg::Pose & base_pose) const;
+
+  // cspell: ignore Toriwaki
+  /// @brief Computes the euclidean distance to the nearest obstacle for each grid cell.
+  /// @cite T., Saito, and J., Toriwaki "New algorithms for euclidean distance transformation of an
+  /// n-dimensional digitized picture with applications," Pattern Recognition 27, 1994
+  /// https://doi.org/10.1016/0031-3203(94)90133-3
+  /// @details first, distance values are computed along each row. Then, the computed values are
+  /// used to to compute the minimum distance along each column.
+  void computeEDTMap();
 
   template <typename IndexType>
   inline bool isOutOfRange(const IndexType & index) const
@@ -194,6 +221,12 @@ class AbstractPlanningAlgorithm
     return is_obstacle_table_[indexToId(index)];
   }
 
+  template <typename IndexType>
+  inline double getObstacleEDT(const IndexType & index) const
+  {
+    return edt_map_[indexToId(index)];
+  }
+
   // compute single dimensional grid cell index from 2 dimensional index
   template <typename IndexType>
   inline int indexToId(const IndexType & index) const
@@ -216,6 +249,9 @@ 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_;
+
   // pose in costmap frame
   geometry_msgs::msg::Pose start_pose_;
   geometry_msgs::msg::Pose goal_pose_;

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp

@@ -43,11 +43,13 @@ struct AstarParam
   // base configs
   bool only_behind_solutions;  // solutions should be behind the goal
   bool use_back;               // backward search
+  bool adapt_expansion_distance;
   double expansion_distance;
 
   // search configs
   double distance_heuristic_weight;  // obstacle threshold on grid [0,255]
   double smoothness_weight;
+  double obstacle_distance_weight;
 };
 
 struct AstarNode
@@ -56,9 +58,11 @@ struct AstarNode
   double x;                              // x
   double y;                              // y
   double theta;                          // theta
-  double gc = 0;                         // actual motion cost
-  double fc = 0;                         // total node cost
-  double dir_distance = 0;               // distance traveled from last direction change
+  double gc = 0.0;                       // actual motion cost
+  double fc = 0.0;                       // total node cost
+  double dir_distance = 0.0;             // distance traveled from last direction change
+  double dist_to_goal = 0.0;             // euclidean distance to goal pose
+  double dist_to_obs = 0.0;              // euclidean distance to nearest obstacle
   int steering_index;                    // steering index
   bool is_back;                          // true if the current direction of the vehicle is back
   AstarNode * parent = nullptr;          // parent node
@@ -82,46 +86,9 @@ struct NodeComparison
   bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; }
 };
 
-struct NodeUpdate
-{
-  double shift_x;
-  double shift_y;
-  double shift_theta;
-  double distance;
-  int steering_index;
-  bool is_back;
-
-  NodeUpdate rotated(const double theta) const
-  {
-    NodeUpdate result = *this;
-    result.shift_x = std::cos(theta) * this->shift_x - std::sin(theta) * this->shift_y;
-    result.shift_y = std::sin(theta) * this->shift_x + std::cos(theta) * this->shift_y;
-    return result;
-  }
-
-  NodeUpdate flipped() const
-  {
-    NodeUpdate result = *this;
-    result.shift_y = -result.shift_y;
-    result.shift_theta = -result.shift_theta;
-    return result;
-  }
-
-  NodeUpdate reversed() const
-  {
-    NodeUpdate result = *this;
-    result.shift_x = -result.shift_x;
-    result.shift_theta = -result.shift_theta;
-    result.is_back = !result.is_back;
-    return result;
-  }
-};
-
 class AstarSearch : public AbstractPlanningAlgorithm
 {
 public:
-  using TransitionTable = std::vector<std::vector<NodeUpdate>>;
-
   AstarSearch(
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
     const AstarParam & astar_param);
@@ -134,12 +101,15 @@ class AstarSearch : public AbstractPlanningAlgorithm
       AstarParam{
         node.declare_parameter<bool>("astar.only_behind_solutions"),
         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.distance_heuristic_weight"),
-        node.declare_parameter<double>("astar.smoothness_weight")})
+        node.declare_parameter<double>("astar.smoothness_weight"),
+        node.declare_parameter<double>("astar.obstacle_distance_weight")})
   {
   }
 
+  void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override;
   bool makePlan(const Pose & start_pose, const Pose & goal_pose) override;
 
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
@@ -150,27 +120,26 @@ class AstarSearch : public AbstractPlanningAlgorithm
   }
 
 private:
-  void setTransitionTable();
   void setCollisionFreeDistanceMap();
   bool search();
-  void expandNodes(AstarNode & current_node);
+  void expandNodes(AstarNode & current_node, const bool is_back = false);
   void resetData();
   void setPath(const AstarNode & goal);
   bool setStartNode();
-  bool setGoalNode();
   double estimateCost(const Pose & pose, const IndexXYT & index) const;
   bool isGoal(const AstarNode & node) 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;
 
   // Algorithm specific param
   AstarParam astar_param_;
 
   // hybrid astar variables
-  TransitionTable transition_table_;
   std::vector<AstarNode> graph_;
   std::vector<double> col_free_distance_map_;
 
@@ -185,6 +154,20 @@ class AstarSearch : public AbstractPlanningAlgorithm
   double steering_resolution_;
   double heading_resolution_;
   double avg_turning_radius_;
+  double min_expansion_dist_;
+  double max_expansion_dist_;
+
+  // 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
+  // parameters.
+
+  // expansion distance factors
+  static constexpr double base_length_max_expansion_factor_ = 0.5;
+  static constexpr double dist_to_goal_expansion_factor_ = 0.15;
+  static constexpr double dist_to_obs_expansion_factor_ = 0.3;
+
+  // cost free obstacle distance
+  static constexpr double cost_free_obs_dist = 5.0;
 };
 }  // namespace autoware::freespace_planning_algorithms
 

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp

@@ -31,39 +31,26 @@ static constexpr double eps = 0.001;
 
 inline double getTurningRadius(const double base_length, const double steering_angle)
 {
-  return base_length / tan(steering_angle);
-}
-
-inline geometry_msgs::msg::Pose getPoseShift(
-  const double yaw, const double base_length, const double steering_angle, const double distance)
-{
-  geometry_msgs::msg::Pose pose;
-  if (abs(steering_angle) < eps) {
-    pose.position.x = distance * cos(yaw);
-    pose.position.y = distance * sin(yaw);
-    return pose;
-  }
-  const double R = getTurningRadius(base_length, steering_angle);
-  const double beta = distance / R;
-  pose.position.x = R * sin(yaw + beta) - R * sin(yaw);
-  pose.position.y = R * cos(yaw) - R * cos(yaw + beta);
-  pose.orientation = autoware::universe_utils::createQuaternionFromYaw(beta);
-  return pose;
+  return base_length / std::tan(steering_angle);
 }
 
 inline geometry_msgs::msg::Pose getPose(
   const geometry_msgs::msg::Pose & current_pose, const double base_length,
   const double steering_angle, const double distance)
 {
-  const double current_yaw = tf2::getYaw(current_pose.orientation);
-  const auto shift = getPoseShift(current_yaw, base_length, steering_angle, distance);
   auto pose = current_pose;
-  pose.position.x += shift.position.x;
-  pose.position.y += shift.position.y;
-  if (abs(steering_angle) > eps) {
-    pose.orientation = autoware::universe_utils::createQuaternionFromYaw(
-      current_yaw + tf2::getYaw(shift.orientation));
+  const double yaw = tf2::getYaw(current_pose.orientation);
+  if (std::abs(steering_angle) < eps) {
+    pose.position.x += distance * std::cos(yaw);
+    pose.position.y += distance * std::sin(yaw);
+    return pose;
   }
+
+  const double R = getTurningRadius(base_length, steering_angle);
+  const double beta = distance / R;
+  pose.position.x += (R * std::sin(yaw + beta) - R * std::sin(yaw));
+  pose.position.y += (R * std::cos(yaw) - R * std::cos(yaw + beta));
+  pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw + beta);
   return pose;
 }
 

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

@@ -114,13 +114,19 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
       .def_readwrite(
         "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
       .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)
+      .def_readwrite(
+        "adapt_expansion_distance",
+        &freespace_planning_algorithms::AstarParam::adapt_expansion_distance)
       .def_readwrite(
         "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance)
       .def_readwrite(
         "distance_heuristic_weight",
         &freespace_planning_algorithms::AstarParam::distance_heuristic_weight)
       .def_readwrite(
-        "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight);
+        "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight)
+      .def_readwrite(
+        "obstacle_distance_weight",
+        &freespace_planning_algorithms::AstarParam::obstacle_distance_weight);
   auto pyPlannerCommonParam =
     py::class_<freespace_planning_algorithms::PlannerCommonParam>(
       p, "PlannerCommonParam", py::dynamic_attr())

planning/autoware_freespace_planning_algorithms/scripts/example/example.py

@@ -46,9 +46,11 @@
 astar_param = fp.AstarParam()
 astar_param.only_behind_solutions = False
 astar_param.use_back = True
+astar_param.adapt_expansion_distance = True
 astar_param.expansion_distance = 0.4
 astar_param.distance_heuristic_weight = 1.0
 astar_param.smoothness_weight = 1.0
+astar_param.obstacle_distance_weight = 1.0
 
 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)