Planning: OpenSpace: grid search map key bugfix

modules/planning/conf/planner_open_space_config.pb.txt

@@ -13,7 +13,7 @@ warm_start_config {
   traj_gear_switch_penalty: 10.0
   traj_steer_penalty: 0.0
   traj_steer_change_penalty: 0.0
-  grid_a_star_xy_resolution: 1.0
+  grid_a_star_xy_resolution: 0.5
   node_radius: 0.5
 }
 dual_variable_warm_start_config {

modules/planning/open_space/coarse_trajectory_generator/grid_search.cc

@@ -105,11 +105,11 @@ bool GridSearch::GenerateAStarPath(
     const std::vector<std::vector<common::math::LineSegment2d>>&
         obstacles_linesegments_vec,
     GridAStartResult* result) {
-  std::priority_queue<std::pair<double, double>,
-                      std::vector<std::pair<double, double>>, cmp>
+  std::priority_queue<std::pair<std::string, double>,
+                      std::vector<std::pair<std::string, double>>, cmp>
       open_pq;
-  std::unordered_map<double, std::shared_ptr<Node2d>> open_set;
-  std::unordered_map<double, std::shared_ptr<Node2d>> close_set;
+  std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
+  std::unordered_map<std::string, std::shared_ptr<Node2d>> close_set;
   XYbounds_ = XYbounds;
   std::shared_ptr<Node2d> start_node =
       std::make_shared<Node2d>(sx, sy, xy_grid_resolution_, XYbounds_);
@@ -123,7 +123,7 @@ bool GridSearch::GenerateAStarPath(
   // Grid a star begins
   size_t explored_node_num = 0;
   while (!open_pq.empty()) {
-    double current_id = open_pq.top().first;
+    std::string current_id = open_pq.top().first;
     open_pq.pop();
     std::shared_ptr<Node2d> current_node = open_set[current_id];
     // Check destination
@@ -167,10 +167,10 @@ bool GridSearch::GenerateDpMap(
     const double& ex, const double& ey, const std::vector<double>& XYbounds,
     const std::vector<std::vector<common::math::LineSegment2d>>&
         obstacles_linesegments_vec) {
-  std::priority_queue<std::pair<double, double>,
-                      std::vector<std::pair<double, double>>, cmp>
+  std::priority_queue<std::pair<std::string, double>,
+                      std::vector<std::pair<std::string, double>>, cmp>
       open_pq;
-  std::unordered_map<double, std::shared_ptr<Node2d>> open_set;
+  std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
   dp_map_ = decltype(dp_map_)();
   XYbounds_ = XYbounds;
   // XYbounds with xmin, xmax, ymin, ymax
@@ -185,7 +185,7 @@ bool GridSearch::GenerateDpMap(
   // Grid a star begins
   size_t explored_node_num = 0;
   while (!open_pq.empty()) {
-    double current_id = open_pq.top().first;
+    std::string current_id = open_pq.top().first;
     open_pq.pop();
     std::shared_ptr<Node2d> current_node = open_set[current_id];
     dp_map_.insert(std::make_pair(current_node->GetIndex(), current_node));
@@ -217,7 +217,7 @@ bool GridSearch::GenerateDpMap(
 }
 
 double GridSearch::CheckDpMap(const double& sx, const double& sy) {
-  double index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
+  std::string index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
   if (dp_map_.find(index) != dp_map_.end()) {
     return dp_map_[index]->GetCost() * xy_grid_resolution_;
   } else {

modules/planning/open_space/coarse_trajectory_generator/grid_search.h

@@ -24,6 +24,7 @@
 #include <limits>
 #include <memory>
 #include <queue>
+#include <string>
 #include <unordered_map>
 #include <utility>
 #include <vector>
@@ -37,55 +38,55 @@ namespace planning {
 
 class Node2d {
  public:
-  Node2d(const double& x, const double& y, const double& xy_resolution,
+  Node2d(const double x, const double y, const double xy_resolution,
          const std::vector<double>& XYbounds) {
     // XYbounds with xmin, xmax, ymin, ymax
-    grid_x_ = std::round((x - XYbounds[0]) / xy_resolution);
-    grid_y_ = std::round((y - XYbounds[2]) / xy_resolution);
-    index_ = grid_x_ * (XYbounds[3] - XYbounds[2]) + grid_y_;
+    grid_x_ = static_cast<int>((x - XYbounds[0]) / xy_resolution);
+    grid_y_ = static_cast<int>((y - XYbounds[2]) / xy_resolution);
+    index_ = std::to_string(grid_x_) + "_" + std::to_string(grid_y_);
   }
-  Node2d(const double& grid_x, const double& grid_y,
+  Node2d(const int grid_x, const int grid_y,
          const std::vector<double>& XYbounds) {
     grid_x_ = grid_x;
     grid_y_ = grid_y;
-    index_ = grid_x_ * (XYbounds[3] - XYbounds[2]) + grid_y_;
+    index_ = std::to_string(grid_x_) + "_" + std::to_string(grid_y_);
   }
-  void SetPathCost(const double& path_cost) {
+  void SetPathCost(const double path_cost) {
     path_cost_ = path_cost;
     cost_ = path_cost_ + heuristic_;
   }
-  void SetHeuristic(const double& heuristic) {
+  void SetHeuristic(const double heuristic) {
     heuristic_ = heuristic;
     cost_ = path_cost_ + heuristic_;
   }
-  void SetCost(const double& cost) { cost_ = cost; }
+  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() { return cost_; }
-  double GetIndex() const { return index_; }
-  std::shared_ptr<Node2d> GetPreNode() { return pre_node_; }
-  static double CalcIndex(const double& x, const double& y,
-                          const double& xy_resolution,
-                          const std::vector<double>& XYbounds) {
+  double GetCost() const { return cost_; }
+  std::string GetIndex() const { return index_; }
+  std::shared_ptr<Node2d> GetPreNode() const { return pre_node_; }
+  static std::string CalcIndex(const double x, const double y,
+                               const double xy_resolution,
+                               const std::vector<double>& XYbounds) {
     // XYbounds with xmin, xmax, ymin, ymax
-    double grid_x = std::round((x - XYbounds[0]) / xy_resolution);
-    double grid_y = std::round((y - XYbounds[2]) / xy_resolution);
-    return grid_x * (XYbounds[3] - XYbounds[2]) + grid_y;
+    int grid_x = static_cast<int>((x - XYbounds[0]) / xy_resolution);
+    int grid_y = static_cast<int>((y - XYbounds[2]) / xy_resolution);
+    return std::to_string(grid_x) + "_" + std::to_string(grid_y);
   }
   bool operator==(const Node2d& right) const {
     return right.GetIndex() == index_;
   }
 
  private:
-  double grid_x_ = 0.0;
-  double grid_y_ = 0.0;
+  int grid_x_ = 0;
+  int grid_y_ = 0;
   double path_cost_ = 0.0;
   double heuristic_ = 0.0;
   double cost_ = 0.0;
-  double index_ = 0.0;
+  std::string index_;
   std::shared_ptr<Node2d> pre_node_ = nullptr;
 };
 
@@ -132,12 +133,12 @@ class GridSearch {
       obstacles_linesegments_vec_;
 
   struct cmp {
-    bool operator()(const std::pair<double, double>& left,
-                    const std::pair<double, double>& right) const {
+    bool operator()(const std::pair<std::string, double>& left,
+                    const std::pair<std::string, double>& right) const {
       return left.second >= right.second;
     }
   };
-  std::unordered_map<double, std::shared_ptr<Node2d>> dp_map_;
+  std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_;
 };
 }  // namespace planning
 }  // namespace apollo

modules/tools/open_space_visualization/auto_param_tuning.py

@@ -27,11 +27,11 @@ from modules.planning.proto import planner_open_space_config_pb2
 
 
 random.seed(99999)
-rand_num = 100
+rand_num = 1000
 original_file_path = "/apollo/modules/planning/conf/planner_open_space_config.pb.txt"
 optimal_file_path = "/apollo/modules/planning/conf/optimal_planner_open_space_config.pb.txt"
-# tunning_object = "coarse_trajectory"
-tunning_object = "smooth_trajectory"
+tunning_object = "coarse_trajectory"
+# tunning_object = "smooth_trajectory"
 
 
 def load_open_space_protobuf(filename):