Go to the documentation of this file.
17 #ifndef FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
18 #define FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
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>
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;
123 void setOccupancyGrid(
const nav_msgs::msg::OccupancyGrid & costmap)
override;
130 const geometry_msgs::msg::Pose & start_pose,
131 const geometry_msgs::msg::Pose & goal_pose)
override;
137 bool setGoalNode()
const;
138 double estimateCost(
const geometry_msgs::msg::Pose & pose)
const;
148 std::vector<std::vector<std::vector<AstarNode>>> nodes_;
149 std::priority_queue<AstarNode *, std::vector<AstarNode *>,
NodeComparison> openlist_;
156 #endif // FREESPACE_PLANNER__ASTAR_SEARCH_HPP_
double step
Definition: astar_search.hpp:79
NodeStatus
Definition: astar_search.hpp:38
NodeUpdate flipped() const
Definition: astar_search.hpp:90
Parameters defining common algorithm configuration.
Definition: base_planning_algorithm.hpp:94
bool use_back
Indicate if should search for solutions in backward direction.
Definition: astar_search.hpp:43
double y
Definition: astar_search.hpp:56
double distance_heuristic_weight
Distance weight for trajectory cost estimation.
Definition: astar_search.hpp:49
NodeUpdate rotated(const double theta) const
Definition: astar_search.hpp:82
Planning algorithm base class.
Definition: base_planning_algorithm.hpp:158
Definition: freespace_planner.launch.py:1
AstarNode * parent
Definition: astar_search.hpp:61
Definition: astar_search.hpp:40
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
double theta
Definition: astar_search.hpp:57
node
Definition: sys_info_node.py:23
Definition: astar_search.hpp:52
double x
Definition: astar_search.hpp:55
A* Hybrid algorithm implementation using ROS2 typical structures.
Definition: astar_search.hpp:110
double cost() const
Definition: astar_search.hpp:63
SearchStatus
Possible planning results.
Definition: base_planning_algorithm.hpp:141
bool is_back
Definition: astar_search.hpp:60
Definition: base_planning_algorithm.hpp:49
double gc
Definition: astar_search.hpp:58
bool use_reeds_shepp
Indicate if cost should be calculated with a use of Reeds-Shepp algorithm.
Definition: astar_search.hpp:47
Definition: astar_search.hpp:66
double hc
Definition: astar_search.hpp:59
bool is_back
Definition: astar_search.hpp:80
bool only_behind_solutions
Indicate if solutions should be exclusively behind the goal.
Definition: astar_search.hpp:45
double shift_y
Definition: astar_search.hpp:77
NodeUpdate reversed() const
Definition: astar_search.hpp:98
NodeStatus status
Definition: astar_search.hpp:54
std::vector< std::vector< NodeUpdate > > TransitionTable
Definition: astar_search.hpp:113
bool operator()(const AstarNode *lhs, const AstarNode *rhs)
Definition: astar_search.hpp:68
double shift_theta
Definition: astar_search.hpp:78
Definition: astar_search.hpp:74
double shift_x
Definition: astar_search.hpp:76