Fawkes API Fawkes Development Version
navgraph.cpp
1
2/***************************************************************************
3 * navgraph.cpp - Topological graph
4 *
5 * Created: Fri Sep 21 15:55:49 2012
6 * Copyright 2012 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/exception.h>
24#include <navgraph/constraints/constraint_repo.h>
25#include <navgraph/navgraph.h>
26#include <navgraph/search_state.h>
27#include <utils/math/common.h>
28#include <utils/search/astar.h>
29
30#include <Eigen/Geometry>
31#include <algorithm>
32#include <cmath>
33#include <cstdio>
34#include <limits>
35#include <list>
36#include <queue>
37#include <set>
38
39namespace fawkes {
40
41namespace navgraph {
42const char *PROP_ORIENTATION = "orientation";
43} // namespace navgraph
44
45/** @class NavGraph <navgraph/navgraph.h>
46 * Topological map graph.
47 * This class represents a topological graph using 2D map coordinates
48 * with nodes and edges. Both can be annotated with certain free-form
49 * properties which can be used at run-time for example to instruct
50 * the robot behavior.
51 *
52 * The class supports change listeners. These are called whenever the graph
53 * is changed, that is if a node or edge is added or if the graph is assigned
54 * from another one (i.e. graph := new_graph).
55 *
56 * This class is based on KBSG RCSoft's MapGraph but has been
57 * abstracted and improved.
58 * @author Tim Niemueller
59 */
60
61/** Constructor.
62 * @param graph_name Name of the graph, for example to handle multiple
63 * graphs, e.g. for multiple levels of a building.
64 */
65NavGraph::NavGraph(const std::string &graph_name)
66{
67 graph_name_ = graph_name;
69 /* recursive mutex */ true);
70 search_default_funcs_ = true;
71 search_estimate_func_ = NavGraphSearchState::straight_line_estimate;
72 search_cost_func_ = NavGraphSearchState::euclidean_cost;
73 reachability_calced_ = false;
74 notifications_enabled_ = true;
75}
76
77/** Copy constructor.
78 * This method will remove internal data like nodes, and edges
79 * and copy the data from the passed instance. The change listeners will
80 * not be copied. The assignment operator will trigger all registered
81 * change listeners to be called.
82 * @param g graph from which to copy the data
83 */
85{
86 graph_name_ = g.graph_name_;
87 nodes_.clear();
88 nodes_ = g.nodes_;
89 edges_.clear();
90 edges_ = g.edges_;
91}
92
93/** Virtual empty destructor. */
95{
96}
97
98/** Assign/copy structures from another graph.
99 * This method will remove internal data like nodes, and edges
100 * and copy the data from the passed instance. The change listeners will
101 * not be copied. The assignment operator will trigger all registered
102 * change listeners to be called.
103 * @param g graph from which to copy the data
104 * @return reference to this instance
105 */
106NavGraph &
108{
109 graph_name_ = g.graph_name_;
110 nodes_.clear();
111 nodes_ = g.nodes_;
112 edges_.clear();
113 edges_ = g.edges_;
114
116
117 return *this;
118}
119
120/** Get graph name.
121 * @return graph name
122 */
123std::string
125{
126 return graph_name_;
127}
128
129/** Get nodes of the graph.
130 * @return const reference to vector of nodes of this graph
131 */
132const std::vector<NavGraphNode> &
134{
135 return nodes_;
136}
137
138/** Get edges of the graph.
139 * @return const reference to vector of edges of this graph
140 */
141const std::vector<NavGraphEdge> &
143{
144 return edges_;
145}
146
147/** Get locked pointer to constraint repository.
148 * @return locked pointer to navgraph constraint repo. Note that you must
149 * lock it when invoking operations on the repo.
150 */
153{
154 return constraint_repo_;
155}
156
157/** Get a specified node.
158 * @param name name of the node to get
159 * @return the node representation of the searched node, if not
160 * found returns an invalid node.
161 */
163NavGraph::node(const std::string &name) const
164{
165 std::vector<NavGraphNode>::const_iterator n =
166 std::find_if(nodes_.begin(), nodes_.end(), [&name](const NavGraphNode &node) {
167 return node.name() == name;
168 });
169 if (n != nodes_.end()) {
170 return *n;
171 } else {
172 return NavGraphNode();
173 }
174}
175
176/** Get node closest to a specified point with a certain property.
177 * This search does *NOT* consider unconnected nodes.
178 * @param pos_x X coordinate in global (map) frame
179 * @param pos_y X coordinate in global (map) frame
180 * @param property property the node must have to be considered,
181 * empty string to not check for any property
182 * @return node closest to the given point in the global frame, or an
183 * invalid node if such a node cannot be found
184 */
186NavGraph::closest_node(float pos_x, float pos_y, const std::string &property) const
187{
188 return closest_node(pos_x, pos_y, false, property);
189}
190
191/** Get node closest to a specified point with a certain property.
192 * This search *does* consider unconnected nodes.
193 * @param pos_x X coordinate in global (map) frame
194 * @param pos_y X coordinate in global (map) frame
195 * @param property property the node must have to be considered,
196 * empty string to not check for any property
197 * @return node closest to the given point in the global frame, or an
198 * invalid node if such a node cannot be found
199 */
201NavGraph::closest_node_with_unconnected(float pos_x, float pos_y, const std::string &property) const
202{
203 return closest_node(pos_x, pos_y, true, property);
204}
205
206/** Get node closest to another node with a certain property.
207 * This search does *NOT* consider unconnected nodes.
208 * @param node_name the name of the node from which to start
209 * @param property property the node must have to be considered,
210 * empty string to not check for any property
211 * @return node closest to the given point in the global frame, or an
212 * invalid node if such a node cannot be found. The node will obviously
213 * not be the node with the name @p node_name.
214 */
216NavGraph::closest_node_to(const std::string &node_name, const std::string &property) const
217{
218 return closest_node_to(node_name, false, property);
219}
220
221/** Get node closest to another node with a certain property.
222 * This search *does* consider unconnected nodes.
223 * @param node_name the name of the node from which to start
224 * @param property property the node must have to be considered,
225 * empty string to not check for any property
226 * @return node closest to the given point in the global frame, or an
227 * invalid node if such a node cannot be found. The node will obviously
228 * not be the node with the name @p node_name.
229 */
232 const std::string &property) const
233{
234 return closest_node_to(node_name, true, property);
235}
236
237/** Get node closest to a specified point with a certain property.
238 * @param pos_x X coordinate in global (map) frame
239 * @param pos_y X coordinate in global (map) frame
240 * @param consider_unconnected consider unconnected node for the search
241 * of the closest node
242 * @param property property the node must have to be considered,
243 * empty string to not check for any property
244 * @return node closest to the given point in the global frame, or an
245 * invalid node if such a node cannot be found
246 */
249 float pos_y,
250 bool consider_unconnected,
251 const std::string &property) const
252{
253 std::vector<NavGraphNode> nodes = search_nodes(property);
254
255 float min_dist = std::numeric_limits<float>::max();
256
257 std::vector<NavGraphNode>::iterator i;
258 std::vector<NavGraphNode>::iterator elem = nodes.end();
259 for (i = nodes.begin(); i != nodes.end(); ++i) {
260 if (!consider_unconnected && i->unconnected())
261 continue;
262
263 float dx = i->x() - pos_x;
264 float dy = i->y() - pos_y;
265 float dist = sqrtf(dx * dx + dy * dy);
266 if (sqrtf(dx * dx + dy * dy) < min_dist) {
267 min_dist = dist;
268 elem = i;
269 }
270 }
271
272 if (elem == nodes.end()) {
273 return NavGraphNode();
274 } else {
275 return *elem;
276 }
277}
278
279/** Get node closest to another node with a certain property.
280 * @param node_name the name of the node from which to start
281 * @param consider_unconnected consider unconnected node for the search
282 * of the closest node
283 * @param property property the node must have to be considered,
284 * empty string to not check for any property
285 * @return node closest to the given point in the global frame, or an
286 * invalid node if such a node cannot be found. The node will obviously
287 * not be the node with the name @p node_name.
288 */
290NavGraph::closest_node_to(const std::string &node_name,
291 bool consider_unconnected,
292 const std::string &property) const
293{
294 NavGraphNode n = node(node_name);
295 std::vector<NavGraphNode> nodes = search_nodes(property);
296
297 float min_dist = std::numeric_limits<float>::max();
298
299 std::vector<NavGraphNode>::iterator i;
300 std::vector<NavGraphNode>::iterator elem = nodes.begin();
301 for (i = nodes.begin(); i != nodes.end(); ++i) {
302 if (!consider_unconnected && i->unconnected())
303 continue;
304
305 float dx = i->x() - n.x();
306 float dy = i->y() - n.y();
307 float dist = sqrtf(dx * dx + dy * dy);
308 if ((sqrtf(dx * dx + dy * dy) < min_dist) && (i->name() != node_name)) {
309 min_dist = dist;
310 elem = i;
311 }
312 }
313
314 if (elem == nodes.end()) {
315 return NavGraphNode();
316 } else {
317 return *elem;
318 }
319}
320
321/** Get a specified edge.
322 * @param from originating node name
323 * @param to target node name
324 * @return the edge representation for the edge with the given
325 * originating and target nodes or an invalid edge if the edge
326 * cannot be found
327 */
329NavGraph::edge(const std::string &from, const std::string &to) const
330{
331 std::vector<NavGraphEdge>::const_iterator e =
332 std::find_if(edges_.begin(), edges_.end(), [&from, &to](const NavGraphEdge &edge) {
333 return (edge.from() == from && edge.to() == to)
334 || (!edge.is_directed() && (edge.to() == from && edge.from() == to));
335 });
336 if (e != edges_.end()) {
337 return *e;
338 } else {
339 return NavGraphEdge();
340 }
341}
342
343/** Get edge closest to a specified point.
344 * The point must be within an imaginery line segment parallel to
345 * the edge, that is a line perpendicular to the edge must go
346 * through the point and a point on the edge line segment.
347 * @param pos_x X coordinate in global (map) frame of point
348 * @param pos_y X coordinate in global (map) frame of point
349 * @return edge closest to the given point, or invalid edge if
350 * such an edge does not exist.
351 */
353NavGraph::closest_edge(float pos_x, float pos_y) const
354{
355 float min_dist = std::numeric_limits<float>::max();
356
357 NavGraphEdge rv;
358
359 Eigen::Vector2f point(pos_x, pos_y);
360 for (const NavGraphEdge &edge : edges_) {
361 const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y());
362 const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y());
363 const Eigen::Vector2f direction(target - origin);
364 const Eigen::Vector2f direction_norm = direction.normalized();
365 const Eigen::Vector2f diff = point - origin;
366 const float t = direction.dot(diff) / direction.squaredNorm();
367
368 if (t >= 0.0 && t <= 1.0) {
369 // projection of the point onto the edge is within the line segment
370 float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
371 if (distance < min_dist) {
372 min_dist = distance;
373 rv = edge;
374 }
375 }
376 }
377
378 return rv;
379}
380
381/** Search nodes for given property.
382 * @param property property name to look for
383 * @return vector of nodes having the specified property
384 */
385std::vector<NavGraphNode>
386NavGraph::search_nodes(const std::string &property) const
387{
388 if (property == "") {
389 return nodes();
390 } else {
391 std::vector<NavGraphNode> rv;
392
393 std::vector<NavGraphNode>::const_iterator i;
394 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
395 if (i->has_property(property))
396 rv.push_back(*i);
397 }
398
399 return rv;
400 }
401}
402
403/** Check if a certain node exists.
404 * @param node node to look for (will check for a node with the same name)
405 * @return true if a node with the same name as the given node exists, false otherwise
406 */
407bool
409{
410 std::vector<NavGraphNode>::const_iterator n = std::find(nodes_.begin(), nodes_.end(), node);
411 return (n != nodes_.end());
412}
413
414/** Check if a certain node exists.
415 * @param name name of the node to look for
416 * @return true if a node with the given name exists, false otherwise
417 */
418bool
419NavGraph::node_exists(const std::string &name) const
420{
421 std::vector<NavGraphNode>::const_iterator n =
422 std::find_if(nodes_.begin(), nodes_.end(), [&name](const NavGraphNode &node) {
423 return node.name() == name;
424 });
425 return (n != nodes_.end());
426}
427
428/** Check if a certain edge exists.
429 * @param edge edge to look for (will check for a node with the same originating and target node)
430 * @return true if an edge with the same originating and target node exists, false otherwise
431 */
432bool
434{
435 std::vector<NavGraphEdge>::const_iterator e = std::find(edges_.begin(), edges_.end(), edge);
436 return (e != edges_.end());
437}
438
439/** Check if a certain edge exists.
440 * @param from originating node name
441 * @param to target node name
442 * @return true if an edge with the same originating and target node exists, false otherwise
443 */
444bool
445NavGraph::edge_exists(const std::string &from, const std::string &to) const
446{
447 std::vector<NavGraphEdge>::const_iterator e =
448 std::find_if(edges_.begin(), edges_.end(), [&from, &to](const NavGraphEdge &e) -> bool {
449 return (from == e.from() && to == e.to())
450 || (!e.is_directed() && (from == e.to() && to == e.from()));
451 });
452 return (e != edges_.end());
453}
454
455/** Add a node.
456 * @param node node to add
457 * @throw Exception thrown if node with the same name as @p node already exists
458 */
459void
461{
462 if (node_exists(node)) {
463 throw Exception("Node with name %s already exists", node.name().c_str());
464 } else {
465 nodes_.push_back(node);
466 apply_default_properties(nodes_.back());
467 reachability_calced_ = false;
469 }
470}
471
472///@cond INTERNAL
473template <class T>
474typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
475almost_equal(T x, T y, int ulp)
476{
477 // the machine epsilon has to be scaled to the magnitude of the values used
478 // and multiplied by the desired precision in ULPs (units in the last place)
479 return (std::abs(x - y) < std::numeric_limits<T>::epsilon() * std::abs(x + y) * ulp)
480 // unless the result is subnormal
481 || std::abs(x - y) < std::numeric_limits<T>::min();
482}
483///@endcond INTERNAL
484
485/** Add a node and connect it to the graph.
486 * The node is added similar to add_node(). Then, an edge is added connecting the
487 * node to the graph. There are two principal methods available:
488 * CLOSEST_NODE: simply connect to an existing node closest to the given node
489 * CLOSEST_EDGE: connect node to the edge in which segment it lies,
490 * i.e. search for an edge where we can find a perpendicular line
491 * going through the given node and any point on the edge's line
492 * segment. If no such segment is found, the node cannot be added.
493 * CLOSEST_EDGE_OR_NODE: first try CLOSEST_EDGE method, if that fails
494 * use CLOSEST_NODE.
495 * @param node node to add
496 * @param conn_mode connection mode to use
497 */
498void
500{
501 add_node(node);
502 switch (conn_mode) {
504
506
508 } catch (Exception &e) {
510 }
511 break;
512 }
513}
514
515/** Connect node to closest node.
516 * @param n node to connect to closest node
517 */
518void
520{
521 NavGraphNode closest = closest_node_to(n.name());
522 NavGraphEdge new_edge(n.name(), closest.name());
523 new_edge.set_property("created-for", n.name() + "--" + closest.name());
524 new_edge.set_property("generated", true);
526}
527
528/** Connect node to closest edge
529 * @param n node to connect to closest node
530 */
531void
533{
534 NavGraphEdge closest = closest_edge(n.x(), n.y());
535 cart_coord_2d_t p = closest.closest_point_on_edge(n.x(), n.y());
536
537 NavGraphNode closest_conn = closest_node(p.x, p.y);
538 NavGraphNode cn;
539 if (almost_equal(closest_conn.distance(p.x, p.y), 0.f, 2)) {
540 cn = closest_conn;
541 } else {
542 cn = NavGraphNode(NavGraph::format_name("C-%s", n.name().c_str()), p.x, p.y);
543 }
544
545 if (closest.from() == cn.name() || closest.to() == cn.name()) {
546 // we actually want to connect to one of the end nodes of the edge,
547 // simply add the new edge and we are done
548 NavGraphEdge new_edge(cn.name(), n.name());
549 new_edge.set_property("generated", true);
550 new_edge.set_property("created-for", cn.name() + "--" + n.name());
552 } else {
553 // we are inserting a new point into the edge
554 remove_edge(closest);
555 NavGraphEdge new_edge_1(closest.from(), cn.name());
556 NavGraphEdge new_edge_2(closest.to(), cn.name());
557 NavGraphEdge new_edge_3(cn.name(), n.name());
558 new_edge_1.set_properties(closest.properties());
559 new_edge_2.set_properties(closest.properties());
560 new_edge_3.set_property("created-for", cn.name() + "--" + n.name());
561 new_edge_3.set_property("generated", true);
562
563 if (!node_exists(cn))
564 add_node(cn);
568 }
569}
570
571/** Add an edge.
572 * @param edge edge to add
573 * @param mode edge add mode
574 * @param allow_existing if true allow an edge with the given parameters
575 * to exist. In that case the method silently returns. Note that you might
576 * loose edge properties in that case. If false, an exception is thrown if
577 * a similar edge exists.
578 * @throw Exception thrown if an edge for the same nodes already exist unless
579 * @p allow_existing is set true, then simply returns.
580 * Takes directed edges into account. So if a directed edge from p1 to p2
581 * exists, it is ok to add a (directed or undirected) edge from p2 to p1.
582 */
583void
584NavGraph::add_edge(const NavGraphEdge &edge, NavGraph::EdgeMode mode, bool allow_existing)
585{
586 if (edge_exists(edge)) {
587 if (allow_existing)
588 return;
589 else
590 throw Exception("Edge from %s to %s already exists", edge.from().c_str(), edge.to().c_str());
591 } else {
592 switch (mode) {
593 case EDGE_NO_INTERSECTION: edge_add_no_intersection(edge); break;
594
595 case EDGE_SPLIT_INTERSECTION: edge_add_split_intersection(edge); break;
596
597 case EDGE_FORCE:
598 edges_.push_back(edge);
599 edges_.back().set_nodes(node(edge.from()), node(edge.to()));
600 break;
601 }
602
603 reachability_calced_ = false;
605 }
606}
607
608/** Remove a node.
609 * @param node node to remove
610 */
611void
613{
614 nodes_.erase(std::remove(nodes_.begin(), nodes_.end(), node));
615 edges_.erase(std::remove_if(edges_.begin(),
616 edges_.end(),
617 [&node](const NavGraphEdge &edge) -> bool {
618 return edge.from() == node.name() || edge.to() == node.name();
619 }),
620 edges_.end());
621 reachability_calced_ = false;
623}
624
625/** Remove a node.
626 * @param node_name name of node to remove
627 */
628void
629NavGraph::remove_node(const std::string &node_name)
630{
631 nodes_.erase(std::remove_if(nodes_.begin(),
632 nodes_.end(),
633 [&node_name](const NavGraphNode &node) -> bool {
634 return node.name() == node_name;
635 }),
636 nodes_.end());
637 edges_.erase(std::remove_if(edges_.begin(),
638 edges_.end(),
639 [&node_name](const NavGraphEdge &edge) -> bool {
640 return edge.from() == node_name || edge.to() == node_name;
641 }),
642 edges_.end());
643 reachability_calced_ = false;
645}
646
647/** Remove orphan nodes.
648 * Remove nodes which are neither marked unconnected nor participate
649 * on any edge.
650 */
651void
653{
654 std::list<std::string> to_remove;
655
656 for (const NavGraphNode &n : nodes_) {
657 if (n.unconnected())
658 continue;
659
660 for (const NavGraphEdge &e : edges_) {
661 if (e.from_node() == n || e.to_node() == n)
662 continue;
663 }
664
665 to_remove.push_back(n.name());
666 }
667
668 for (const auto &nn : to_remove) {
669 remove_node(nn);
670 }
671}
672
673/** Remove an edge
674 * @param edge edge to remove
675 */
676void
678{
679 edges_.erase(std::remove_if(edges_.begin(),
680 edges_.end(),
681 [&edge](const NavGraphEdge &e) -> bool {
682 return (edge.from() == e.from() && edge.to() == e.to())
683 || (!e.is_directed()
684 && (edge.from() == e.to() && edge.to() == e.from()));
685 }),
686 edges_.end());
687 reachability_calced_ = false;
689}
690
691/** Remove an edge
692 * @param from originating node name
693 * @param to target node name
694 */
695void
696NavGraph::remove_edge(const std::string &from, const std::string &to)
697{
698 edges_.erase(std::remove_if(edges_.begin(),
699 edges_.end(),
700 [&from, &to](const NavGraphEdge &edge) -> bool {
701 return (edge.from() == from && edge.to() == to)
702 || (!edge.is_directed()
703 && (edge.to() == from && edge.from() == to));
704 }),
705 edges_.end());
706 reachability_calced_ = false;
708}
709
710/** Update a given node.
711 * Will search for a node with the same name as the given node and will then
712 * call the assignment operator. This is intended to update properties of a node.
713 * @param node node to update
714 */
715void
717{
718 std::vector<NavGraphNode>::iterator n = std::find(nodes_.begin(), nodes_.end(), node);
719 if (n != nodes_.end()) {
720 *n = node;
721 } else {
722 throw Exception("No node with name %s known", node.name().c_str());
723 }
724}
725
726/** Update a given edge.
727 * Will search for an edge with the same originating and target node as the
728 * given edge and will then call the assignment operator. This is intended
729 * to update properties of an edge.
730 * @param edge edge to update
731 */
732void
734{
735 std::vector<NavGraphEdge>::iterator e = std::find(edges_.begin(), edges_.end(), edge);
736 if (e != edges_.end()) {
737 *e = edge;
738 } else {
739 throw Exception("No edge from %s to %s is known", edge.from().c_str(), edge.to().c_str());
740 }
741}
742
743/** Remove all nodes and edges from navgraph.
744 * Use with caution, this means that no more searches etc. are possible.
745 */
746void
748{
749 nodes_.clear();
750 edges_.clear();
751 default_properties_.clear();
753}
754
755/** Get all default properties.
756 * @return property map
757 */
758const std::map<std::string, std::string> &
760{
761 return default_properties_;
762}
763
764/** Check if graph has specified default property.
765 * @param property property key
766 * @return true if node has specified property, false otherwise
767 */
768bool
769NavGraph::has_default_property(const std::string &property) const
770{
771 return default_properties_.find(property) != default_properties_.end();
772}
773
774/** Get specified default property as string.
775 * @param prop property key
776 * @return default property value as string
777 */
778std::string
779NavGraph::default_property(const std::string &prop) const
780{
781 std::map<std::string, std::string>::const_iterator p;
782 if ((p = default_properties_.find(prop)) != default_properties_.end()) {
783 return p->second;
784 } else {
785 return "";
786 }
787}
788
789/** Get property converted to float.
790 * @param prop property key
791 * @return property value
792 */
793float
794NavGraph::default_property_as_float(const std::string &prop) const
795{
797}
798
799/** Get property converted to int.
800 * @param prop property key
801 * @return property value
802 */
803int
804NavGraph::default_property_as_int(const std::string &prop) const
805{
807}
808
809/** Get property converted to bol.
810 * @param prop property key
811 * @return property value
812 */
813bool
814NavGraph::default_property_as_bool(const std::string &prop) const
815{
817}
818
819/** Set property.
820 * @param property property key
821 * @param value property value
822 */
823void
824NavGraph::set_default_property(const std::string &property, const std::string &value)
825{
826 default_properties_[property] = value;
827}
828
829/** Set default properties.
830 * This overwrites all existing properties.
831 * @param properties map of property name to value as string
832 */
833void
834NavGraph::set_default_properties(const std::map<std::string, std::string> &properties)
835{
836 default_properties_ = properties;
837}
838
839/** Set property.
840 * @param property property key
841 * @param value property value
842 */
843void
844NavGraph::set_default_property(const std::string &property, float value)
845{
846 default_properties_[property] = StringConversions::to_string(value);
847}
848
849/** Set property.
850 * @param property property key
851 * @param value property value
852 */
853void
854NavGraph::set_default_property(const std::string &property, int value)
855{
856 default_properties_[property] = StringConversions::to_string(value);
857}
858
859/** Set property.
860 * @param property property key
861 * @param value property value
862 */
863void
864NavGraph::set_default_property(const std::string &property, bool value)
865{
866 default_properties_[property] = value ? "true" : "false";
867}
868
869/** Set default properties on node for which no local value exists.
870 * This sets all default properties on the node, for which no
871 * property of the same name has been set on the node.
872 * @param node node to apply default properties to
873 */
874void
876{
877 for (const auto &p : default_properties_) {
878 if (!node.has_property(p.first)) {
879 node.set_property(p.first, p.second);
880 }
881 }
882}
883
884/** Get nodes reachable from specified nodes.
885 * @param node_name name of the node to get reachable nodes for
886 * @return vector of names of nodes reachable from the specified node
887 */
888std::vector<std::string>
889NavGraph::reachable_nodes(const std::string &node_name) const
890{
891 std::vector<std::string> rv;
892
893 NavGraphNode n = node(node_name);
894 if (!n.is_valid())
895 return rv;
896
897 std::vector<NavGraphEdge>::const_iterator i;
898 for (i = edges_.begin(); i != edges_.end(); ++i) {
899 if (i->is_directed()) {
900 if (i->from() == node_name) {
901 rv.push_back(i->to());
902 }
903 } else {
904 if (i->from() == node_name) {
905 rv.push_back(i->to());
906 } else if (i->to() == node_name) {
907 rv.push_back(i->from());
908 }
909 }
910 }
911
912 std::sort(rv.begin(), rv.end());
913 std::unique(rv.begin(), rv.end());
914 return rv;
915}
916
917/** Set cost and cost estimation function for searching paths.
918 * Note that this will influence each and every search (unless
919 * custom functions are passed for the search). So use with caution.
920 * We recommend to encapsulate different search modes as a plugin
921 * that can be loaded to enable to new search functions.
922 * Make sure to call unset_search_funcs() to restore the defaults.
923 * The function points must obviously be valid for the whole lifetime
924 * of the NavGraph or until unset.
925 * @param estimate_func cost estimation function
926 * @param cost_func actual cost function
927 * @see NavGraph::search_path
928 * @throw Exception if search functions have already been set.
929 */
930void
931NavGraph::set_search_funcs(navgraph::EstimateFunction estimate_func,
932 navgraph::CostFunction cost_func)
933{
934 if (!search_default_funcs_) {
935 throw Exception("Custom actual and estimated cost functions have already been set");
936 }
937 search_default_funcs_ = false;
938 search_estimate_func_ = estimate_func;
939 search_cost_func_ = cost_func;
940}
941
942/** Reset actual and estimated cost function to defaults. */
943void
945{
946 search_default_funcs_ = true;
947 search_estimate_func_ = NavGraphSearchState::straight_line_estimate;
948 search_cost_func_ = NavGraphSearchState::euclidean_cost;
949}
950
951/** Search for a path between two nodes with default distance costs.
952 * This function executes an A* search to find an (optimal) path
953 * from node @p from to node @p to.
954 * By default (unless set otherwise, confirm using uses_default_search()),
955 * the cost and estimated costs are calculated as the spatial euclidean
956 * distance between nodes. The cost is the sum of costs of all edges
957 * along the way from one node to another. The estimate is the straight line
958 * distance from any given node to the goal node (which is provably admissible).
959 * @param from node to search from
960 * @param to goal node
961 * @param use_constraints true to respect constraints imposed by the constraint
962 * repository, false to ignore the repository searching as if there were no
963 * constraints whatsoever.
964 * @param compute_constraints if true re-compute constraints, otherwise use constraints
965 * as-is, for example if they have been computed before to check for changes.
966 * @return ordered vector of nodes which denote a path from @p from to @p to.
967 * Note that the vector is empty if no path could be found (i.e. there is non
968 * or it was prohibited when using constraints.
969 */
972 const NavGraphNode &to,
973 bool use_constraints,
974 bool compute_constraints)
975{
976 return search_path(
977 from, to, search_estimate_func_, search_cost_func_, use_constraints, compute_constraints);
978}
979
980/** Search for a path between two nodes with default distance costs.
981 * This function executes an A* search to find an (optimal) path
982 * from node @p from to node @p to.
983 * By default (unless set otherwise, confirm using uses_default_search()),
984 * the cost and estimated costs are calculated as the spatial euclidean
985 * distance between nodes. The cost is the sum of costs of all edges
986 * along the way from one node to another. The estimate is the straight line
987 * distance from any given node to the goal node (which is provably admissible).
988 * @param from name of node to search from
989 * @param to name of the goal node
990 * @param use_constraints true to respect constraints imposed by the constraint
991 * repository, false to ignore the repository searching as if there were no
992 * constraints whatsoever.
993 * @param compute_constraints if true re-compute constraints, otherwise use constraints
994 * as-is, for example if they have been computed before to check for changes.
995 * @return ordered vector of nodes which denote a path from @p from to @p to.
996 * Note that the vector is empty if no path could be found (i.e. there is non
997 * or it was prohibited when using constraints.
998 */
1000NavGraph::search_path(const std::string &from,
1001 const std::string &to,
1002 bool use_constraints,
1003 bool compute_constraints)
1004{
1005 return search_path(
1006 from, to, search_estimate_func_, search_cost_func_, use_constraints, compute_constraints);
1007}
1008
1009/** Search for a path between two nodes.
1010 * This function executes an A* search to find an (optimal) path
1011 * from node @p from to node @p to.
1012 * @param from name of node to search from
1013 * @param to name of the goal node
1014 * @param estimate_func function to estimate the cost from any node to the goal.
1015 * Note that the estimate function must be admissible for optimal A* search. That
1016 * means that for no query may the calculated estimate be higher than the actual
1017 * cost.
1018 * @param cost_func function to calculate the cost from a node to another adjacent
1019 * node. Note that the cost function is directly related to the estimate function.
1020 * For example, the cost can be calculated in terms of distance between nodes, or in
1021 * time that it takes to travel from one node to the other. The estimate function must
1022 * match the cost function to be admissible.
1023 * @param use_constraints true to respect constraints imposed by the constraint
1024 * repository, false to ignore the repository searching as if there were no
1025 * constraints whatsoever.
1026 * @param compute_constraints if true re-compute constraints, otherwise use constraints
1027 * as-is, for example if they have been computed before to check for changes.
1028 * @return ordered vector of nodes which denote a path from @p from to @p to.
1029 * Note that the vector is empty if no path could be found (i.e. there is non
1030 * or it was prohibited when using constraints.
1031 */
1033NavGraph::search_path(const std::string & from,
1034 const std::string & to,
1035 navgraph::EstimateFunction estimate_func,
1036 navgraph::CostFunction cost_func,
1037 bool use_constraints,
1038 bool compute_constraints)
1039{
1040 NavGraphNode from_node(node(from));
1041 NavGraphNode to_node(node(to));
1042 return search_path(
1043 from_node, to_node, estimate_func, cost_func, use_constraints, compute_constraints);
1044}
1045
1046/** Search for a path between two nodes.
1047 * This function executes an A* search to find an (optimal) path
1048 * from node @p from to node @p to.
1049 * @param from node to search from
1050 * @param to goal node
1051 * @param estimate_func function to estimate the cost from any node to the goal.
1052 * Note that the estimate function must be admissible for optimal A* search. That
1053 * means that for no query may the calculated estimate be higher than the actual
1054 * cost.
1055 * @param cost_func function to calculate the cost from a node to another adjacent
1056 * node. Note that the cost function is directly related to the estimate function.
1057 * For example, the cost can be calculated in terms of distance between nodes, or in
1058 * time that it takes to travel from one node to the other. The estimate function must
1059 * match the cost function to be admissible.
1060 * @param use_constraints true to respect constraints imposed by the constraint
1061 * repository, false to ignore the repository searching as if there were no
1062 * constraints whatsoever.
1063 * @param compute_constraints if true re-compute constraints, otherwise use constraints
1064 * as-is, for example if they have been computed before to check for changes.
1065 * @return ordered vector of nodes which denote a path from @p from to @p to.
1066 * Note that the vector is empty if no path could be found (i.e. there is non
1067 * or it was prohibited when using constraints.
1068 */
1071 const NavGraphNode & to,
1072 navgraph::EstimateFunction estimate_func,
1073 navgraph::CostFunction cost_func,
1074 bool use_constraints,
1075 bool compute_constraints)
1076{
1077 if (!reachability_calced_)
1078 calc_reachability(/* allow multi graph */ true);
1079
1080 AStar astar;
1081
1082 std::vector<AStarState *> a_star_solution;
1083
1084 if (use_constraints) {
1085 constraint_repo_.lock();
1086 if (compute_constraints && constraint_repo_->has_constraints()) {
1087 constraint_repo_->compute();
1088 }
1089
1090 NavGraphSearchState *initial_state =
1091 new NavGraphSearchState(from, to, this, estimate_func, cost_func, *constraint_repo_);
1092 a_star_solution = astar.solve(initial_state);
1093 constraint_repo_.unlock();
1094 } else {
1095 NavGraphSearchState *initial_state =
1096 new NavGraphSearchState(from, to, this, estimate_func, cost_func);
1097 a_star_solution = astar.solve(initial_state);
1098 }
1099
1100 std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
1101 NavGraphSearchState * solstate;
1102 for (unsigned int i = 0; i < a_star_solution.size(); ++i) {
1103 solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]);
1104 path[i] = solstate->node();
1105 }
1106
1107 float cost = (!a_star_solution.empty())
1108 ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
1109 : -1;
1110
1111 return NavGraphPath(this, path, cost);
1112}
1113
1114/** Calculate cost between two adjacent nodes.
1115 * It is not verified whether the nodes are actually adjacent, but the cost
1116 * function is simply applied. This is done to increase performance.
1117 * The calculation will use the currently registered cost function.
1118 * @param from first node
1119 * @param to second node
1120 * @return cost from @p from to @p to
1121 */
1122float
1123NavGraph::cost(const NavGraphNode &from, const NavGraphNode &to) const
1124{
1125 return search_cost_func_(from, to);
1126}
1127
1128/** Make sure each node in the edges exists. */
1129void
1130NavGraph::assert_valid_edges()
1131{
1132 for (size_t i = 0; i < edges_.size(); ++i) {
1133 if (!node_exists(edges_[i].from())) {
1134 throw Exception("Node '%s' for edge '%s' -> '%s' does not exist",
1135 edges_[i].from().c_str(),
1136 edges_[i].from().c_str(),
1137 edges_[i].to().c_str());
1138 }
1139
1140 if (!node_exists(edges_[i].to())) {
1141 throw Exception("Node '%s' for edge '%s' -> '%s' does not exist",
1142 edges_[i].to().c_str(),
1143 edges_[i].from().c_str(),
1144 edges_[i].to().c_str());
1145 }
1146 }
1147}
1148
1149void
1150NavGraph::edge_add_no_intersection(const NavGraphEdge &edge)
1151{
1152 try {
1153 const NavGraphNode &n1 = node(edge.from());
1154 const NavGraphNode &n2 = node(edge.to());
1155 for (const NavGraphEdge &ne : edges_) {
1156 if (edge.from() == ne.from() || edge.from() == ne.to() || edge.to() == ne.to()
1157 || edge.to() == ne.from())
1158 continue;
1159
1160 if (ne.intersects(n1.x(), n1.y(), n2.x(), n2.y())) {
1161 throw Exception("Edge %s-%s%s not added: intersects with %s-%s%s",
1162 edge.from().c_str(),
1163 edge.is_directed() ? ">" : "-",
1164 edge.to().c_str(),
1165 ne.from().c_str(),
1166 ne.is_directed() ? ">" : "-",
1167 ne.to().c_str());
1168 }
1169 }
1171 } catch (Exception &ex) {
1172 throw Exception("Failed to add edge %s-%s%s: %s",
1173 edge.from().c_str(),
1174 edge.is_directed() ? ">" : "-",
1175 edge.to().c_str(),
1176 ex.what_no_backtrace());
1177 }
1178}
1179
1180void
1181NavGraph::edge_add_split_intersection(const NavGraphEdge &edge)
1182{
1183 std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections;
1184 const NavGraphNode & n1 = node(edge.from());
1185 const NavGraphNode & n2 = node(edge.to());
1186
1187 try {
1188 for (const NavGraphEdge &e : edges_) {
1189 cart_coord_2d_t ip;
1190 if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) {
1191 // we need to split the edge at the given intersection point,
1192 // and the new line segments as well
1193 intersections.push_back(std::make_pair(ip, e));
1194 }
1195 }
1196
1197 std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge>>::iterator> deletions;
1198
1199 for (auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) {
1200 const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1;
1201 const cart_coord_2d_t & c1 = p1.first;
1202 const NavGraphEdge & e1 = p1.second;
1203
1204 const NavGraphNode &n1_from = node(e1.from());
1205 const NavGraphNode &n1_to = node(e1.to());
1206
1207 for (auto i2 = std::next(i1); i2 != intersections.end(); ++i2) {
1208 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2;
1209 const cart_coord_2d_t & c2 = p2.first;
1210 const NavGraphEdge & e2 = p2.second;
1211
1212 if (points_different(c1.x, c1.y, c2.x, c2.y))
1213 continue;
1214
1215 float d = 1.;
1216 if (e1.from() == e2.from() || e1.from() == e2.to()) {
1217 d = point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y);
1218 } else if (e1.to() == e2.to() || e1.to() == e2.from()) {
1219 d = point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y);
1220 }
1221 if (d < 1e-4) {
1222 // the intersection point is the same as a common end
1223 // point of the two edges, only keep it once
1224 deletions.push_back(i1);
1225 break;
1226 }
1227 }
1228 }
1229 for (auto d = deletions.rbegin(); d != deletions.rend(); ++d) {
1230 intersections.erase(*d);
1231 }
1232
1233 if (intersections.empty()) {
1234 NavGraphEdge e(edge);
1235 e.set_property("created-for", edge.from() + "--" + edge.to());
1236 add_edge(e, EDGE_FORCE);
1237 } else {
1238 Eigen::Vector2f e_origin(n1.x(), n1.y());
1239 Eigen::Vector2f e_target(n2.x(), n2.y());
1240 Eigen::Vector2f e_dir = (e_target - e_origin).normalized();
1241
1242 intersections.sort([&e_origin, &e_dir](const std::pair<cart_coord_2d_t, NavGraphEdge> &p1,
1243 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2) {
1244 const Eigen::Vector2f p1p(p1.first.x, p1.first.y);
1245 const Eigen::Vector2f p2p(p2.first.x, p2.first.y);
1246 const float k1 = e_dir.dot(p1p - e_origin);
1247 const float k2 = e_dir.dot(p2p - e_origin);
1248 return k1 < k2;
1249 });
1250
1251 std::string en_from = edge.from();
1252 cart_coord_2d_t ec_from(n1.x(), n1.y());
1253 std::string prev_to;
1254 for (const auto &i : intersections) {
1255 const cart_coord_2d_t &c = i.first;
1256 const NavGraphEdge & e = i.second;
1257
1258 // add intersection point (if necessary)
1259 NavGraphNode ip = closest_node(c.x, c.y);
1260 if (!ip || points_different(c.x, c.y, ip.x(), ip.y())) {
1261 ip = NavGraphNode(gen_unique_name(), c.x, c.y);
1262 add_node(ip);
1263 }
1264
1265 // if neither edge end node is the intersection point, split the edge
1266 if (ip.name() != e.from() && ip.name() != e.to()) {
1267 NavGraphEdge e1(e.from(), ip.name(), e.is_directed());
1268 NavGraphEdge e2(ip.name(), e.to(), e.is_directed());
1269 remove_edge(e);
1270 e1.set_properties(e.properties());
1271 e2.set_properties(e.properties());
1272 add_edge(e1, EDGE_FORCE, /* allow existing */ true);
1273 add_edge(e2, EDGE_FORCE, /* allow existing */ true);
1274
1275 // this is a special case: we might intersect an edge
1276 // which has the same end node and thus the new edge
1277 // from the intersection node to the end node would
1278 // be added twice
1279 prev_to = e.to();
1280 }
1281
1282 // add segment edge
1283 if (en_from != ip.name() && prev_to != ip.name()) {
1284 NavGraphEdge e3(en_from, ip.name(), edge.is_directed());
1285 e3.set_property("created-for", en_from + "--" + ip.name());
1286 add_edge(e3, EDGE_FORCE, /* allow existing */ true);
1287 }
1288
1289 en_from = ip.name();
1290 ec_from = c;
1291 }
1292
1293 if (en_from != edge.to()) {
1294 NavGraphEdge e3(en_from, edge.to(), edge.is_directed());
1295 e3.set_property("created-for", en_from + "--" + edge.to());
1296 add_edge(e3, EDGE_FORCE, /* allow existing */ true);
1297 }
1298 }
1299
1300 } catch (Exception &ex) {
1301 throw Exception("Failed to add edge %s-%s%s: %s",
1302 edge.from().c_str(),
1303 edge.is_directed() ? ">" : "-",
1304 edge.to().c_str(),
1305 ex.what_no_backtrace());
1306 }
1307}
1308
1309void
1310NavGraph::assert_connected()
1311{
1312 std::set<std::string> traversed;
1313 std::set<std::string> nodeset;
1314 std::queue<NavGraphNode> q;
1315
1316 // find first connected not
1317 auto fcon = std::find_if_not(nodes_.begin(), nodes_.end(), [](const NavGraphNode &n) {
1318 return n.unconnected();
1319 });
1320 if (fcon == nodes_.end()) {
1321 // no connected nodes
1322 return;
1323 }
1324
1325 q.push(*fcon);
1326
1327 while (!q.empty()) {
1328 NavGraphNode &n = q.front();
1329 traversed.insert(n.name());
1330
1331 const std::vector<std::string> &reachable = n.reachable_nodes();
1332
1333 if (n.unconnected() && !reachable.empty()) {
1334 throw Exception("Node %s is marked unconnected but nodes are reachable from it",
1335 n.name().c_str());
1336 }
1337 std::vector<std::string>::const_iterator r;
1338 for (r = reachable.begin(); r != reachable.end(); ++r) {
1339 NavGraphNode target(node(*r));
1340 if (target.unconnected()) {
1341 throw Exception("Node %s is marked unconnected but is reachable from node %s\n",
1342 target.name().c_str(),
1343 n.name().c_str());
1344 }
1345 if (traversed.find(*r) == traversed.end())
1346 q.push(node(*r));
1347 }
1348 q.pop();
1349 }
1350
1351 std::vector<NavGraphNode>::iterator n;
1352 for (n = nodes_.begin(); n != nodes_.end(); ++n) {
1353 nodeset.insert(n->name());
1354 }
1355
1356 if (traversed.size() != nodeset.size()) {
1357 std::set<std::string> nodediff;
1358 std::set_difference(nodeset.begin(),
1359 nodeset.end(),
1360 traversed.begin(),
1361 traversed.end(),
1362 std::inserter(nodediff, nodediff.begin()));
1363
1364 // the nodes might be unconnected, in which case it is not
1365 // an error that they were mentioned. But it might still be
1366 // a problem if there was a *directed* outgoing edge from the
1367 // unconnected node, which we couldn't have spotted earlier
1368 std::set<std::string>::const_iterator ud = nodediff.begin();
1369 while (ud != nodediff.end()) {
1370 NavGraphNode udnode(node(*ud));
1371 if (udnode.unconnected()) {
1372 // it's ok to be in the disconnected set, but check if it has any
1373 // reachable nodes which is forbidden
1374 if (!udnode.reachable_nodes().empty()) {
1375 throw Exception("Node %s is marked unconnected but nodes are reachable from it",
1376 ud->c_str());
1377 }
1378#if __cplusplus > 201100L || defined(__GXX_EXPERIMENTAL_CXX0X__)
1379 ud = nodediff.erase(ud);
1380#else
1381 std::set<std::string>::const_iterator delit = ud;
1382 ++ud;
1383 nodediff.erase(delit);
1384#endif
1385 } else {
1386 ++ud;
1387 }
1388 }
1389
1390 if (!nodediff.empty()) {
1391 std::set<std::string>::iterator d = nodediff.begin();
1392 std::string disconnected = *d;
1393 for (++d; d != nodediff.end(); ++d) {
1394 disconnected += ", " + *d;
1395 }
1396 throw Exception("The graph is not fully connected, "
1397 "cannot reach (%s) from '%s' for example",
1398 disconnected.c_str(),
1399 fcon->name().c_str());
1400 }
1401 }
1402}
1403
1404/** Calculate eachability relations.
1405 * This will set the directly reachable nodes on each
1406 * of the graph nodes.
1407 * @param allow_multi_graph if true, allows multiple disconnected graph segments.
1408 */
1409void
1410NavGraph::calc_reachability(bool allow_multi_graph)
1411{
1412 if (nodes_.empty())
1413 return;
1414
1415 assert_valid_edges();
1416
1417 std::vector<NavGraphNode>::iterator i;
1418 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
1419 i->set_reachable_nodes(reachable_nodes(i->name()));
1420 }
1421
1422 std::vector<NavGraphEdge>::iterator e;
1423 for (e = edges_.begin(); e != edges_.end(); ++e) {
1424 e->set_nodes(node(e->from()), node(e->to()));
1425 }
1426
1427 if (!allow_multi_graph)
1428 assert_connected();
1429 reachability_calced_ = true;
1430}
1431
1432/** Generate a unique node name for the given prefix.
1433 * Will simply add a number and tries from 0 to MAXINT.
1434 * Note that to add a unique name you must protect the navgraph
1435 * from concurrent modification.
1436 * @param prefix the node name prefix
1437 * @return unique node name
1438 */
1439std::string
1440NavGraph::gen_unique_name(const char *prefix)
1441{
1442 for (unsigned int i = 0; i < std::numeric_limits<unsigned int>::max(); ++i) {
1443 std::string name = format_name("%s%i", prefix, i);
1444 if (!node(name)) {
1445 return name;
1446 }
1447 }
1448 throw Exception("Cannot generate unique name for given prefix, all taken");
1449}
1450
1451/** Create node name from a format string.
1452 * @param format format for the name according to sprintf arguments
1453 * @param ... parameters according to format
1454 * @return generated name
1455 */
1456std::string
1457NavGraph::format_name(const char *format, ...)
1458{
1459 va_list arg;
1460 va_start(arg, format);
1461 char * tmp;
1462 std::string rv;
1463 if (vasprintf(&tmp, format, arg) != -1) {
1464 rv = tmp;
1465 free(tmp);
1466 }
1467 va_end(arg);
1468 return rv;
1469}
1470
1471/** Add a change listener.
1472 * @param listener listener to add
1473 */
1474void
1476{
1477 change_listeners_.push_back(listener);
1478}
1479
1480/** Remove a change listener.
1481 * @param listener listener to remove
1482 */
1483void
1485{
1486 change_listeners_.remove(listener);
1487}
1488
1489/** Enable or disable notifications.
1490 * When performing many operations in a row, processing the individual
1491 * events can be overwhelming, especially if there are many
1492 * listeners. Therefore, in such situations notifications should be
1493 * disabled and later re-enabled, followed by a call to
1494 * notify_of_change().
1495 * @param enabled true to enable notifications, false to disable
1496 */
1497void
1499{
1500 notifications_enabled_ = enabled;
1501}
1502
1503/** Notify all listeners of a change. */
1504void
1506{
1507 if (!notifications_enabled_)
1508 return;
1509
1510 std::list<ChangeListener *> tmp_listeners = change_listeners_;
1511
1512 std::list<ChangeListener *>::iterator l;
1513 for (l = tmp_listeners.begin(); l != tmp_listeners.end(); ++l) {
1514 (*l)->graph_changed();
1515 }
1516}
1517
1518/** @class NavGraph::ChangeListener <navgraph/navgraph.h>
1519 * Topological graph change listener.
1520 * @author Tim Niemueller
1521 *
1522 * @fn void NavGraph::ChangeListener::graph_changed() noexcept = 0
1523 * Function called if the graph has been changed.
1524 */
1525
1526/** Virtual empty destructor. */
1528{
1529}
1530
1531} // end of namespace fawkes
This is an implementation of the A* search algorithm.
Definition: astar.h:37
std::vector< AStarState * > solve(AStarState *initialState)
Solves a situation given by the initial state with AStar, and returns a vector of AStarStates that so...
Definition: astar.cpp:79
Base class for exceptions in Fawkes.
Definition: exception.h:36
LockPtr<> is a reference-counting shared lockable smartpointer.
Definition: lockptr.h:55
Constraint repository to maintain blocks on nodes.
Topological graph edge.
Definition: navgraph_edge.h:38
bool is_directed() const
Check if edge is directed.
void set_property(const std::string &property, const std::string &value)
Set property.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
const std::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_edge.h:96
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
const NavGraphNode & from_node() const
Get edge originating node.
Definition: navgraph_edge.h:70
const NavGraphNode & to_node() const
Get edge target node.
Definition: navgraph_edge.h:78
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
void set_property(const std::string &property, const std::string &value)
Set property.
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
float distance(const NavGraphNode &n)
Get euclidean distance from this node to another.
Definition: navgraph_node.h:88
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
Class representing a path for a NavGraph.
Definition: navgraph_path.h:37
Graph-based path planner A* search state.
Definition: search_state.h:36
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
Definition: search_state.h:69
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 graph change listener.
Definition: navgraph.h:179
virtual ~ChangeListener()
Virtual empty destructor.
Definition: navgraph.cpp:1527
Topological map graph.
Definition: navgraph.h:50
ConnectionMode
Connect mode enum for connect_node_* methods.
Definition: navgraph.h:53
@ CLOSEST_EDGE
Connect to closest edge.
Definition: navgraph.h:55
@ CLOSEST_EDGE_OR_NODE
try to connect to closest edge, if that fails, connect to closest node
Definition: navgraph.h:56
@ CLOSEST_NODE
Connect to closest node.
Definition: navgraph.h:54
void update_node(const NavGraphNode &node)
Update a given node.
Definition: navgraph.cpp:716
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
Definition: navgraph.cpp:1484
NavGraph & operator=(const NavGraph &g)
Assign/copy structures from another graph.
Definition: navgraph.cpp:107
std::vector< std::string > reachable_nodes(const std::string &node_name) const
Get nodes reachable from specified nodes.
Definition: navgraph.cpp:889
void clear()
Remove all nodes and edges from navgraph.
Definition: navgraph.cpp:747
NavGraphNode closest_node_to_with_unconnected(const std::string &node_name, const std::string &property="") const
Get node closest to another node with a certain property.
Definition: navgraph.cpp:231
virtual ~NavGraph()
Virtual empty destructor.
Definition: navgraph.cpp:94
void remove_orphan_nodes()
Remove orphan nodes.
Definition: navgraph.cpp:652
NavGraphEdge edge(const std::string &from, const std::string &to) const
Get a specified edge.
Definition: navgraph.cpp:329
void unset_search_funcs()
Reset actual and estimated cost function to defaults.
Definition: navgraph.cpp:944
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
Definition: navgraph.cpp:769
NavGraphNode closest_node_with_unconnected(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Definition: navgraph.cpp:201
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:584
bool default_property_as_bool(const std::string &prop) const
Get property converted to bol.
Definition: navgraph.cpp:814
void remove_edge(const NavGraphEdge &edge)
Remove an edge.
Definition: navgraph.cpp:677
void set_default_property(const std::string &property, const std::string &value)
Set property.
Definition: navgraph.cpp:824
std::string default_property(const std::string &prop) const
Get specified default property as string.
Definition: navgraph.cpp:779
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
Definition: navgraph.cpp:759
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
Definition: navgraph.cpp:1123
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Definition: navgraph.cpp:186
void update_edge(const NavGraphEdge &edge)
Update a given edge.
Definition: navgraph.cpp:733
static std::string format_name(const char *format,...)
Create node name from a format string.
Definition: navgraph.cpp:1457
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
Definition: navgraph.cpp:152
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
Definition: navgraph.cpp:386
int default_property_as_int(const std::string &prop) const
Get property converted to int.
Definition: navgraph.cpp:804
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:460
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
Definition: navgraph.cpp:834
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1410
bool node_exists(const NavGraphNode &node) const
Check if a certain node exists.
Definition: navgraph.cpp:408
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
EdgeMode
Mode to use to add edges.
Definition: navgraph.h:61
@ EDGE_SPLIT_INTERSECTION
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
Definition: navgraph.h:65
@ EDGE_NO_INTERSECTION
Only add edge if it does not intersect.
Definition: navgraph.h:63
@ EDGE_FORCE
add nodes no matter what (be careful)
Definition: navgraph.h:62
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:142
void set_notifications_enabled(bool enabled)
Enable or disable notifications.
Definition: navgraph.cpp:1498
float default_property_as_float(const std::string &prop) const
Get property converted to float.
Definition: navgraph.cpp:794
NavGraphEdge closest_edge(float pos_x, float pos_y) const
Get edge closest to a specified point.
Definition: navgraph.cpp:353
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
Definition: navgraph.cpp:519
std::string gen_unique_name(const char *prefix="U-")
Generate a unique node name for the given prefix.
Definition: navgraph.cpp:1440
void remove_node(const NavGraphNode &node)
Remove a node.
Definition: navgraph.cpp:612
void set_search_funcs(navgraph::EstimateFunction estimate_func, navgraph::CostFunction cost_func)
Set cost and cost estimation function for searching paths.
Definition: navgraph.cpp:931
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
Definition: navgraph.cpp:1000
NavGraphNode node(const std::string &name) const
Get a specified node.
Definition: navgraph.cpp:163
std::string name() const
Get graph name.
Definition: navgraph.cpp:124
NavGraph(const std::string &graph_name)
Constructor.
Definition: navgraph.cpp:65
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
Definition: navgraph.cpp:875
bool edge_exists(const NavGraphEdge &edge) const
Check if a certain edge exists.
Definition: navgraph.cpp:433
void add_change_listener(ChangeListener *listener)
Add a change listener.
Definition: navgraph.cpp:1475
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
Definition: navgraph.cpp:532
void add_node_and_connect(const NavGraphNode &node, ConnectionMode conn_mode)
Add a node and connect it to the graph.
Definition: navgraph.cpp:499
void notify_of_change() noexcept
Notify all listeners of a change.
Definition: navgraph.cpp:1505
NavGraphNode closest_node_to(const std::string &node_name, const std::string &property="") const
Get node closest to another node with a certain property.
Definition: navgraph.cpp:216
static float to_float(std::string s)
Convert string to a float value.
static bool to_bool(std::string s)
Convert string to a bool value.
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
static int to_int(std::string s)
Convert string to an int value.
Fawkes library namespace.
float point_dist(float x1, float y1, float x2, float y2)
Get distance of two points.
Definition: common.h:82
bool points_different(float x1, float y1, float x2, float y2, float threshold=1e-4)
Check if two points are different with regard to a given threshold.
Definition: common.h:100
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66