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;
79 NavGraphSearchState::NavGraphSearchState(
const NavGraphNode & node,
87 map_graph_ = map_graph;
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;
This is the abstract(!) class for an A* State.
float total_estimated_cost
Total estimated cost.
float path_cost
Cost of path leading to this search state.
Constraint repository to maintain blocks on nodes.
NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if any constraint in the repo increases the cost of the edge.
NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node)
Check if any constraint in the repo blocks the node.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
const std::string & name() const
Get name of node.
~NavGraphSearchState()
Destructor.
virtual bool is_goal()
Check, wether we reached a goal or not.
virtual float estimate()
Estimate the heuristic cost to the goal.
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
NavGraphSearchState(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal, fawkes::NavGraph *map_graph, fawkes::NavGraphConstraintRepo *constraint_repo=NULL)
Constructor.
fawkes::NavGraphNode & node()
Get graph node corresponding to this search state.
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two nodes.
NavGraphNode node(const std::string &name) const
Get a specified node.
Fawkes library namespace.