Fawkes API Fawkes Development Version
search_state.cpp
1/***************************************************************************
2 * search_state.cpp - Graph-based global path planning - A-Star search state
3 *
4 * Created: Tue Sep 18 18:21:56 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#include <navgraph/search_state.h>
23
24#include <cmath>
25#include <functional>
26
27namespace fawkes {
28
29/** @class NavGraphSearchState <navgraph/search_state.h>
30 * Graph-based path planner A* search state.
31 * @author Tim Niemueller
32 */
33
34/** Constructor.
35 * @param node graph node this search state represents
36 * @param goal graph node of the goal
37 * @param cost_sofar the cost until to this node from the start
38 * @param parent parent search state
39 * @param map_graph map graph
40 * @param estimate_func function to estimate the cost from any node to the goal.
41 * Note that the estimate function must be admissible for optimal A* search. That
42 * means that for no query may the calculated estimate be higher than the actual
43 * cost.
44 * @param cost_func function to calculate the cost from a node to another adjacent
45 * node. Note that the cost function is directly related to the estimate function.
46 * For example, the cost can be calculated in terms of distance between nodes, or in
47 * time that it takes to travel from one node to the other. The estimate function must
48 * match the cost function to be admissible.
49 * @param constraint_repo constraint repository, null to plan only without constraints
50 */
51NavGraphSearchState::NavGraphSearchState(const NavGraphNode & node,
52 const NavGraphNode & goal,
53 double cost_sofar,
54 NavGraphSearchState * parent,
55 NavGraph * map_graph,
56 navgraph::EstimateFunction estimate_func,
57 navgraph::CostFunction cost_func,
58 fawkes::NavGraphConstraintRepo *constraint_repo)
59: AStarState(cost_sofar, parent), estimate_func_(estimate_func), cost_func_(cost_func)
60{
61 node_ = node;
62 goal_ = goal;
63 map_graph_ = map_graph;
64
65 total_estimated_cost = path_cost + estimate();
66
67 std::hash<std::string> h;
68 key_ = h(node_.name());
69
70 constraint_repo_ = constraint_repo;
71}
72
73/** Constructor.
74 * @param node graph node this search state represents
75 * @param goal graph node of the goal
76 * @param map_graph map graph
77 * @param constraint_repo constraint repository, null to plan only without constraints
78 */
79NavGraphSearchState::NavGraphSearchState(const NavGraphNode & node,
80 const NavGraphNode & goal,
81 NavGraph * map_graph,
82 fawkes::NavGraphConstraintRepo *constraint_repo)
83: AStarState(0, NULL)
84{
85 node_ = node;
86 goal_ = goal;
87 map_graph_ = map_graph;
88
89 estimate_func_ = straight_line_estimate;
90 cost_func_ = euclidean_cost;
91
93
94 std::hash<std::string> h;
95 key_ = h(node_.name());
96
97 constraint_repo_ = constraint_repo;
98}
99
100/** Constructor.
101 * @param node graph node this search state represents
102 * @param goal graph node of the goal
103 * @param map_graph map graph
104 * @param estimate_func function to estimate the cost from any node to the goal.
105 * Note that the estimate function must be admissible for optimal A* search. That
106 * means that for no query may the calculated estimate be higher than the actual
107 * cost.
108 * @param cost_func function to calculate the cost from a node to another adjacent
109 * node. Note that the cost function is directly related to the estimate function.
110 * For example, the cost can be calculated in terms of distance between nodes, or in
111 * time that it takes to travel from one node to the other. The estimate function must
112 * match the cost function to be admissible.
113 * @param constraint_repo constraint repository, null to plan only without constraints
114 */
116 const NavGraphNode & goal,
117 NavGraph * map_graph,
118 navgraph::EstimateFunction estimate_func,
119 navgraph::CostFunction cost_func,
120 fawkes::NavGraphConstraintRepo *constraint_repo)
121: AStarState(0, NULL), estimate_func_(estimate_func), cost_func_(cost_func)
122{
123 node_ = node;
124 goal_ = goal;
125 map_graph_ = map_graph;
126
128
129 std::hash<std::string> h;
130 key_ = h(node_.name());
131
132 constraint_repo_ = constraint_repo;
133}
134
135/** Destructor. */
137{
138}
139
140/** Get graph node corresponding to this search state.
141 * @return graph node corresponding to this search state
142 */
145{
146 return node_;
147}
148
149float
151{
152 return estimate_func_(node_, goal_);
153}
154
155bool
157{
158 return (node_.name() == goal_.name());
159}
160
161std::vector<AStarState *>
162NavGraphSearchState::children()
163{
164 std::vector<AStarState *> children;
165 children.clear();
166
167 std::vector<std::string> descendants = node_.reachable_nodes();
168
169 for (unsigned int i = 0; i < descendants.size(); ++i) {
170 NavGraphNode d = map_graph_->node(descendants[i]);
171
172 bool expand = true;
173 if (constraint_repo_) {
174 if (constraint_repo_->blocks(d)) {
175 expand = false;
176 } else if (constraint_repo_->blocks(node_, d)) {
177 expand = false;
178 }
179 }
180
181 if (expand) {
182 float d_cost = cost_func_(node_, d);
183
184 if (constraint_repo_) {
185 float cost_factor = 0.;
186 if (constraint_repo_->increases_cost(node_, d, cost_factor)) {
187 d_cost *= cost_factor;
188 }
189 }
190
191 children.push_back(new NavGraphSearchState(d,
192 goal_,
193 path_cost + d_cost,
194 this,
195 map_graph_,
196 estimate_func_,
197 cost_func_,
198 constraint_repo_));
199 }
200 }
201
202 return children;
203}
204
205} // end of namespace fawkes
This is the abstract(!) class for an A* State.
Definition: astar_state.h:39
float total_estimated_cost
Total estimated cost.
Definition: astar_state.h:81
float path_cost
Cost of path leading to this search state.
Definition: astar_state.h:78
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.
Topological graph node.
Definition: navgraph_node.h:36
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
const std::vector< std::string > & reachable_nodes() const
Get reachable nodes.
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
NavGraphNode node(const std::string &name) const
Get a specified node.
Definition: navgraph.cpp:163
Fawkes library namespace.