22 #include <navgraph/search_state.h>
52 const NavGraphNode & goal,
54 NavGraphSearchState * parent,
56 navgraph::EstimateFunction estimate_func,
57 navgraph::CostFunction cost_func,
59 : AStarState(cost_sofar, parent), estimate_func_(estimate_func), cost_func_(cost_func)
63 map_graph_ = map_graph;
65 total_estimated_cost = path_cost + estimate();
67 std::hash<std::string> h;
68 key_ = h(node_.name());
70 constraint_repo_ = constraint_repo;
80 const NavGraphNode & goal,
87 map_graph_ = map_graph;
89 estimate_func_ = straight_line_estimate;
90 cost_func_ = euclidean_cost;
92 total_estimated_cost = path_cost + estimate();
94 std::hash<std::string> h;
95 key_ = h(node_.name());
97 constraint_repo_ = constraint_repo;
118 navgraph::EstimateFunction estimate_func,
119 navgraph::CostFunction cost_func,
121 :
AStarState(0, NULL), estimate_func_(estimate_func), cost_func_(cost_func)
125 map_graph_ = map_graph;
129 std::hash<std::string> h;
130 key_ = h(node_.
name());
132 constraint_repo_ = constraint_repo;
152 return estimate_func_(node_, goal_);
158 return (node_.
name() == goal_.
name());
161 std::vector<AStarState *>
162 NavGraphSearchState::children()
164 std::vector<AStarState *> children;
169 for (
unsigned int i = 0; i < descendants.size(); ++i) {
173 if (constraint_repo_) {
174 if (constraint_repo_->
blocks(d)) {
176 }
else if (constraint_repo_->
blocks(node_, d)) {
182 float d_cost = cost_func_(node_, d);
184 if (constraint_repo_) {
185 float cost_factor = 0.;
187 d_cost *= cost_factor;