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