Planning: fix collision check in grid_search (#11524)

modules/planning/open_space/coarse_trajectory_generator/grid_search.cc

@@ -47,8 +47,7 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
   for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
     for (const common::math::LineSegment2d& linesegment :
          obstacle_linesegments) {
-      if (linesegment.DistanceTo({node->GetGridX(), node->GetGridY()}) <
-          node_radius_) {
+      if (linesegment.DistanceTo({node->GetX(), node->GetY()}) < node_radius_) {
         return false;
       }
     }
@@ -58,34 +57,42 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
 
 std::vector<std::shared_ptr<Node2d>> GridSearch::GenerateNextNodes(
     std::shared_ptr<Node2d> current_node) {
-  double current_node_x = current_node->GetGridX();
-  double current_node_y = current_node->GetGridY();
+  double current_node_x = current_node->GetX();
+  double current_node_y = current_node->GetY();
   double current_node_path_cost = current_node->GetPathCost();
   double diagonal_distance = std::sqrt(2.0);
   std::vector<std::shared_ptr<Node2d>> next_nodes;
-  std::shared_ptr<Node2d> up =
-      std::make_shared<Node2d>(current_node_x, current_node_y + 1.0, XYbounds_);
+  std::shared_ptr<Node2d> up = std::make_shared<Node2d>(
+      current_node_x, current_node_y + xy_grid_resolution_, xy_grid_resolution_,
+      XYbounds_);
   up->SetPathCost(current_node_path_cost + 1.0);
   std::shared_ptr<Node2d> up_right = std::make_shared<Node2d>(
-      current_node_x + 1.0, current_node_y + 1.0, XYbounds_);
+      current_node_x + xy_grid_resolution_,
+      current_node_y + xy_grid_resolution_, xy_grid_resolution_, XYbounds_);
   up_right->SetPathCost(current_node_path_cost + diagonal_distance);
   std::shared_ptr<Node2d> right =
-      std::make_shared<Node2d>(current_node_x + 1.0, current_node_y, XYbounds_);
+      std::make_shared<Node2d>(current_node_x + xy_grid_resolution_,
+                               current_node_y, xy_grid_resolution_, XYbounds_);
   right->SetPathCost(current_node_path_cost + 1.0);
   std::shared_ptr<Node2d> down_right = std::make_shared<Node2d>(
-      current_node_x + 1.0, current_node_y - 1.0, XYbounds_);
+      current_node_x + xy_grid_resolution_,
+      current_node_y - xy_grid_resolution_, xy_grid_resolution_, XYbounds_);
   down_right->SetPathCost(current_node_path_cost + diagonal_distance);
-  std::shared_ptr<Node2d> down =
-      std::make_shared<Node2d>(current_node_x, current_node_y - 1.0, XYbounds_);
+  std::shared_ptr<Node2d> down = std::make_shared<Node2d>(
+      current_node_x, current_node_y - xy_grid_resolution_, xy_grid_resolution_,
+      XYbounds_);
   down->SetPathCost(current_node_path_cost + 1.0);
   std::shared_ptr<Node2d> down_left = std::make_shared<Node2d>(
-      current_node_x - 1.0, current_node_y - 1.0, XYbounds_);
+      current_node_x - xy_grid_resolution_,
+      current_node_y - xy_grid_resolution_, xy_grid_resolution_, XYbounds_);
   down_left->SetPathCost(current_node_path_cost + diagonal_distance);
   std::shared_ptr<Node2d> left =
-      std::make_shared<Node2d>(current_node_x - 1.0, current_node_y, XYbounds_);
+      std::make_shared<Node2d>(current_node_x - xy_grid_resolution_,
+                               current_node_y, xy_grid_resolution_, XYbounds_);
   left->SetPathCost(current_node_path_cost + 1.0);
   std::shared_ptr<Node2d> up_left = std::make_shared<Node2d>(
-      current_node_x - 1.0, current_node_y + 1.0, XYbounds_);
+      current_node_x - xy_grid_resolution_,
+      current_node_y + xy_grid_resolution_, xy_grid_resolution_, XYbounds_);
   up_left->SetPathCost(current_node_path_cost + diagonal_distance);
 
   next_nodes.emplace_back(up);

modules/planning/open_space/coarse_trajectory_generator/grid_search.h

@@ -41,16 +41,12 @@ class Node2d {
   Node2d(const double x, const double y, const double xy_resolution,
          const std::vector<double>& XYbounds) {
     // XYbounds with xmin, xmax, ymin, ymax
+    x_ = x;
+    y_ = y;
     grid_x_ = static_cast<int>((x - XYbounds[0]) / xy_resolution);
     grid_y_ = static_cast<int>((y - XYbounds[2]) / xy_resolution);
     index_ = ComputeStringIndex(grid_x_, 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_ = ComputeStringIndex(grid_x_, grid_y_);
-  }
   void SetPathCost(const double path_cost) {
     path_cost_ = path_cost;
     cost_ = path_cost_ + heuristic_;
@@ -61,6 +57,8 @@ class Node2d {
   }
   void SetCost(const double cost) { cost_ = cost; }
   void SetPreNode(std::shared_ptr<Node2d> pre_node) { pre_node_ = pre_node; }
+  double GetX() const { return x_; }
+  double GetY() const { return y_; }
   double GetGridX() const { return grid_x_; }
   double GetGridY() const { return grid_y_; }
   double GetPathCost() const { return path_cost_; }
@@ -86,6 +84,8 @@ class Node2d {
   }
 
  private:
+  double x_ = 0.0;
+  double y_ = 0.0;
   int grid_x_ = 0;
   int grid_y_ = 0;
   double path_cost_ = 0.0;