Fawkes API  Fawkes Development Version
navgraph_path.cpp
1 
2 /***************************************************************************
3  * navgraph_path.cpp - Topological graph - path
4  *
5  * Created: Mon Jan 12 10:57:24 2015
6  * Copyright 2015 Tim Niemueller [www.niemueller.de]
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. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #include <navgraph/navgraph_path.h>
24 #include <navgraph/navgraph.h>
25 #include <core/exceptions/software.h>
26 #include <utils/misc/string_split.h>
27 
28 #include <cmath>
29 #include <algorithm>
30 
31 namespace fawkes {
32 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 /** @class NavGraphPath <navgraph/navgraph_path.h>
37  * Class representing a path for a NavGraph.
38  * A path is a consecutive sequence of nodes, where each node is either
39  * the first node in the path or reachable by its predecessor.
40  * A path may or may not specify a total cost. If the value is unavailable
41  * it shall be less than zero. If positive, the cost is valid. The unit
42  * of the cost is not specified but depends on the search metrics used
43  * to determine the path. Make sure to only compare paths that have been
44  * created with the same metrics.
45  * @author Tim Niemueller
46  */
47 
48 /** Default constructor.
49  * Creates an invalid path.
50  */
52  : graph_(NULL)
53 {
54  cost_ = -1;
55 }
56 
57 
58 /** Constructor.
59  * @param graph navgraph this path is based on
60  * @param nodes nodes that the path should follow. The nodes must build
61  * a sequence where each node is directly reachable from its predecessor.
62  * This is not verified internally.
63  * @param cost cost of the path, set to a value less than zero if unknown
64  */
66  std::vector<NavGraphNode> &nodes, float cost)
67  : graph_(graph), nodes_(nodes), cost_(cost)
68 {
69 }
70 
71 /** Check if this path is cheaper than the other path.
72  * If both paths have negative costs (the cost is unknown), then
73  * they are considered to be equal. Only if both cost values are
74  * positive are they compared.
75  * @param p path to compare to
76  * @return true if this path is cheaper in terms of cost than the
77  * other path, false if both costs are negative or the other path
78  * is cheaper.
79  */
80 bool
82 {
83  if (cost_ < 0 && p.cost_ < 0) return false;
84  return cost_ < p.cost_;
85 }
86 
87 
88 /** Check if two paths are the same.
89  * Two paths are the same iff they contain the same nodes in the
90  * exact same order and if they have the same cost (within a small
91  * epsilon of 0.00001 and only if both costs are positive). Costs
92  * are ignored should any of the two cost values be less than zero
93  * (unknown).
94  * @param p path to compare to
95  * @return true if the paths are the same by the definition above,
96  * false otherwise.
97  */
98 bool
100 {
101  if (nodes_.size() != p.nodes_.size()) return false;
102 
103  for (size_t i = 0; i < nodes_.size(); ++i) {
104  if (nodes_[i] != p.nodes_[i]) return false;
105  }
106 
107  if (cost_ >= 0 && p.cost_ >= 0 && fabs(cost_ - p.cost_) <= 0.00001) return false;
108 
109  return true;
110 }
111 
112 
113 /** Add a node to the path.
114  * The node must be reachable directly from the last node in the
115  * path (not verified internally) or the first node.
116  * @param node node to add to the path
117  * @param cost_from_end cost to the node from the current end of
118  * the path. It is added to the current total cost. The value is
119  * ignored if it is less than zero.
120  */
121 void
122 NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end)
123 {
124  nodes_.push_back(node);
125  if (cost_from_end > 0) {
126  cost_ += cost_from_end;
127  }
128 }
129 
130 
131 /** Set nodes erasing the current path.
132  * @param nodes nodes that the path should follow. The nodes must build
133  * a sequence where each node is directly reachable from its predecessor.
134  * This is not verified internally. This also invalidates any running
135  * traversal.
136  * @param cost cost of the path, set to a value less than zero if unknown
137  */
138 void
139 NavGraphPath::set_nodes(const std::vector<NavGraphNode> &nodes, float cost)
140 {
141  nodes_ = nodes;
142  cost_ = cost;
143 }
144 
145 
146 /** Check if path is empty.
147  * @return true if path is empty, i.e. it has no nodes at all,
148  * false otherwise.
149  */
150 bool
152 {
153  return nodes_.empty();
154 }
155 
156 
157 /** Get size of path.
158  * @return number of nodes in path
159  */
160 size_t
162 {
163  return nodes_.size();
164 }
165 
166 
167 /** Clear all nodes on this path.
168  * This sets the length of the path to zero and cost to unknown.
169  */
170 void
172 {
173  nodes_.clear();
174  cost_ = -1;
175 }
176 
177 
178 /** Check if the path contains a given node.
179  * @param node node to check for in current path
180  * @return true if the node is contained in the current path, false otherwise
181  */
182 bool
184 {
185  return (std::find(nodes_.begin(), nodes_.end(), node) != nodes_.end());
186 }
187 
188 
189 /** Get goal of path.
190  * @return goal of this path, i.e. the last node in the sequence of nodes.
191  * @throw Exeption if there are no nodes in this path
192  */
193 const NavGraphNode &
195 {
196  if (nodes_.empty()) {
197  throw Exception("No nodes in plan, cannot retrieve goal");
198  }
199 
200  return nodes_[nodes_.size() - 1];
201 }
202 
203 
204 /** Get graph this path is based on.
205  * @return const reference to graph this path is based on
206  */
207 const NavGraph &
209 {
210  return *graph_;
211 }
212 
213 
214 /** Get a new path traversal handle.
215  * @return new path traversal handle
216  */
219 {
220  return Traversal(this);
221 }
222 
223 /** @class NavGraphPath::Traversal <navgraph/navgraph_path.h>
224  * Sub-class representing a navgraph path traversal.
225  * A traversal is a step-by-step run through the node sequence (in order).
226  * There maybe any number of traversal open for a given path. But they
227  * become invalid should new nodes be set on the path.
228  * After creating a new traversal, you always need to call next for
229  * each new node including the first one. Code could look like this.
230  * @code
231  * NavGraphPath path = navgraph->search_path("from-here", "to-there");
232  * NavGraphPath::Traversal traversal = path.traversal();
233  *
234  * while (traversal.next()) {
235  * const NavGraphNode &current = traversal.current();
236  * // operate on node
237  * if (traversal.last()) {
238  * // current is the last node on the path and traversal
239  * // will end after this iteration.
240  * }
241  * }
242  * @endcode
243  * @author Tim Niemueller
244  */
245 
246 
247 /** Constructor. */
249  : path_(NULL), current_(-1)
250 {
251 }
252 
253 
254 /** Constructor.
255  * @param path parent path to traverse
256  */
258  : path_(path), current_(-1)
259 {
260 }
261 
262 
263 /** Invalidate this traversal.
264  * This will reset the parent path and the current node.
265  * This traversal can now longer be used afterwards other
266  * than assigning a new traversal.
267  */
268 void
270 {
271  current_ = -1;
272  path_ = NULL;
273 }
274 
275 void
276 NavGraphPath::Traversal::assert_initialized() const
277 {
278  if (! path_) throw NullPointerException("Traversal has not been properly initialized");
279 }
280 
281 /** Get current node in path.
282  * @return current node in traversal
283  * @throw Exception if no traversal is active, i.e. next() has not been called
284  * after a traversal reset or if the path has already been traversed completley.
285  */
286 const NavGraphNode &
288 {
289  assert_initialized();
290  if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
291  return path_->nodes_[current_];
292  } else {
293  throw OutOfBoundsException("No more nodes in path to query.");
294  }
295 }
296 
297 
298 /** Peek on the next node.
299  * Get the node following the current node without advancing
300  * the current index (the current node remains the same).
301  * @return node following the current node
302  * @throw OutOfBoundsException if the traversal has not been
303  * started with an initial call to next() or if the traversal
304  * has already ended or is currently at the last node.
305  */
306 const NavGraphNode &
308 {
309  assert_initialized();
310  if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size() - 1) {
311  return path_->nodes_[current_ + 1];
312  } else {
313  throw OutOfBoundsException("Next node not available, cannot peek");
314  }
315 }
316 
317 
318 /** Check if traversal is currently runnung.
319  * @return true if current() will return a valid result
320  */
321 bool
323 {
324  return (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size());
325 }
326 
327 /** Get index of current node in path.
328  * @return index of current node in traversal
329  * @throw Exception if no traversal is active, i.e. next() has not been called
330  * after a traversal reset or if the path has already been traversed completley.
331  */
332 size_t
334 {
335  assert_initialized();
336  if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
337  return current_;
338  } else {
339  throw OutOfBoundsException("No more nodes in path to query.");
340  }
341 }
342 
343 /** Move on traversal to next node.
344  * @return bool, if there was another node to traverse, false otherwise
345  */
346 bool
348 {
349  assert_initialized();
350  if (current_ < (ssize_t)path_->nodes_.size()) current_ += 1;
351 
352  return (current_ < (ssize_t)path_->nodes_.size());
353 }
354 
355 
356 /** Check if the current node is the last node in the path.
357  * @return true if the current node is the last node in the path,
358  * false otherwise
359  */
360 bool
362 {
363  assert_initialized();
364  return (current_ >= 0 && (size_t)current_ == (path_->nodes_.size() - 1));
365 }
366 
367 
368 /** Get the number of remaining nodes in path traversal.
369  * The number of remaining nodes is the number of nodes
370  * including the current node up until the last node in the path.
371  * @return number of remaining nodes in path traversal
372  */
373 size_t
375 {
376  assert_initialized();
377  if (current_ < 0) return path_->nodes_.size();
378  return path_->nodes_.size() - (size_t)current_;
379 }
380 
381 
382 /** Get the remaining cost to the goal.
383  * This sums the costs from the current to the goal node using
384  * the default registered cost function of the parent navgraph.
385  * @return cost from current to goal node
386  */
387 float
389 {
390  assert_initialized();
391  if (! path_->graph_) {
392  throw NullPointerException("Parent graph has not been set");
393  }
394 
395  if (current_ < 0) return path_->cost();
396 
397  float cost = 0.;
398  for (ssize_t i = current_; i < (ssize_t)path_->nodes_.size() - 1; ++i) {
399  cost += path_->graph_->cost(path_->nodes_[i], path_->nodes_[i+1]);
400  }
401 
402  return cost;
403 }
404 
405 
406 /** Reset an ongoing traversal.
407  * A new traversal afterwards will start the traversal from the beginning.
408  */
409 void
411 {
412  current_ = -1;
413 }
414 
415 
416 /** Set the current node.
417  * @param new_current new current node
418  * @throw OutOfBoundsException thrown if new current node is beyond
419  * number of nodes in path.
420  */
421 void
423 {
424  assert_initialized();
425  if (new_current >= path_->nodes_.size()) {
426  throw OutOfBoundsException("Invalid new index, is beyond path length");
427  }
428  current_ = new_current;
429 }
430 
431 
432 /** Get string representation of path.
433  * @param delim custom delimiter
434  * @return all nodes of the path in one string
435  */
436 std::string
437 NavGraphPath::get_path_as_string(const char delim) const
438 {
439  return str_join(get_node_names(), delim);
440 }
441 
442 /** Get names of nodes in path.
443  * @return vector of strings for all nodes
444  */
445 std::vector<std::string>
447 {
448  std::vector<std::string> nodes(nodes_.size());
449  for (size_t i = 0; i < nodes_.size(); ++i){
450  nodes[i] = nodes_[i].name();
451  }
452  return nodes;
453 }
454 
455 } // end of namespace fawkes
void clear()
Clear all nodes on this path.
size_t size() const
Get size of path.
void reset()
Reset an ongoing traversal.
const NavGraphNode & current() const
Get current node in path.
std::string get_path_as_string(const char delim=':') const
Get string representation of path.
Fawkes library namespace.
bool operator<(const NavGraphPath &p) const
Check if this path is cheaper than the other path.
Topological map graph.
Definition: navgraph.h:57
Class representing a path for a NavGraph.
Definition: navgraph_path.h:39
A NULL pointer was supplied where not allowed.
Definition: software.h:34
void set_current(size_t new_current)
Set the current node.
void set_nodes(const std::vector< NavGraphNode > &nodes, float cost=-1)
Set nodes erasing the current path.
float cost() const
Get cost of path from start to to end.
bool last() const
Check if the current node is the last node in the path.
Base class for exceptions in Fawkes.
Definition: exception.h:36
std::vector< std::string > get_node_names() const
Get names of nodes in path.
const NavGraphNode & peek_next() const
Peek on the next node.
size_t remaining() const
Get the number of remaining nodes in path traversal.
static std::string str_join(const std::vector< std::string > &v, char delim='/')
Join vector of strings string using given delimiter.
Definition: string_split.h:96
const NavGraph & graph() const
Get graph this path is based on.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Definition: navgraph_path.h:94
Traversal traversal() const
Get a new path traversal handle.
Sub-class representing a navgraph path traversal.
Definition: navgraph_path.h:41
bool running() const
Check if traversal is currently runnung.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:63
bool empty() const
Check if path is empty.
size_t current_index() const
Get index of current node in path.
Topological graph node.
Definition: navgraph_node.h:38
float remaining_cost() const
Get the remaining cost to the goal.
bool operator==(const NavGraphPath &p) const
Check if two paths are the same.
void invalidate()
Invalidate this traversal.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
bool contains(const NavGraphNode &node) const
Check if the path contains a given node.
Index out of bounds.
Definition: software.h:88
bool next()
Move on traversal to next node.
NavGraphPath()
Default constructor.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1107
const NavGraphNode & goal() const
Get goal of path.