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