From c10d88ce2020a9b6b8b4ebb543de5867ac37ace7 Mon Sep 17 00:00:00 2001 From: JasonZhou404 Date: Fri, 14 Sep 2018 10:40:55 -0700 Subject: [PATCH] Planning: Create Node3d for Hybrid Astar --- modules/planning/open_space/node3d.cc | 57 +++++++++++++++ modules/planning/open_space/node3d.h | 70 +++++++++++++++++++ .../planner/open_space/hybrid_a_star.cc | 37 ++-------- .../planner/open_space/hybrid_a_star.h | 60 ++++++---------- 4 files changed, 151 insertions(+), 73 deletions(-) create mode 100644 modules/planning/open_space/node3d.cc create mode 100644 modules/planning/open_space/node3d.h diff --git a/modules/planning/open_space/node3d.cc b/modules/planning/open_space/node3d.cc new file mode 100644 index 0000000000..8ed38ace77 --- /dev/null +++ b/modules/planning/open_space/node3d.cc @@ -0,0 +1,57 @@ +/****************************************************************************** + * 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 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 diff --git a/modules/planning/open_space/node3d.h b/modules/planning/open_space/node3d.h new file mode 100644 index 0000000000..d48ab18786 --- /dev/null +++ b/modules/planning/open_space/node3d.h @@ -0,0 +1,70 @@ +/****************************************************************************** + * 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 +#include + +#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 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 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 diff --git a/modules/planning/planner/open_space/hybrid_a_star.cc b/modules/planning/planner/open_space/hybrid_a_star.cc index 209e59e983..9f5a2c2c23 100644 --- a/modules/planning/planner/open_space/hybrid_a_star.cc +++ b/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 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 diff --git a/modules/planning/planner/open_space/hybrid_a_star.h b/modules/planning/planner/open_space/hybrid_a_star.h index 5b2983df03..637138d946 100644 --- a/modules/planning/planner/open_space/hybrid_a_star.h +++ b/modules/planning/planner/open_space/hybrid_a_star.h @@ -24,52 +24,32 @@ #include #include #include +#include #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 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 obstacles_; - std::unique_ptr start_node_; - std::unique_ptr end_node_; - auto cmp = [](Node* left, Node* right) { + std::unique_ptr start_node_; + std::unique_ptr end_node_; + auto cmp = [](SmartPtr left, SmartPtr right) { return left->GetCost() <= right->GetCost(); }; - std::priority_queue, decltype(cmp)> open_pq_(cmp); - std::map open_set_; - std::map close_set_; + std::priority_queue, std::vector>, decltype(cmp)> open_pq_(cmp); + std::map> open_set_; + std::map> close_set_; }; } // namespace planning -- GitLab