Autoware.Auto
astar_search.hpp
Go to the documentation of this file.
1 // Copyright 2021 The Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Robotec.AI sp. z o.o.
16 
17 #ifndef FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
18 #define FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
19 
23 
24 #include <geometry_msgs/msg/pose_array.hpp>
25 #include <nav_msgs/msg/occupancy_grid.hpp>
26 #include <nav_msgs/msg/path.hpp>
27 #include <std_msgs/msg/header.hpp>
28 
29 #include <vector>
30 #include <queue>
31 
32 namespace autoware
33 {
34 namespace planning
35 {
37 {
38 enum class NodeStatus : uint8_t { None, Open, Closed, Obstacle };
39 
40 struct FREESPACE_PLANNER_PUBLIC AstarParam
41 {
43  bool use_back;
50 };
51 
52 struct AstarNode
53 {
55  double x; // x
56  double y; // y
57  double theta; // theta
58  double gc = 0; // actual cost
59  double hc = 0; // heuristic cost
60  bool is_back; // true if the current direction of the vehicle is back
61  AstarNode * parent = nullptr; // parent node
62 
63  double cost() const {return gc + hc;}
64 };
65 
67 {
68  bool operator()(const AstarNode * lhs, const AstarNode * rhs)
69  {
70  return lhs->cost() > rhs->cost();
71  }
72 };
73 
74 struct NodeUpdate
75 {
76  double shift_x;
77  double shift_y;
78  double shift_theta;
79  double step;
80  bool is_back;
81 
82  NodeUpdate rotated(const double theta) const
83  {
84  NodeUpdate result = *this;
85  result.shift_x = std::cos(theta) * this->shift_x - std::sin(theta) * this->shift_y;
86  result.shift_y = std::sin(theta) * this->shift_x + std::cos(theta) * this->shift_y;
87  return result;
88  }
89 
91  {
92  NodeUpdate result = *this;
93  result.shift_y = -result.shift_y;
94  result.shift_theta = -result.shift_theta;
95  return result;
96  }
97 
99  {
100  NodeUpdate result = *this;
101  result.shift_x = -result.shift_x;
102  result.shift_theta = -result.shift_theta;
103  result.is_back = !result.is_back;
104  return result;
105  }
106 };
107 
110 class FREESPACE_PLANNER_PUBLIC AstarSearch : public BasePlanningAlgorithm
111 {
112 public:
113  using TransitionTable = std::vector<std::vector<NodeUpdate>>;
114 
118  explicit AstarSearch(
119  const PlannerCommonParam & planner_common_param, const AstarParam & astar_param);
120 
123  void setOccupancyGrid(const nav_msgs::msg::OccupancyGrid & costmap) override;
124 
129  SearchStatus makePlan(
130  const geometry_msgs::msg::Pose & start_pose,
131  const geometry_msgs::msg::Pose & goal_pose) override;
132 
133 private:
134  SearchStatus search();
135  void setPath(const AstarNode & goal);
136  bool setStartNode();
137  bool setGoalNode() const;
138  double estimateCost(const geometry_msgs::msg::Pose & pose) const;
139  bool isGoal(const AstarNode & node) const;
140 
141  AstarNode * getNodeRef(const IndexXYT & index);
142 
143  // Algorithm specific param
144  AstarParam astar_param_;
145 
146  // hybrid astar variables
147  TransitionTable transition_table_;
148  std::vector<std::vector<std::vector<AstarNode>>> nodes_;
149  std::priority_queue<AstarNode *, std::vector<AstarNode *>, NodeComparison> openlist_;
150 };
151 
152 } // namespace freespace_planner
153 } // namespace planning
154 } // namespace autoware
155 
156 #endif // FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
autoware::planning::freespace_planner::NodeUpdate::step
double step
Definition: astar_search.hpp:79
autoware::planning::freespace_planner::NodeStatus
NodeStatus
Definition: astar_search.hpp:38
autoware::planning::freespace_planner::NodeUpdate::flipped
NodeUpdate flipped() const
Definition: astar_search.hpp:90
autoware::planning::freespace_planner::NodeStatus::None
@ None
autoware::planning::freespace_planner::PlannerCommonParam
Parameters defining common algorithm configuration.
Definition: base_planning_algorithm.hpp:94
autoware::planning::freespace_planner::AstarParam::use_back
bool use_back
Indicate if should search for solutions in backward direction.
Definition: astar_search.hpp:43
autoware::planning::freespace_planner::AstarNode::y
double y
Definition: astar_search.hpp:56
autoware::planning::freespace_planner::AstarParam::distance_heuristic_weight
double distance_heuristic_weight
Distance weight for trajectory cost estimation.
Definition: astar_search.hpp:49
autoware::planning::freespace_planner::NodeUpdate::rotated
NodeUpdate rotated(const double theta) const
Definition: astar_search.hpp:82
autoware::planning::freespace_planner::BasePlanningAlgorithm
Planning algorithm base class.
Definition: base_planning_algorithm.hpp:158
base_planning_algorithm.hpp
freespace_planner
Definition: freespace_planner.launch.py:1
autoware::planning::freespace_planner::AstarNode::parent
AstarNode * parent
Definition: astar_search.hpp:61
autoware::planning::freespace_planner::AstarParam
Definition: astar_search.hpp:40
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::planning::freespace_planner::AstarNode::theta
double theta
Definition: astar_search.hpp:57
sys_info_node.node
node
Definition: sys_info_node.py:23
autoware::planning::freespace_planner::AstarNode
Definition: astar_search.hpp:52
autoware::planning::freespace_planner::AstarNode::x
double x
Definition: astar_search.hpp:55
autoware::planning::freespace_planner::AstarSearch
A* Hybrid algorithm implementation using ROS2 typical structures.
Definition: astar_search.hpp:110
autoware::planning::freespace_planner::AstarNode::cost
double cost() const
Definition: astar_search.hpp:63
autoware::planning::freespace_planner::SearchStatus
SearchStatus
Possible planning results.
Definition: base_planning_algorithm.hpp:141
autoware::planning::freespace_planner::AstarNode::is_back
bool is_back
Definition: astar_search.hpp:60
autoware::planning::freespace_planner::IndexXYT
Definition: base_planning_algorithm.hpp:49
autoware::planning::freespace_planner::AstarNode::gc
double gc
Definition: astar_search.hpp:58
autoware::planning::freespace_planner::AstarParam::use_reeds_shepp
bool use_reeds_shepp
Indicate if cost should be calculated with a use of Reeds-Shepp algorithm.
Definition: astar_search.hpp:47
autoware::planning::freespace_planner::NodeComparison
Definition: astar_search.hpp:66
autoware::planning::freespace_planner::NodeStatus::Closed
@ Closed
autoware::planning::freespace_planner::AstarNode::hc
double hc
Definition: astar_search.hpp:59
autoware::planning::freespace_planner::NodeUpdate::is_back
bool is_back
Definition: astar_search.hpp:80
autoware::planning::freespace_planner::AstarParam::only_behind_solutions
bool only_behind_solutions
Indicate if solutions should be exclusively behind the goal.
Definition: astar_search.hpp:45
autoware::planning::freespace_planner::NodeUpdate::shift_y
double shift_y
Definition: astar_search.hpp:77
autoware::planning::freespace_planner::NodeUpdate::reversed
NodeUpdate reversed() const
Definition: astar_search.hpp:98
autoware::planning::freespace_planner::AstarNode::status
NodeStatus status
Definition: astar_search.hpp:54
autoware::planning::freespace_planner::AstarSearch::TransitionTable
std::vector< std::vector< NodeUpdate > > TransitionTable
Definition: astar_search.hpp:113
autoware::planning::freespace_planner::NodeComparison::operator()
bool operator()(const AstarNode *lhs, const AstarNode *rhs)
Definition: astar_search.hpp:68
autoware::planning::freespace_planner::NodeUpdate::shift_theta
double shift_theta
Definition: astar_search.hpp:78
autoware::planning::freespace_planner::NodeUpdate
Definition: astar_search.hpp:74
visibility_control.hpp
autoware::planning::freespace_planner::NodeUpdate::shift_x
double shift_x
Definition: astar_search.hpp:76
autoware::planning::freespace_planner::NodeStatus::Open
@ Open
reeds_shepp.hpp
autoware::planning::freespace_planner::NodeStatus::Obstacle
@ Obstacle