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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Topological graph node.
Definition: navgraph_node.h:36
Sub-class representing a navgraph path traversal.
Definition: navgraph_path.h:40
bool last() const
Check if the current node is the last node in the path.
float remaining_cost() const
Get the remaining cost to the goal.
size_t current_index() const
Get index of current node in path.
void invalidate()
Invalidate this traversal.
const NavGraphNode & current() const
Get current node in path.
bool next()
Move on traversal to next node.
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool running() const
Check if traversal is currently runnung.
void set_current(size_t new_current)
Set the current node.
void reset()
Reset an ongoing traversal.
const NavGraphNode & peek_next() const
Peek on the next node.
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
std::vector< std::string > get_node_names() const
Get names of nodes in path.
Traversal traversal() const
Get a new path traversal handle.
bool empty() const
Check if path is empty.
bool operator<(const NavGraphPath &p) const
Check if this path is cheaper than the other path.
bool operator==(const NavGraphPath &p) const
Check if two paths are the same.
NavGraphPath()
Default constructor.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
size_t size() const
Get size of path.
void clear()
Clear all nodes on this path.
const NavGraph & graph() const
Get graph this path is based on.
float cost() const
Get cost of path from start to to end.
bool contains(const NavGraphNode &node) const
Check if the path contains a given node.
const NavGraphNode & goal() const
Get goal of path.
std::string get_path_as_string(const char delim=':') const
Get string representation of path.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
void set_nodes(const std::vector< NavGraphNode > &nodes, float cost=-1)
Set nodes erasing the current path.
Topological map graph.
Definition: navgraph.h:50
A NULL pointer was supplied where not allowed.
Definition: software.h:32
Index out of bounds.
Definition: software.h:86
Fawkes library namespace.
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