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
31namespace 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 */
60NavGraphPath::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 */
74bool
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 */
92bool
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 */
117void
118NavGraphPath::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 */
133void
134NavGraphPath::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 */
144bool
146{
147 return nodes_.empty();
148}
149
150/** Get size of path.
151 * @return number of nodes in path
152 */
153size_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 */
162void
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 */
173bool
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 */
183const 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 */
196const 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. */
235NavGraphPath::Traversal::Traversal() : path_(NULL), current_(-1)
236{
237}
238
239/** Constructor.
240 * @param path parent path to traverse
241 */
242NavGraphPath::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 */
251void
253{
254 current_ = -1;
255 path_ = NULL;
256}
257
258void
259NavGraphPath::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 */
270const 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 */
289const 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 */
303bool
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 */
314size_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 */
328bool
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 */
342bool
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 */
354size_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 */
368float
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 */
390void
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 */
401void
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 */
415std::string
416NavGraphPath::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 */
424std::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.
size_t size() const
Get size of path.
void clear()
Clear all nodes on this path.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the 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