22#ifndef _LIBS_NAVGRAPH_SEARCH_STATE_H_
23#define _LIBS_NAVGRAPH_SEARCH_STATE_H_
25#include <core/utils/lockptr.h>
26#include <navgraph/constraints/constraint_repo.h>
27#include <navgraph/navgraph.h>
28#include <utils/search/astar_state.h>
46 navgraph::EstimateFunction estimate_func,
71 return sqrtf(powf(to.
x() - from.
x(), 2) + powf(to.
y() - from.
y(), 2));
82 return sqrtf(powf(goal.
x() -
node.
x(), 2) + powf(goal.
y() -
node.
y(), 2));
91 navgraph::EstimateFunction estimate_func,
92 navgraph::CostFunction cost_func,
96 std::vector<AStarState *> children();
110 navgraph::EstimateFunction estimate_func_;
111 navgraph::CostFunction cost_func_;
This is the abstract(!) class for an A* State.
Constraint repository to maintain blocks on nodes.
float x() const
Get X coordinate in global frame.
float y() const
Get Y coordinate in global frame.
Graph-based path planner A* search state.
~NavGraphSearchState()
Destructor.
virtual size_t key()
Generates a unique key for this state.
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.
Fawkes library namespace.