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 
27 namespace 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  */
51 NavGraphSearchState::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  */
79 NavGraphSearchState::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 
149 float
151 {
152  return estimate_func_(node_, goal_);
153 }
154 
155 bool
157 {
158  return (node_.name() == goal_.name());
159 }
160 
161 std::vector<AStarState *>
162 NavGraphSearchState::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::vector< std::string > & reachable_nodes() const
Get reachable nodes.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
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.