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