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