Fawkes API Fawkes Development Version
search_state.h
1/***************************************************************************
2 * search_state.h - Graph-based global path planning - A-Star search state
3 *
4 * Created: Tue Sep 18 18:14:58 2012
5 * Copyright 2012 Tim Niemueller [www.niemueller.de]
6 * 2002 Stefan Jacobs
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#ifndef _LIBS_NAVGRAPH_SEARCH_STATE_H_
23#define _LIBS_NAVGRAPH_SEARCH_STATE_H_
24
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>
29
30#include <cmath>
31#include <functional>
32
33namespace fawkes {
34
36{
37public:
39 const fawkes::NavGraphNode & goal,
40 fawkes::NavGraph * map_graph,
41 fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
42
44 const fawkes::NavGraphNode &goal,
45 fawkes::NavGraph * map_graph,
46 navgraph::EstimateFunction estimate_func,
47 navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
48 fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
49
51
53
54 virtual size_t
56 {
57 return key_;
58 }
59 virtual float estimate();
60 virtual bool is_goal();
61
62 /** Determine euclidean cost between two nodes.
63 * Note that the given notes are assumed to be adjacent nodes.
64 * @param from originating node
65 * @param to destination node
66 * @return cost from @p from to @p to.
67 */
68 static float
70 {
71 return sqrtf(powf(to.x() - from.x(), 2) + powf(to.y() - from.y(), 2));
72 }
73
74 /** Determine straight line estimate between two nodes.
75 * @param node node to query heuristic value for
76 * @param goal goal node to get estimate for
77 * @return estimate of cost from @p node to @p goal.
78 */
79 static float
81 {
82 return sqrtf(powf(goal.x() - node.x(), 2) + powf(goal.y() - node.y(), 2));
83 }
84
85private:
87 const fawkes::NavGraphNode & goal,
88 double cost_sofar,
89 NavGraphSearchState * parent_state,
90 fawkes::NavGraph * map_graph,
91 navgraph::EstimateFunction estimate_func,
92 navgraph::CostFunction cost_func,
93 fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
94
95private:
96 std::vector<AStarState *> children();
97
98 // state information
100
101 // goal information
103
104 fawkes::NavGraph *map_graph_;
105
106 fawkes::NavGraphConstraintRepo *constraint_repo_;
107
108 size_t key_;
109
110 navgraph::EstimateFunction estimate_func_;
111 navgraph::CostFunction cost_func_;
112};
113
114} // end of namespace fawkes
115
116#endif
This is the abstract(!) class for an A* State.
Definition: astar_state.h:39
Constraint repository to maintain blocks on nodes.
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
Graph-based path planner A* search state.
Definition: search_state.h:36
virtual size_t key()
Generates a unique key for this state.
Definition: search_state.h:55
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.
Definition: search_state.h:69
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.
Definition: search_state.h:80
Topological map graph.
Definition: navgraph.h:50
Fawkes library namespace.