Planning: Create Node3d for Hybrid Astar

modules/planning/open_space/node3d.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * node3d.cc
+ */
+
+#include "modules/planning/open_space/node3d.h"
+
+namespace apollo {
+namespace planning {
+
+Node3d::Node3d(double x, double y, double phi, double x_grid, double y_grid,
+           double phi_grid) {
+  x_ = x;
+  y_ = y;
+  phi_ = phi;
+  x_grid_ = x_grid;
+  y_grid_ = y_grid;
+  phi_grid_ = phi_grid;
+}
+
+Box2d Node3d::GetBoundingBox(const common::VehicleParam& vehicle_param_) {
+  double ego_length = vehicle_config_.vehicle_param().length();
+  double ego_width = vehicle_config_.vehicle_param().width();
+  Box2d ego_box({x_, y_}, phi_, ego_length, ego_width);
+  double shift_distance =
+      ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
+  Vec2d shift_vec{shift_distance * std::cos(phi_),
+                  shift_distance * std::sin(phi_)};
+  ego_box.Shift(shift_vec);
+  return ego_box;
+}
+
+bool Node3d::operator==(const SmartPtr<Node3d> right) const {
+  return x_grid_ == right->GetGridX() && y_grid_ == right->GetGridY() &&
+         phi_grid_ == right->GetGridPhi();
+}
+
+int Node3d::GetIndex() {
+  int index = 
+}
+}
+}
\ No newline at end of file

