refactor(freespace_planning_algorithm): refactor and improve astar se…

…arch (autowarefoundation#8068) * 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> * 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> * 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> * remove unnecessary code 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> * 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> * minor fix Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * remove unnecessary code Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * change default parameter values Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * rename parameter 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> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
mkquda · Jul 29, 2024 · 357d93c · 357d93c
1 parent 5093495
commit 357d93c
Show file tree

Hide file tree

Showing 16 changed files with 474 additions and 319 deletions.
diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md
@@ -59,14 +59,15 @@ None
 | Parameter                 | Type   | Description                                        |
 | ------------------------- | ------ | -------------------------------------------------- |
 | `time_limit`              | double | time limit of planning                             |
-| `minimum_turning_radius`  | double | minimum turning radius of robot                    |
-| `maximum_turning_radius`  | double | maximum turning radius of robot                    |
+| `maximum_turning_ratio`   | double | max ratio of actual turning range to use           |
+| `turning_steps`           | double | number of turning steps within turning range       |
 | `theta_size`              | double | the number of angle's discretization               |
 | `lateral_goal_range`      | double | goal range of lateral position                     |
 | `longitudinal_goal_range` | double | goal range of longitudinal position                |
 | `angle_goal_range`        | double | goal range of angle                                |
 | `curve_weight`            | double | additional cost factor for curve actions           |
 | `reverse_weight`          | double | additional cost factor for reverse actions         |
+| `direction_change_weight` | double | additional cost factor for switching direction     |
 | `obstacle_threshold`      | double | threshold for regarding a certain grid as obstacle |
 
 #### A\* search parameters
@@ -75,7 +76,9 @@ None
 | --------------------------- | ------ | ------------------------------------------------------- |
 | `only_behind_solutions`     | bool   | whether restricting the solutions to be behind the goal |
 | `use_back`                  | bool   | whether using backward trajectory                       |
+| `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                     |
 
 #### 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
@@ -15,24 +15,26 @@
     # -- Configurations common to the all planners --
     # base configs
     time_limit: 30000.0
-    minimum_turning_radius: 9.0
-    maximum_turning_radius: 9.0
-    turning_radius_size: 1
+    max_turning_ratio: 0.5 # ratio of actual turning limit of vehicle
+    turning_steps: 1
     # search configs
     theta_size: 144
     angle_goal_range: 6.0
-    curve_weight: 1.2
-    reverse_weight: 2.0
     lateral_goal_range: 0.5
     longitudinal_goal_range: 2.0
+    curve_weight: 0.5
+    reverse_weight: 1.0
+    direction_change_weight: 1.5
     # costmap configs
     obstacle_threshold: 100
 
     # -- A* search Configurations --
     astar:
       only_behind_solutions: false
       use_back: true
+      expansion_distance: 0.5
       distance_heuristic_weight: 1.0
+      smoothness_weight: 0.5
 
     # -- RRT* search Configurations --
     rrtstar:

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
@@ -245,6 +245,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
       autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
     vehicle_shape_.length = vehicle_info.vehicle_length_m;
     vehicle_shape_.width = vehicle_info.vehicle_width_m;
+    vehicle_shape_.base_length = vehicle_info.wheel_base_m;
+    vehicle_shape_.max_steering = vehicle_info.max_steer_angle_rad;
     vehicle_shape_.base2back = vehicle_info.rear_overhang_m;
   }
 
@@ -296,18 +298,16 @@ PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam()
 
   // search configs
   p.time_limit = declare_parameter<double>("time_limit");
-  p.minimum_turning_radius = declare_parameter<double>("minimum_turning_radius");
-  p.maximum_turning_radius = declare_parameter<double>("maximum_turning_radius");
-  p.turning_radius_size = declare_parameter<int>("turning_radius_size");
-  p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius);
-  p.turning_radius_size = std::max(p.turning_radius_size, 1);
 
   p.theta_size = declare_parameter<int>("theta_size");
   p.angle_goal_range = declare_parameter<double>("angle_goal_range");
   p.curve_weight = declare_parameter<double>("curve_weight");
   p.reverse_weight = declare_parameter<double>("reverse_weight");
+  p.direction_change_weight = declare_parameter<double>("direction_change_weight");
   p.lateral_goal_range = declare_parameter<double>("lateral_goal_range");
   p.longitudinal_goal_range = declare_parameter<double>("longitudinal_goal_range");
+  p.max_turning_ratio = declare_parameter<double>("max_turning_ratio");
+  p.turning_steps = declare_parameter<int>("turning_steps");
 
   // costmap configs
   p.obstacle_threshold = declare_parameter<int>("obstacle_threshold");
@@ -517,7 +517,13 @@ void FreespacePlannerNode::planTrajectory()
 
   // execute planning
   const rclcpp::Time start = get_clock()->now();
-  const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame);
+  std::string error_msg;
+  bool result = false;
+  try {
+    result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame);
+  } catch (const std::exception & e) {
+    error_msg = e.what();
+  }
   const rclcpp::Time end = get_clock()->now();
 
   RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds());
@@ -530,9 +536,8 @@ void FreespacePlannerNode::planTrajectory()
     prev_target_index_ = 0;
     target_index_ =
       getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_);
-
   } else {
-    RCLCPP_INFO(get_logger(), "Can't find goal...");
+    RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str());
     reset();
   }
 }

diff --git a/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -23,6 +23,7 @@
 
 #include <tf2/utils.h>
 
+#include <algorithm>
 #include <vector>
 
 namespace autoware::freespace_planning_algorithms
@@ -57,21 +58,30 @@ geometry_msgs::msg::Pose local2global(
 
 struct VehicleShape
 {
-  double length;     // X [m]
-  double width;      // Y [m]
+  double length;  // X [m]
+  double width;   // Y [m]
+  double base_length;
+  double max_steering;
   double base2back;  // base_link to rear [m]
 
   VehicleShape() = default;
 
-  VehicleShape(double length, double width, double base2back)
-  : length(length), width(width), base2back(base2back)
+  VehicleShape(
+    double length, double width, double base_length, double max_steering, double base2back)
+  : length(length),
+    width(width),
+    base_length(base_length),
+    max_steering(max_steering),
+    base2back(base2back)
   {
   }
 
   explicit VehicleShape(
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0)
   : length(vehicle_info.vehicle_length_m + margin),
     width(vehicle_info.vehicle_width_m + margin),
+    base_length(vehicle_info.wheel_base_m),
+    max_steering(vehicle_info.max_steer_angle_rad),
     base2back(vehicle_info.rear_overhang_m + margin / 2.0)
   {
   }
@@ -82,18 +92,16 @@ struct PlannerCommonParam
   // base configs
   double time_limit;  // planning time limit [msec]
 
-  // robot configs
-  double minimum_turning_radius;  // [m]
-  double maximum_turning_radius;  // [m]
-  int turning_radius_size;        // discretized turning radius table size [-]
-
   // search configs
   int theta_size;                  // discretized angle table size [-]
   double curve_weight;             // curve moving cost [-]
   double reverse_weight;           // backward moving cost [-]
+  double direction_change_weight;  // direction change cost [-]
   double lateral_goal_range;       // reaching threshold, lateral error [m]
   double longitudinal_goal_range;  // reaching threshold, longitudinal error [m]
   double angle_goal_range;         // reaching threshold, angle error [deg]
+  double max_turning_ratio;        // factor of max turning range to use [-]
+  int turning_steps;               // number of turning steps [-]
 
   // costmap configs
   int obstacle_threshold;  // obstacle threshold on grid [-]
@@ -120,6 +128,8 @@ class AbstractPlanningAlgorithm
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape)
   : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape)
   {
+    planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
+    collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
     is_collision_table_initialized = false;
   }
 
@@ -128,6 +138,8 @@ class AbstractPlanningAlgorithm
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0)
   : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin)
   {
+    planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
+    collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
   }
 
   virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
@@ -142,8 +154,11 @@ class AbstractPlanningAlgorithm
   void computeCollisionIndexes(
     int theta_index, std::vector<IndexXY> & indexes,
     std::vector<IndexXY> & vertex_indexes_2d) const;
+  bool detectBoundaryExit(const IndexXYT & base_index) const;
   bool detectCollision(const IndexXYT & base_index) const;
-  inline bool isOutOfRange(const IndexXYT & index) const
+
+  template <typename IndexType>
+  inline bool isOutOfRange(const IndexType & index) const
   {
     if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
       return true;
@@ -153,16 +168,41 @@ class AbstractPlanningAlgorithm
     }
     return false;
   }
-  inline bool isObs(const IndexXYT & index) const
+
+  template <typename IndexType>
+  inline bool isWithinMargin(const IndexType & index) const
+  {
+    if (
+      index.x < nb_of_margin_cells_ ||
+      static_cast<int>(costmap_.info.width) - index.x < nb_of_margin_cells_) {
+      return false;
+    }
+    if (
+      index.y < nb_of_margin_cells_ ||
+      static_cast<int>(costmap_.info.height) - index.y < nb_of_margin_cells_) {
+      return false;
+    }
+    return true;
+  }
+
+  template <typename IndexType>
+  inline bool isObs(const IndexType & index) const
   {
     // NOTE: Accessing by .at() instead makes 1.2 times slower here.
     // Also, boundary check is already done in isOutOfRange before calling this function.
     // So, basically .at() is not necessary.
-    return is_obstacle_table_[index.y][index.x];
+    return is_obstacle_table_[indexToId(index)];
+  }
+
+  // compute single dimensional grid cell index from 2 dimensional index
+  template <typename IndexType>
+  inline int indexToId(const IndexType & index) const
+  {
+    return index.y * costmap_.info.width + index.x;
   }
 
   PlannerCommonParam planner_common_param_;
-  const VehicleShape collision_vehicle_shape_;
+  VehicleShape collision_vehicle_shape_;
 
   // costmap as occupancy grid
   nav_msgs::msg::OccupancyGrid costmap_;
@@ -174,7 +214,7 @@ class AbstractPlanningAlgorithm
   std::vector<std::vector<IndexXY>> vertex_indexes_table_;
 
   // is_obstacle's table
-  std::vector<std::vector<bool>> is_obstacle_table_;
+  std::vector<bool> is_obstacle_table_;
 
   // pose in costmap frame
   geometry_msgs::msg::Pose start_pose_;
@@ -185,6 +225,8 @@ class AbstractPlanningAlgorithm
 
   // result path
   PlannerWaypoints waypoints_;
+
+  int nb_of_margin_cells_;
 };
 
 }  // namespace autoware::freespace_planning_algorithms