feat(freespace_planning_algorithms): implement option for backward se…

…arch from goal to start (autowarefoundation#8091) * 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]> * 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]> * 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]> * 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]> * 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]> * transform pose to local frame in getDistanceToObstacle funcion 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]> * 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 · Aug 22, 2024 · c83e3a1 · c83e3a1
1 parent a7db560
commit c83e3a1
Show file tree

Hide file tree

Showing 14 changed files with 87 additions and 28 deletions.
diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md
@@ -77,6 +77,7 @@ None
 
 | Parameter                   | Type   | Description                                             |
 | --------------------------- | ------ | ------------------------------------------------------- |
+| `search_method`             | string | method of searching, start to goal or vice versa        |
 | `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  |

diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
@@ -31,6 +31,7 @@
 
     # -- A* search Configurations --
     astar:
+      search_method: "forward"  # options: forward, backward
       only_behind_solutions: false
       use_back: true
       adapt_expansion_distance: true

diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json
@@ -120,6 +120,12 @@
         "astar": {
           "type": "object",
           "properties": {
+            "search_method": {
+              "type": "string",
+              "enum": ["forward", "backward"],
+              "default": "forward",
+              "description": "Search method to use, options: forward, backward."
+            },
             "only_behind_solutions": {
               "type": "boolean",
               "default": false,
@@ -130,6 +136,11 @@
               "default": true,
               "description": "Allow reverse motion in A* search."
             },
+            "adapt_expansion_distance": {
+              "type": "boolean",
+              "default": true,
+              "description": "Allow varying A* expansion distance based on space configuration."
+            },
             "expansion_distance": {
               "type": "number",
               "default": 0.5,
@@ -143,7 +154,12 @@
             "smoothness_weight": {
               "type": "number",
               "default": 0.5,
-              "description": "Weight for the smoothness heuristic in A* search."
+              "description": "Weight for the smoothness (change in curvature) in A* search."
+            },
+            "obstacle_distance_weight": {
+              "type": "number",
+              "default": 0.5,
+              "description": "Weight for distance to obstacle in A* search."
             }
           },
           "required": [

diff --git a/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/...planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -164,6 +164,7 @@ class AbstractPlanningAlgorithm
     const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 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;
 
   virtual ~AbstractPlanningAlgorithm() {}
 

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
@@ -41,6 +41,7 @@ enum class NodeStatus : uint8_t { None, Open, Closed };
 struct AstarParam
 {
   // base configs
+  std::string search_method;
   bool only_behind_solutions;  // solutions should be behind the goal
   bool use_back;               // backward search
   bool adapt_expansion_distance;
@@ -99,6 +100,7 @@ class AstarSearch : public AbstractPlanningAlgorithm
   : AstarSearch(
       planner_common_param, collision_vehicle_shape,
       AstarParam{
+        node.declare_parameter<std::string>("astar.search_method"),
         node.declare_parameter<bool>("astar.only_behind_solutions"),
         node.declare_parameter<bool>("astar.use_back"),
         node.declare_parameter<bool>("astar.adapt_expansion_distance"),
@@ -125,7 +127,7 @@ class AstarSearch : public AbstractPlanningAlgorithm
   void expandNodes(AstarNode & current_node, const bool is_back = false);
   void resetData();
   void setPath(const AstarNode & goal);
-  bool setStartNode();
+  void setStartNode();
   double estimateCost(const Pose & pose, const IndexXYT & index) const;
   bool isGoal(const AstarNode & node) const;
   Pose node2pose(const AstarNode & node) const;
@@ -156,6 +158,7 @@ class AstarSearch : public AbstractPlanningAlgorithm
   double avg_turning_radius_;
   double min_expansion_dist_;
   double max_expansion_dist_;
+  bool is_backward_search_;
 
   // 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

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
@@ -111,6 +111,7 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
   auto pyAstarParam =
     py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam", py::dynamic_attr())
       .def(py::init<>())
+      .def_readwrite("search_method", &freespace_planning_algorithms::AstarParam::search_method)
       .def_readwrite(
         "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
       .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)

diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
@@ -44,6 +44,7 @@
 
 # -- A* search Configurations --
 astar_param = fp.AstarParam()
+astar_param.search_method = "forward"
 astar_param.only_behind_solutions = False
 astar_param.use_back = True
 astar_param.adapt_expansion_distance = True

diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
@@ -138,6 +138,16 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost
     std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution);
 }
 
+double AbstractPlanningAlgorithm::getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const
+{
+  const auto local_pose = global2local(costmap_, pose);
+  const auto index = pose2index(costmap_, local_pose, planner_common_param_.theta_size);
+  if (indexToId(index) >= static_cast<int>(edt_map_.size())) {
+    return std::numeric_limits<double>::max();
+  }
+  return getObstacleEDT(index);
+}
+
 void AbstractPlanningAlgorithm::computeEDTMap()
 {
   const int height = costmap_.info.height;

diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
@@ -29,6 +29,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <algorithm>
 #include <vector>
 
 namespace autoware::freespace_planning_algorithms
@@ -83,6 +84,8 @@ AstarSearch::AstarSearch(
   avg_turning_radius_ =
     kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering);
 
+  is_backward_search_ = astar_param_.search_method == "backward";
+
   min_expansion_dist_ = astar_param_.expansion_distance;
   max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_;
 }
@@ -104,15 +107,15 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose)
   start_pose_ = global2local(costmap_, start_pose);
   goal_pose_ = global2local(costmap_, goal_pose);
 
-  if (detectCollision(goal_pose_)) {
-    throw std::logic_error("Invalid goal pose");
+  if (detectCollision(start_pose_) || detectCollision(goal_pose_)) {
+    throw std::logic_error("Invalid start or goal pose");
   }
 
+  if (is_backward_search_) std::swap(start_pose_, goal_pose_);
+
   setCollisionFreeDistanceMap();
 
-  if (!setStartNode()) {
-    throw std::logic_error("Invalid start pose");
-  }
+  setStartNode();
 
   if (!search()) {
     throw std::logic_error("HA* failed to find path to goal");
@@ -174,12 +177,9 @@ void AstarSearch::setCollisionFreeDistanceMap()
   }
 }
 
-bool AstarSearch::setStartNode()
+void AstarSearch::setStartNode()
 {
   const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size);
-
-  if (detectCollision(index)) return false;
-
   // Set start node
   AstarNode * start_node = &graph_[getKey(index)];
   start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false);
@@ -191,8 +191,6 @@ bool AstarSearch::setStartNode()
 
   // Push start node to openlist
   openlist_.push(start_node);
-
-  return true;
 }
 
 double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const
@@ -242,7 +240,8 @@ bool AstarSearch::search()
 void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back)
 {
   const auto current_pose = node2pose(current_node);
-  double distance = getExpansionDistance(current_node) * (is_back ? -1.0 : 1.0);
+  const double direction = (is_back == is_backward_search_) ? 1.0 : -1.0;
+  const double distance = getExpansionDistance(current_node) * direction;
   int steering_index = -1 * planner_common_param_.turning_steps;
   for (; steering_index <= planner_common_param_.turning_steps; ++steering_index) {
     // skip expansion back to parent
@@ -333,46 +332,65 @@ void AstarSearch::setPath(const AstarNode & goal_node)
   header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
   header.frame_id = costmap_.header.frame_id;
 
-  waypoints_.header = header;
-  waypoints_.waypoints.clear();
-
   // From the goal node to the start node
   const AstarNode * node = &goal_node;
 
+  std::vector<PlannerWaypoint> waypoints;
+
   geometry_msgs::msg::PoseStamped pose;
   pose.header = header;
 
-  const auto interpolate = [this, &pose](const AstarNode & node) {
+  const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) {
     if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return;
     const auto parent_pose = node2pose(*node.parent);
     const double distance_2d = calcDistance2d(node2pose(node), parent_pose);
     const int n = static_cast<int>(distance_2d / min_expansion_dist_);
     for (int i = 1; i < n; ++i) {
-      const double dist = ((distance_2d * i) / n) * (node.is_back ? -1.0 : 1.0);
+      const double dist =
+        ((distance_2d * i) / n) * (node.is_back == is_backward_search_ ? 1.0 : -1.0);
       const double steering = node.steering_index * steering_resolution_;
       const auto local_pose = kinematic_bicycle_model::getPose(
         parent_pose, collision_vehicle_shape_.base_length, steering, dist);
       pose.pose = local2global(costmap_, local_pose);
-      waypoints_.waypoints.push_back({pose, node.is_back});
+      waypoints.push_back({pose, node.is_back});
     }
   };
 
   // push astar nodes poses
   while (node != nullptr) {
     pose.pose = local2global(costmap_, node2pose(*node));
-    waypoints_.waypoints.push_back({pose, node->is_back});
+    waypoints.push_back({pose, node->is_back});
     interpolate(*node);
     // To the next node
     node = node->parent;
   }
 
-  // Reverse the vector to be start to goal order
-  std::reverse(waypoints_.waypoints.begin(), waypoints_.waypoints.end());
+  if (waypoints.empty()) return;
 
-  // Update first point direction
-  if (waypoints_.waypoints.size() > 1) {
-    waypoints_.waypoints.at(0).is_back = waypoints_.waypoints.at(1).is_back;
+  if (waypoints.size() > 1) waypoints.back().is_back = waypoints.rbegin()[1].is_back;
+
+  if (!is_backward_search_) {
+    // Reverse the vector to be start to goal order
+    std::reverse(waypoints.begin(), waypoints.end());
+  }
+
+  waypoints_.header = header;
+  waypoints_.waypoints.clear();
+
+  for (size_t i = 0; i < waypoints.size() - 1; ++i) {
+    const auto & current = waypoints[i];
+    const auto & next = waypoints[i + 1];
+
+    waypoints_.waypoints.push_back(current);
+
+    if (current.is_back != next.is_back) {
+      waypoints_.waypoints.push_back(
+        {is_backward_search_ ? next.pose : current.pose,
+         is_backward_search_ ? current.is_back : next.is_back});
+    }
   }
+
+  waypoints_.waypoints.push_back(waypoints.back());
 }
 
 bool AstarSearch::isGoal(const AstarNode & node) const

diff --git a/...ng/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/...ng/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
@@ -209,6 +209,7 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
   }
 
   // configure astar param
+  const std::string search_method = "forward";
   const bool only_behind_solutions = false;
   const bool use_back = true;
   const bool adapt_expansion_distance = true;
@@ -217,8 +218,8 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
   const double smoothness_weight = 0.5;
   const double obstacle_distance_weight = 1.7;
   const auto astar_param = fpa::AstarParam{
-    only_behind_solutions,     use_back,          adapt_expansion_distance, expansion_distance,
-    distance_heuristic_weight, smoothness_weight, obstacle_distance_weight};
+    search_method,      only_behind_solutions,     use_back,          adapt_expansion_distance,
+    expansion_distance, distance_heuristic_weight, smoothness_weight, obstacle_distance_weight};
 
   auto algo = std::make_unique<fpa::AstarSearch>(planner_common_param, vehicle_shape, astar_param);
   return algo;

diff --git a/...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -106,6 +106,7 @@
             obstacle_threshold: 30
           # -- A* search Configurations --
           astar:
+            search_method: "forward"  # options: forward, backward
             only_behind_solutions: false
             use_back: false
             distance_heuristic_weight: 1.0

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -235,6 +235,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
   //  freespace parking astar
   {
     const std::string ns = base_ns + "pull_over.freespace_parking.astar.";
+    p.astar_parameters.search_method = node->declare_parameter<std::string>(ns + "search_method");
     p.astar_parameters.only_behind_solutions =
       node->declare_parameter<bool>(ns + "only_behind_solutions");
     p.astar_parameters.use_back = node->declare_parameter<bool>(ns + "use_back");
@@ -623,6 +624,7 @@ void GoalPlannerModuleManager::updateModuleParams(
   //  freespace parking astar
   {
     const std::string ns = base_ns + "pull_over.freespace_parking.astar.";
+    updateParam<std::string>(parameters, ns + "search_method", p->astar_parameters.search_method);
     updateParam<bool>(
       parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions);
     updateParam<bool>(parameters, ns + "use_back", p->astar_parameters.use_back);

diff --git a/..._path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/..._path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -77,6 +77,7 @@
           obstacle_threshold: 30
         # -- A* search Configurations --
         astar:
+          search_method: "forward"  # options: forward, backward
           only_behind_solutions: false
           use_back: false
           distance_heuristic_weight: 1.0

diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
@@ -167,6 +167,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   //  freespace planner astar
   {
     const std::string ns = "start_planner.freespace_planner.astar.";
+    p.astar_parameters.search_method = node->declare_parameter<std::string>(ns + "search_method");
     p.astar_parameters.only_behind_solutions =
       node->declare_parameter<bool>(ns + "only_behind_solutions");
     p.astar_parameters.use_back = node->declare_parameter<bool>(ns + "use_back");
@@ -501,6 +502,7 @@ void StartPlannerModuleManager::updateModuleParams(
   {
     const std::string ns = "start_planner.freespace_planner.astar.";
 
+    updateParam<std::string>(parameters, ns + "search_method", p->astar_parameters.search_method);
     updateParam<bool>(parameters, ns + "use_back", p->astar_parameters.use_back);
     updateParam<bool>(
       parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions);