Planning: OpenSpace: integrate dp heu into hybrid a*

modules/planning/conf/planner_open_space_config.pb.txt

@@ -12,7 +12,7 @@ roi_config : {
 warm_start_config : {
   xy_grid_resolution : 0.3
   phi_grid_resolution : 0.1
-  next_node_num : 20
+  next_node_num : 10
   step_size : 0.5
   traj_forward_penalty : 1.0
   traj_back_penalty : 1.0

modules/planning/open_space/coarse_trajectory_generator/grid_search.cc

@@ -37,6 +37,12 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
   if (obstacles_linesegments_vec_.size() == 0) {
     return true;
   }
+  double node_grid_x = node->GetGridX();
+  double node_grid_y = node->GetGridY();
+  if (node_grid_x > max_grid_x_ || node_grid_x < 0 ||
+      node_grid_y > max_grid_y_ || node_grid_y < 0) {
+    return false;
+  }
   for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
     for (const common::math::LineSegment2d& linesegment :
          obstacle_linesegments) {
@@ -97,7 +103,7 @@ bool GridSearch::GenerateAStarPath(
     const std::vector<double>& XYbounds,
     const std::vector<std::vector<common::math::LineSegment2d>>&
         obstacles_linesegments_vec,
-    double* optimal_path_cost) {
+    GridAStartResult* result) {
   std::priority_queue<std::pair<double, double>,
                       std::vector<std::pair<double, double>>, cmp>
       open_pq;
@@ -151,7 +157,7 @@ bool GridSearch::GenerateAStarPath(
     AERROR << "Grid A searching return null ptr(open_set ran out)";
     return false;
   }
-  *optimal_path_cost = final_node_->GetPathCost() * xy_grid_resolution_;
+  LoadGridAStarResult(result);
   ADEBUG << "explored node num is " << explored_node_num;
   return true;
 }
@@ -166,6 +172,9 @@ bool GridSearch::GenerateDpMap(
   std::unordered_map<double, std::shared_ptr<Node2d>> open_set;
   dp_map_ = decltype(dp_map_)();
   XYbounds_ = XYbounds;
+  // XYbounds with xmin, xmax, ymin, ymax
+  max_grid_y_ = std::round((XYbounds_[3] - XYbounds_[2]) / xy_grid_resolution_);
+  max_grid_x_ = std::round((XYbounds_[1] - XYbounds_[0]) / xy_grid_resolution_);
   std::shared_ptr<Node2d> end_node =
       std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);
   obstacles_linesegments_vec_ = obstacles_linesegments_vec;
@@ -208,8 +217,29 @@ bool GridSearch::GenerateDpMap(
 
 double GridSearch::CheckDpMap(const double& sx, const double& sy) {
   double index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
-  return dp_map_[index]->GetCost();
+  if (dp_map_.find(index) != dp_map_.end()) {
+    return dp_map_[index]->GetCost() * xy_grid_resolution_;
+  } else {
+    return std::numeric_limits<double>::infinity();
+  }
 }
 
+void GridSearch::LoadGridAStarResult(GridAStartResult* result) {
+  (*result).path_cost = final_node_->GetPathCost() * xy_grid_resolution_;
+  std::shared_ptr<Node2d> current_node = final_node_;
+  std::vector<double> grid_a_x;
+  std::vector<double> grid_a_y;
+  while (current_node->GetPreNode() != nullptr) {
+    grid_a_x.push_back(current_node->GetGridX() * xy_grid_resolution_ +
+                       XYbounds_[0]);
+    grid_a_y.push_back(current_node->GetGridY() * xy_grid_resolution_ +
+                       XYbounds_[2]);
+    current_node = current_node->GetPreNode();
+  }
+  std::reverse(grid_a_x.begin(), grid_a_x.end());
+  std::reverse(grid_a_y.begin(), grid_a_y.end());
+  (*result).x = std::move(grid_a_x);
+  (*result).y = std::move(grid_a_y);
+}
 }  // namespace planning
 }  // namespace apollo

modules/planning/open_space/coarse_trajectory_generator/grid_search.h

@@ -25,6 +25,8 @@
 #include <unordered_map>
 #include <utility>
 #include <vector>
+#include <algorithm>
+#include <limits>
 
 #include "cyber/common/log.h"
 #include "modules/common/math/line_segment2d.h"
@@ -48,18 +50,21 @@ class Node2d {
     grid_y_ = grid_y;
     index_ = grid_x_ * (XYbounds[3] - XYbounds[2]) + grid_y_;
   }
-  void SetPathCost(const double& path_cost) { path_cost_ = path_cost; }
-  void SetHeuristic(const double& heuristic) { heuristic_ = heuristic; }
+  void SetPathCost(const double& path_cost) {
+    path_cost_ = path_cost;
+    cost_ = path_cost_ + heuristic_;
+  }
+  void SetHeuristic(const double& heuristic) {
+    heuristic_ = heuristic;
+    cost_ = path_cost_ + heuristic_;
+  }
   void SetCost(const double& cost) { cost_ = cost; }
   void SetPreNode(std::shared_ptr<Node2d> pre_node) { pre_node_ = pre_node; }
   double GetGridX() const { return grid_x_; }
   double GetGridY() const { return grid_y_; }
   double GetPathCost() const { return path_cost_; }
   double GetHeuCost() const { return heuristic_; }
-  double GetCost() {
-    cost_ = path_cost_ + heuristic_;
-    return cost_;
-  }
+  double GetCost() { return cost_; }
   double GetIndex() const { return index_; }
   std::shared_ptr<Node2d> GetPreNode() { return pre_node_; }
   static double CalcIndex(const double& x, const double& y,
@@ -80,10 +85,16 @@ class Node2d {
   double path_cost_ = 0.0;
   double heuristic_ = 0.0;
   double cost_ = 0.0;
-  double index_ = 0;
+  double index_ = 0.0;
   std::shared_ptr<Node2d> pre_node_ = nullptr;
 };
 
+struct GridAStartResult {
+  std::vector<double> x;
+  std::vector<double> y;
+  double path_cost = 0.0;
+};
+
 class GridSearch {
  public:
   explicit GridSearch(const PlannerOpenSpaceConfig& open_space_conf);
@@ -93,7 +104,7 @@ class GridSearch {
       const std::vector<double>& XYbounds,
       const std::vector<std::vector<common::math::LineSegment2d>>&
           obstacles_linesegments_vec,
-      double* optimal_path_cost);
+      GridAStartResult* result);
   bool GenerateDpMap(
       const double& ex, const double& ey, const std::vector<double>& XYbounds,
       const std::vector<std::vector<common::math::LineSegment2d>>&
@@ -106,11 +117,14 @@ class GridSearch {
   std::vector<std::shared_ptr<Node2d>> GenerateNextNodes(
       std::shared_ptr<Node2d> node);
   bool CheckConstraints(std::shared_ptr<Node2d> node);
+  void LoadGridAStarResult(GridAStartResult* result);
 
  private:
   double xy_grid_resolution_ = 0.0;
   double node_radius_ = 0.0;
   std::vector<double> XYbounds_;
+  double max_grid_x_ = 0.0;
+  double max_grid_y_ = 0.0;
   std::shared_ptr<Node2d> start_node_;
   std::shared_ptr<Node2d> end_node_;
   std::shared_ptr<Node2d> final_node_;

modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc

@@ -102,7 +102,7 @@ bool HybridAStar::ValidityCheck(std::shared_ptr<Node3d> node) {
     }
   }
   return true;
-}  // namespace planning
+}
 
 std::shared_ptr<Node3d> HybridAStar::LoadRSPinCS(
     const std::shared_ptr<ReedSheppPath> reeds_shepp_to_end,
@@ -210,13 +210,8 @@ double HybridAStar::TrajCost(std::shared_ptr<Node3d> current_node,
 
 bool HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node,
                                         double* optimal_path_cost) {
-  if (!grid_a_star_heuristic_generator_->GenerateAStarPath(
-          next_node->GetX(), next_node->GetY(), end_node_->GetX(),
-          end_node_->GetY(), XYbounds_, obstacles_linesegments_vec_,
-          optimal_path_cost)) {
-    AERROR << "grid_a_star_heuristic failed";
-    return false;
-  }
+  *optimal_path_cost = grid_a_star_heuristic_generator_->CheckDpMap(
+      next_node->GetX(), next_node->GetY());
   return true;
 }
 
@@ -345,6 +340,10 @@ bool HybridAStar::Plan(
     AERROR << "end_node in collision with obstacles";
     return false;
   }
+  double map_time = Clock::NowInSeconds();
+  grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
+                                                  obstacles_linesegments_vec_);
+  AINFO << "map time " << Clock::NowInSeconds() - map_time;
   // load open set, pq
   open_set_.insert(std::make_pair(start_node_->GetIndex(), start_node_));
   open_pq_.push(
@@ -353,6 +352,7 @@ bool HybridAStar::Plan(
   // Hybrid A* begins
   size_t explored_node_num = 0;
   double heuristic_time = 0.0;
+  double rs_time = 0.0;
   double start_time = 0.0;
   double end_time = 0.0;
   while (!open_pq_.empty()) {
@@ -362,9 +362,12 @@ bool HybridAStar::Plan(
     std::shared_ptr<Node3d> current_node = open_set_[current_id];
     // check if a analystic curve could be connected from current configuration
     // to the end configuration without collision. if so, search ends.
+    start_time = Clock::NowInSeconds();
     if (AnalyticExpansion(current_node)) {
       break;
     }
+    end_time = Clock::NowInSeconds();
+    rs_time += end_time - start_time;
     close_set_.insert(std::make_pair(current_node->GetIndex(), current_node));
     for (size_t i = 0; i < next_node_num_; ++i) {
       std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
@@ -406,6 +409,7 @@ bool HybridAStar::Plan(
   }
   AINFO << "explored node num is " << explored_node_num;
   AINFO << "heuristic time is " << heuristic_time;
+  AINFO << "rs time is "<< rs_time;
   return true;
 }
 }  // namespace planning

modules/planning/testdata/conf/open_space_standard_parking_lot.pb.txt

@@ -6,23 +6,21 @@ is_near_destination_threshold : 0.05
 roi_config : {
   roi_longitudinal_range : 15.0
   parking_start_range : 12.0
+  parking_inwards : false
 }
 
 warm_start_config : {
   xy_grid_resolution : 0.3
   phi_grid_resolution : 0.1
-  next_node_num : 20
+  next_node_num : 10
   step_size : 0.5
   traj_forward_penalty : 1.0
   traj_back_penalty : 1.0
   traj_gear_switch_penalty : 10.0
   traj_steer_penalty : 0.0
   traj_steer_change_penalty : 0.0
-  heu_rs_forward_penalty : 1.0
-  heu_rs_back_penalty : 1.0
-  heu_rs_gear_switch_penalty : 10.0
-  heu_rs_steer_penalty : 1.0
-  heu_rs_steer_change_penalty : 1.0
+  grid_a_star_xy_resolution : 1.0
+  node_radius : 0.5
 }
 
 dual_variable_warm_start_config : {
@@ -76,4 +74,13 @@ distance_approach_config : {
     ipopt_alpha_for_y : "min"
     ipopt_recalc_y : "yes"
   }
+}
+
+trajectory_partition_config : {
+  interpolated_pieces_num : 50
+  initial_gear_check_horizon : 3
+  heading_searching_range : 0.3
+  gear_shift_period_duration : 2.0
+  gear_shift_max_t : 3.0
+  gear_shift_unit_t : 0.02
 }
\ No newline at end of file