modules/planning/open_space/node3d.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * node3d.h
+ */
+
+#ifndef MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
+#define MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
+
+#include <memory>
+#include <vector>
+
+#include "modules/common/math/box2d.h"
+#include "modules/planning/common/obstacle.h"
+#include "modules/planning/constraint_checker/collision_checker.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::math::Box2d;
+
+class Node3d {
+ public:
+  explicit Node3d(double x, double y, double phi, double x_grid, double y_grid,
+                double phi_grid);
+  virtual ~Node3d() = default;
+  Box2d GetBoundingBox(const common::VehicleParam& vehicle_param_);
+  double GetCost() { return current_cost_ + heuristic_cost_; };
+  std::size_t GetGridX() { return x_grid_; };
+  std::size_t GetGridY() { return y_grid_; };
+  std::size_t GetGridPhi() { return phi_grid_; };
+  double GetX() { return x_; };
+  double GetY() { return y_; };
+  double GetPhi() { return phi_; };
+  bool operator==(const SmartPtr<Node3d> right) const;
+  int GetIndex();
+
+ private:
+  double x_ = 0.0;
+  double y_ = 0.0;
+  double phi_ = 0.0;
+  std::size_t x_grid_ = 0;
+  std::size_t y_grid_ = 0;
+  std::size_t phi_grid_ = 0;
+  double current_cost_ = 0.0;
+  double heuristic_cost_ = 0.0;
+  SmartPtr<Node3d> pre_node = nullptr;
+  double steering_ = 0.0;
+  // true for moving forward and false for moving backward
+  bool direction_ = true;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
\ No newline at end of file

modules/planning/planner/open_space/hybrid_a_star.cc

@@ -23,52 +23,23 @@
 namespace apollo {
 namespace planning {
 
-Node::Node(double x, double y, double phi, double x_grid, double y_grid,
-           double phi_grid) {
-  x_ = x;
-  y_ = y;
-  phi_ = phi;
-  x_grid_ = x_grid;
-  y_grid_ = y_grid;
-  phi_grid_ = phi_grid;
-}
-
-Box2d Node::GetBoundingBox(const common::VehicleParam& vehicle_param_) {
-  double ego_length = vehicle_config_.vehicle_param().length();
-  double ego_width = vehicle_config_.vehicle_param().width();
-  Box2d ego_box({x_, y_}, phi_, ego_length, ego_width);
-  double shift_distance =
-      ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
-  Vec2d shift_vec{shift_distance * std::cos(phi_),
-                  shift_distance * std::sin(phi_)};
-  ego_box.Shift(shift_vec);
-  return ego_box;
-}
-
-bool Node::operator==(const Node* right) const {
-  return x_grid_ == right->GetGridX() && y_grid_ == right->GetGridY() &&
-         phi_grid_ == right->GetGridPhi();
-}
-
 HybridAStar::HybridAStar(double sx, double sy, double sphi, double ex,
                          double ey, double ephi,
                          std::vector<const Obstacle*> obstacles,
                          double xy_grid_resolution,
                          double phi_grid_resolution) {
-  start_node_.reset(new Node(sx, sy, sphi, sx / xy_grid_resolution,
+  start_node_.reset(new Node3d(sx, sy, sphi, sx / xy_grid_resolution,
                              sy / xy_grid_resolution,
                              sphi / phi_grid_resolution));
-  end_node_.reset(new Node(ex, ey, ephi, ex / xy_grid_resolution,
+  end_node_.reset(new Node3d(ex, ey, ephi, ex / xy_grid_resolution,
                            ey / xy_grid_resolution,
                            ephi / phi_grid_resolution));
   obstacles_ = obstacles;
   xy_grid_resolution_ = xy_grid_resolution;
   phi_grid_resolution_ = phi_grid_resolution;
-};
-
-Status HybridAStar::Plan(){
+}
 
-};
+Status HybridAStar::Plan() {}
 
 }  // namespace planning
 }  // namespace apollo
\ No newline at end of file

modules/planning/planner/open_space/hybrid_a_star.h

@@ -24,52 +24,32 @@
 #include <map>
 #include <queue>
 #include <vector>
+#include <memory>
 
 #include "modules/common/log.h"
-#include "modules/common/math/box2d.h"
 #include "modules/planning/common/obstacle.h"
 #include "modules/planning/common/planning_gflags.h"
-#include "modules/planning/constraint_checker/collision_checker.h"
 
 namespace apollo {
 namespace planning {
 
 using apollo::common::Status;
-using apollo::common::math::Box2d;
-
-class Node {
- public:
-  explicit Node(double x, double y, double phi, double x_grid, double y_grid,
-                double phi_grid);
-  virtual ~Node() = default;
-  Box2d GetBoundingBox(const common::VehicleParam& vehicle_param_);
-  double GetCost() { return current_cost_ + heuristic_cost_; };
-  std::size_t GetGridX() { return x_grid_; };
-  std::size_t GetGridY() { return y_grid_; };
-  std::size_t GetGridPhi() { return phi_grid_; };
-  double GetX() { return x_; };
-  double GetY() { return y_; };
-  double GetPhi() { return phi_; };
-  bool operator==(const Node* right) const;
-
- private:
-  double x_ = 0.0;
-  double y_ = 0.0;
-  double phi_ = 0.0;
-  std::size_t x_grid_ = 0;
-  std::size_t y_grid_ = 0;
-  std::size_t phi_grid_ = 0;
-  double current_cost_ = 0.0;
-  double heuristic_cost_ = 0.0;
-  Node* pre_node = nullptr;
-  double steering_ = 0.0;
-  // true for moving forward and false for moving backward
-  bool direction_ = true;
-};
 
+struct OpenSpaceConf {
+  // for Hybrid A Star Warm Start
+  double xy_grid_resolution;
+  double phi_grid_resolution;
+  double obstacle_grid_resolution;
+  double max_x;
+  double max_y;
+  double max_phi;
+  double min_x;
+  double min_y;
+  double min_phi;
+}
 class HybridAStar {
  public:
-  explicit HybridAStar(Node start_node, Node end_node,
+  explicit HybridAStar(Node3d start_node, Node3d end_node,
                        std::vector<const Obstacle*> obstacles,
                        double xy_grid_resolution, double phi_grid_resolution,
                        double obstacle_grid_resolution);
@@ -91,14 +71,14 @@ class HybridAStar {
   double xy_grid_resolution_;
   double phi_grid_resolution_;
   std::vector<const Obstacle*> obstacles_;
-  std::unique_ptr<Node> start_node_;
-  std::unique_ptr<Node> end_node_;
-  auto cmp = [](Node* left, Node* right) {
+  std::unique_ptr<Node3d> start_node_;
+  std::unique_ptr<Node3d> end_node_;
+  auto cmp = [](SmartPtr<Node3d> left, SmartPtr<Node3d> right) {
     return left->GetCost() <= right->GetCost();
   };
-  std::priority_queue<Node*, std::vector<Node*>, decltype(cmp)> open_pq_(cmp);
-  std::map<double, Node*> open_set_;
-  std::map<double, Node*> close_set_;
+  std::priority_queue<SmartPtr<Node3d>, std::vector<SmartPtr<Node3d>>, decltype(cmp)> open_pq_(cmp);
+  std::map<double, SmartPtr<Node3d>> open_set_;
+  std::map<double, SmartPtr<Node3d>> close_set_;
 };
 
 }  // namespace planning