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 
39 namespace fawkes {
40 
41 namespace navgraph {
42 const 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  */
65 NavGraph::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  */
106 NavGraph &
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  */
123 std::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  */
132 const 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  */
141 const 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  */
163 NavGraph::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  */
186 NavGraph::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  */
201 NavGraph::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  */
216 NavGraph::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  */
231 NavGraph::closest_node_to_with_unconnected(const std::string &node_name,
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  */
290 NavGraph::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  */
329 NavGraph::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  */
353 NavGraph::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  */
385 std::vector<NavGraphNode>
386 NavGraph::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  */
407 bool
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  */
418 bool
419 NavGraph::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  */
432 bool
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  */
444 bool
445 NavGraph::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  */
459 void
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
473 template <class T>
474 typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
475 almost_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  */
498 void
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  */
518 void
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  */
531 void
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);
565  add_edge(new_edge_1, EDGE_SPLIT_INTERSECTION);
566  add_edge(new_edge_2, EDGE_SPLIT_INTERSECTION);
567  add_edge(new_edge_3, EDGE_SPLIT_INTERSECTION);
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  */
583 void
584 NavGraph::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  */
611 void
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  */
628 void
629 NavGraph::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  */
651 void
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  */
676 void
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  */
695 void
696 NavGraph::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  */
715 void
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  */
732 void
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  */
746 void
748 {
749  nodes_.clear();
750  edges_.clear();
751  default_properties_.clear();
753 }
754 
755 /** Get all default properties.
756  * @return property map
757  */
758 const 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  */
768 bool
769 NavGraph::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  */
778 std::string
779 NavGraph::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  */
793 float
794 NavGraph::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  */
803 int
804 NavGraph::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  */
813 bool
814 NavGraph::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  */
823 void
824 NavGraph::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  */
833 void
834 NavGraph::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  */
843 void
844 NavGraph::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  */
853 void
854 NavGraph::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  */
863 void
864 NavGraph::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  */
874 void
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  */
888 std::vector<std::string>
889 NavGraph::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  */
930 void
931 NavGraph::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. */
943 void
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  */
1000 NavGraph::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  */
1033 NavGraph::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  */
1122 float
1123 NavGraph::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. */
1129 void
1130 NavGraph::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 
1149 void
1150 NavGraph::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 
1180 void
1181 NavGraph::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 
1309 void
1310 NavGraph::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  */
1409 void
1410 NavGraph::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  */
1439 std::string
1440 NavGraph::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  */
1456 std::string
1457 NavGraph::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  */
1474 void
1476 {
1477  change_listeners_.push_back(listener);
1478 }
1479 
1480 /** Remove a change listener.
1481  * @param listener listener to remove
1482  */
1483 void
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  */
1497 void
1499 {
1500  notifications_enabled_ = enabled;
1501 }
1502 
1503 /** Notify all listeners of a change. */
1504 void
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() throw() = 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::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_edge.h:96
const NavGraphNode & to_node() const
Get edge target node.
Definition: navgraph_edge.h:78
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
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 std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
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
void notify_of_change()
Notify all listeners of a change.
Definition: navgraph.cpp:1505
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
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