22 #include <navgraph/search_state.h> 55 double cost_sofar, NavGraphSearchState *parent,
57 navgraph::EstimateFunction estimate_func,
58 navgraph::CostFunction cost_func,
60 : AStarState(cost_sofar, parent), estimate_func_(estimate_func), cost_func_(cost_func)
64 map_graph_ = map_graph;
66 total_estimated_cost = path_cost + estimate();
68 std::hash<std::string> h;
69 key_ = h(node_.name());
71 constraint_repo_ = constraint_repo;
88 map_graph_ = map_graph;
95 std::hash<std::string> h;
96 key_ = h(node_.
name());
98 constraint_repo_ = constraint_repo;
119 navgraph::EstimateFunction estimate_func,
120 navgraph::CostFunction cost_func,
122 :
AStarState(0, NULL), estimate_func_(estimate_func), cost_func_(cost_func)
126 map_graph_ = map_graph;
130 std::hash<std::string> h;
131 key_ = h(node_.
name());
133 constraint_repo_ = constraint_repo;
156 return estimate_func_(node_, goal_);
163 return (node_.
name() == goal_.
name());
167 std::vector<AStarState *>
168 NavGraphSearchState::children()
170 std::vector< AStarState * > children;
175 for (
unsigned int i = 0; i < descendants.size(); ++i) {
179 if (constraint_repo_) {
180 if (constraint_repo_->
blocks(d)) {
182 }
else if (constraint_repo_->
blocks(node_, d)) {
188 float d_cost = cost_func_(node_, d);
190 if (constraint_repo_) {
191 float cost_factor = 0.;
193 d_cost *= cost_factor;
198 map_graph_, estimate_func_, cost_func_,
This is the abstract(!) class for an A* State.
~NavGraphSearchState()
Destructor.
Fawkes library namespace.
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two 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.
virtual bool is_goal()
Check, wether we reached a goal or not.
float path_cost
Cost of path leading to this search state.
virtual float estimate()
Estimate the heuristic cost to the goal.
NavGraphSearchState(fawkes::NavGraphNode node, fawkes::NavGraphNode goal, fawkes::NavGraph *map_graph, fawkes::NavGraphConstraintRepo *constraint_repo=NULL)
Constructor.
Constraint repository to maintain blocks on nodes.
NavGraphNode node(const std::string &name) const
Get a specified node.
const std::string & name() const
Get name of node.
float total_estimated_cost
Total estimated cost.
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
fawkes::NavGraphNode & node()
Get graph node corresponding to this search state.
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node)
Check if any constraint in the repo blocks the node.
const NavGraphNode & goal() const
Get goal of path.