Fawkes API  Fawkes Development Version
yaml_navgraph.cpp
1 
2 /***************************************************************************
3  * yaml_navgraph.cpp - Nav graph stored in a YAML file
4  *
5  * Created: Fri Sep 21 18:37:16 2012
6  * Copyright 2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #include <core/exception.h>
24 #include <navgraph/navgraph.h>
25 #include <navgraph/yaml_navgraph.h>
26 #include <yaml-cpp/yaml.h>
27 
28 #include <fstream>
29 
30 namespace fawkes {
31 
32 /** Read topological map node from YAML iterator.
33  * @param n iterator to node representing a topological map graph node
34  * @param node node to fill
35  */
36 static void
37 operator>>(const YAML::Node &n, NavGraphNode &node)
38 {
39 #ifdef HAVE_YAMLCPP_0_5
40  const std::string name = n["name"].as<std::string>();
41 #else
42  std::string name;
43  n["name"] >> name;
44 #endif
45 
46 #ifdef HAVE_OLD_YAMLCPP
47  if (n.GetType() != YAML::CT_MAP) {
48 #else
49  if (n.Type() != YAML::NodeType::Map) {
50 #endif
51  throw Exception("Node %s is not a map!?", name.c_str());
52  }
53 
54  try {
55  if (n["pos"].size() != 2) {
56  throw Exception("Invalid position for node %s, "
57  "must be list of [x,y] coordinates",
58  name.c_str());
59  }
60  float x, y;
61 #ifdef HAVE_YAMLCPP_0_5
62  x = n["pos"][0].as<float>();
63  y = n["pos"][1].as<float>();
64 #else
65  n["pos"][0] >> x;
66  n["pos"][1] >> y;
67 #endif
68 
69  node.set_x(x);
70  node.set_y(y);
71  } catch (YAML::Exception &e) {
72  throw fawkes::Exception("Failed to parse node: %s", e.what());
73  }
74 
75 #ifdef HAVE_OLD_YAMLCPP
76  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/unconnected") {
77 #else
78  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/unconnected") {
79 #endif
80  node.set_unconnected(true);
81  }
82 
83  bool has_properties = true;
84  try {
85 #ifdef HAVE_YAMLCPP_0_5
86  has_properties = n["properties"].IsDefined();
87 #else
88  has_properties = (n.FindValue("properties") != NULL);
89 #endif
90  } catch (YAML::Exception &e) {
91  has_properties = false;
92  }
93 
94  if (has_properties) {
95  try {
96  const YAML::Node &props = n["properties"];
97  if (props.Type() != YAML::NodeType::Sequence) {
98  throw Exception("Properties must be a list");
99  }
100 
101  std::map<std::string, std::string> properties;
102 
103 #ifdef HAVE_YAMLCPP_0_5
104  YAML::const_iterator p;
105 #else
106  YAML::Iterator p;
107 #endif
108  for (p = props.begin(); p != props.end(); ++p) {
109 #ifdef HAVE_OLD_YAMLCPP
110  if (p->GetType() == YAML::CT_SCALAR) {
111 #else
112  if (p->Type() == YAML::NodeType::Scalar) {
113 #endif
114 #ifdef HAVE_YAMLCPP_0_5
115  std::string key = p->as<std::string>();
116 #else
117  std::string key;
118  *p >> key;
119 #endif
120  node.set_property(key, "true");
121 #ifdef HAVE_OLD_YAMLCPP
122  } else if (p->GetType() == YAML::CT_MAP) {
123 #else
124  } else if (p->Type() == YAML::NodeType::Map) {
125 #endif
126 #ifdef HAVE_YAMLCPP_0_5
127  for (YAML::const_iterator i = p->begin(); i != p->end(); ++i) {
128  std::string key = i->first.as<std::string>();
129  std::string value = i->second.as<std::string>();
130 #else
131  for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
132  std::string key, value;
133  i.first() >> key;
134  i.second() >> value;
135 #endif
136  node.set_property(key, value);
137  }
138  } else {
139  throw Exception("Invalid property for node '%s'", name.c_str());
140  }
141  }
142  } catch (YAML::Exception &e) {
143  throw Exception("Failed to read propery of %s: %s", name.c_str(), e.what());
144  } // ignored
145  }
146 
147  node.set_name(name);
148 }
149 
150 /** Read topological map edge from YAML iterator.
151  * @param n iterator to node representing a topological map graph edge
152  * @param edge edge to fill
153  */
154 static void
155 operator>>(const YAML::Node &n, NavGraphEdge &edge)
156 {
157 #ifdef HAVE_OLD_YAMLCPP
158  if (n.GetType() != YAML::CT_SEQUENCE || n.size() != 2) {
159 #else
160  if (n.Type() != YAML::NodeType::Sequence || n.size() != 2) {
161 #endif
162  throw Exception("Invalid edge");
163  }
164  std::string from, to;
165 #ifdef HAVE_YAMLCPP_0_5
166  from = n[0].as<std::string>();
167  to = n[1].as<std::string>();
168 #else
169  n[0] >> from;
170  n[1] >> to;
171 #endif
172 
173  edge.set_from(from);
174  edge.set_to(to);
175 
176 #ifdef HAVE_OLD_YAMLCPP
177  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/dir") {
178 #else
179  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/dir") {
180 #endif
181  edge.set_directed(true);
182  }
183 
184 #ifdef HAVE_OLD_YAMLCPP
185  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/allow-intersection") {
186 #else
187  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/allow-intersection") {
188 #endif
189  edge.set_property("insert-mode", "force");
190  }
191 
192 #ifdef HAVE_OLD_YAMLCPP
193  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/no-intersection") {
194 #else
195  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/no-intersection") {
196 #endif
197  edge.set_property("insert-mode", "no-intersection");
198  }
199 
200 #ifdef HAVE_OLD_YAMLCPP
201  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/split-intersection") {
202 #else
203  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/split-intersection") {
204 #endif
205  edge.set_property("insert-mode", "split-intersection");
206  }
207 }
208 
209 /** Read default properties for graph from YAML node.
210  * @param graph the graph to assign the properties to
211  * @param doc the root document of the YAML graph definition
212  */
213 void
214 read_default_properties(NavGraph *graph, YAML::Node &doc)
215 {
216  bool has_properties = true;
217  try {
218 #ifdef HAVE_YAMLCPP_0_5
219  has_properties = doc["default-properties"].IsDefined();
220 #else
221  has_properties = (doc.FindValue("default-properties") != NULL);
222 #endif
223  } catch (YAML::Exception &e) {
224  has_properties = false;
225  }
226 
227  if (has_properties) {
228  try {
229  const YAML::Node &props = doc["default-properties"];
230  if (props.Type() != YAML::NodeType::Sequence) {
231  throw Exception("Default properties must be a list");
232  }
233 
234  std::map<std::string, std::string> properties;
235 
236 #ifdef HAVE_YAMLCPP_0_5
237  YAML::const_iterator p;
238 #else
239  YAML::Iterator p;
240 #endif
241  for (p = props.begin(); p != props.end(); ++p) {
242 #ifdef HAVE_OLD_YAMLCPP
243  if (p->GetType() == YAML::CT_SCALAR) {
244 #else
245  if (p->Type() == YAML::NodeType::Scalar) {
246 #endif
247 #ifdef HAVE_YAMLCPP_0_5
248  std::string key = p->as<std::string>();
249 #else
250  std::string key;
251  *p >> key;
252 #endif
253  properties[key] = "true";
254 #ifdef HAVE_OLD_YAMLCPP
255  } else if (p->GetType() == YAML::CT_MAP) {
256 #else
257  } else if (p->Type() == YAML::NodeType::Map) {
258 #endif
259 #ifdef HAVE_YAMLCPP_0_5
260  for (YAML::const_iterator i = p->begin(); i != p->end(); ++i) {
261  std::string key = i->first.as<std::string>();
262  std::string value = i->second.as<std::string>();
263 #else
264  for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
265  std::string key, value;
266  i.first() >> key;
267  i.second() >> value;
268 #endif
269  properties[key] = value;
270  }
271  } else {
272  throw Exception("Invalid default property for graph %s", graph->name().c_str());
273  }
274  }
275 
276  graph->set_default_properties(properties);
277  } catch (YAML::Exception &e) {
278  throw Exception("Failed to read default property of graph %s: %s",
279  graph->name().c_str(),
280  e.what());
281  }
282  }
283 }
284 
285 /** Load topological map graph stored in RCSoft format.
286  * @param filename path to the file to read
287  * @param allow_multi_graph if true, allows multiple disconnected graph segments
288  * @return topological map graph read from file
289  * @exception Exception thrown on any error to read the graph file
290  */
291 NavGraph *
292 load_yaml_navgraph(std::string filename, bool allow_multi_graph)
293 {
294  //try to fix use of relative paths
295  if (filename[0] != '/') {
296  filename = std::string(CONFDIR) + "/" + filename;
297  }
298 
299  YAML::Node doc;
300 #ifdef HAVE_YAMLCPP_0_5
301  if (!(doc = YAML::LoadFile(filename))) {
302 #else
303  std::ifstream fin(filename.c_str());
304  YAML::Parser parser(fin);
305  if (!parser.GetNextDocument(doc)) {
306 #endif
307  throw fawkes::Exception("Failed to read YAML file %s", filename.c_str());
308  }
309 
310 #ifdef HAVE_YAMLCPP_0_5
311  std::string graph_name = doc["graph-name"].as<std::string>();
312 #else
313  std::string graph_name;
314  doc["graph-name"] >> graph_name;
315 #endif
316 
317  NavGraph *graph = new NavGraph(graph_name);
318 
319  read_default_properties(graph, doc);
320 
321  const YAML::Node &ynodes = doc["nodes"];
322 #ifdef HAVE_YAMLCPP_0_5
323  for (YAML::const_iterator n = ynodes.begin(); n != ynodes.end(); ++n) {
324 #else
325  for (YAML::Iterator n = ynodes.begin(); n != ynodes.end(); ++n) {
326 #endif
327  NavGraphNode node;
328  *n >> node;
329  graph->add_node(node);
330  }
331 
332  const YAML::Node &yedges = doc["connections"];
333 #ifdef HAVE_YAMLCPP_0_5
334  for (YAML::const_iterator e = yedges.begin(); e != yedges.end(); ++e) {
335 #else
336  for (YAML::Iterator e = yedges.begin(); e != yedges.end(); ++e) {
337 #endif
338  NavGraphEdge edge;
339  *e >> edge;
340  if (edge.has_property("insert-mode")) {
341  std::string mode = edge.property("insert-mode");
342  if (mode == "force") {
343  graph->add_edge(edge, NavGraph::EDGE_FORCE);
344  } else if (mode == "no-intersection") {
346  } else if (mode == "split-intersection") {
348  }
349  } else {
350  graph->add_edge(edge);
351  }
352  }
353 
354  graph->calc_reachability(allow_multi_graph);
355 
356  const std::vector<NavGraphNode> &nodes = graph->nodes();
357  for (const NavGraphNode &n : nodes) {
358  if (n.has_property("insert-mode")) {
359  std::string ins_mode = n.property("insert-mode");
360  if (ins_mode == "closest-node" || ins_mode == "CLOSEST_NODE") {
362  } else if (ins_mode == "closest-edge" || ins_mode == "CLOSEST_EDGE") {
364  } else if (ins_mode == "closest-edge-or-node" || ins_mode == "CLOSEST_EDGE_OR_NODE") {
365  try {
367  } catch (Exception &e) {
369  }
370  } else if (ins_mode == "unconnected" || ins_mode == "UNCONNECTED") {
371  NavGraphNode updated_n(n);
372  updated_n.set_unconnected(true);
373  graph->update_node(updated_n);
374  } // else NOT_CONNECTED, the default, do nothing here
375  }
376  }
377 
378  return graph;
379 }
380 
381 /** Save navgraph to YAML file.
382  * @param filename name of file to save to
383  * @param graph graph to save to
384  */
385 void
386 save_yaml_navgraph(std::string filename, NavGraph *graph)
387 {
388  if (filename[0] != '/') {
389  filename = std::string(CONFDIR) + "/" + filename;
390  }
391 
392  YAML::Emitter out;
393  out << YAML::TrueFalseBool << YAML::BeginMap << YAML::Key << "graph-name" << YAML::Value
394  << graph->name();
395 
396  const std::map<std::string, std::string> &def_props = graph->default_properties();
397  if (!def_props.empty()) {
398  out << YAML::Key << "default-properties" << YAML::Value << YAML::BeginSeq;
399  for (auto &p : def_props) {
400  out << YAML::BeginMap << YAML::Key << p.first << YAML::Value << p.second << YAML::EndMap;
401  }
402  out << YAML::EndSeq;
403  }
404 
405  out << YAML::Key << "nodes" << YAML::Value << YAML::BeginSeq;
406 
407  const std::vector<NavGraphNode> &nodes = graph->nodes();
408  for (const NavGraphNode &node : nodes) {
409  if (node.unconnected())
410  out << YAML::LocalTag("unconnected");
411  out << YAML::BeginMap << YAML::Key << "name" << YAML::Value << node.name() << YAML::Key << "pos"
412  << YAML::Value << YAML::Flow << YAML::BeginSeq << node.x() << node.y() << YAML::EndSeq;
413 
414  const std::map<std::string, std::string> &props = node.properties();
415  if (!props.empty()) {
416  out << YAML::Key << "properties" << YAML::Value << YAML::BeginSeq;
417  for (auto &p : props) {
418  out << YAML::BeginMap << YAML::Key << p.first << YAML::Value << p.second << YAML::EndMap;
419  }
420  out << YAML::EndSeq;
421  }
422 
423  out << YAML::EndMap;
424  }
425  out << YAML::EndSeq << YAML::Key << "connections" << YAML::Value << YAML::BeginSeq;
426 
427  const std::vector<NavGraphEdge> &edges = graph->edges();
428  for (const NavGraphEdge &edge : edges) {
429  if (edge.is_directed())
430  out << YAML::LocalTag("dir");
431  if (edge.has_property("insert-mode")) {
432  std::string insert_mode = edge.property("insert-mode");
433  if (insert_mode == "force") {
434  out << YAML::LocalTag("allow-intersection");
435  } else if (insert_mode == "no-intersection") {
436  out << YAML::LocalTag("no-intersection");
437  } else if (insert_mode == "split-intersection") {
438  out << YAML::LocalTag("split-intersection");
439  }
440  }
441  out << YAML::Flow << YAML::BeginSeq << edge.from() << edge.to() << YAML::EndSeq;
442  }
443 
444  out << YAML::EndSeq << YAML::EndMap;
445 
446  std::ofstream s(filename);
447  s << "%YAML 1.2" << std::endl
448  << "%TAG ! tag:fawkesrobotics.org,navgraph/" << std::endl
449  << "---" << std::endl
450  << out.c_str();
451 }
452 
453 } // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
Topological graph edge.
Definition: navgraph_edge.h:38
bool has_property(const std::string &property) const
Check if node has specified property.
bool is_directed() const
Check if edge is directed.
void set_property(const std::string &property, const std::string &value)
Set property.
void set_from(const std::string &from)
Set originating node name.
void set_to(const std::string &to)
Set target node name.
std::string property(const std::string &prop) const
Get specified property as string.
void set_directed(bool directed)
Set directed state.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
void set_x(float x)
Set X position.
void set_property(const std::string &property, const std::string &value)
Set property.
void set_name(const std::string &name)
Set name of node.
bool unconnected() const
Check if this node shall be unconnected.
Definition: navgraph_node.h:74
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
const std::map< std::string, std::string > & properties() const
Get all properties.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
void set_unconnected(bool unconnected)
Set unconnected state of the node.
void set_y(float y)
Set Y position.
Topological map graph.
Definition: navgraph.h:50
void update_node(const NavGraphNode &node)
Update a given node.
Definition: navgraph.cpp:716
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:584
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
Definition: navgraph.cpp:759
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:460
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
Definition: navgraph.cpp:834
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1410
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
@ EDGE_SPLIT_INTERSECTION
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
Definition: navgraph.h:65
@ EDGE_NO_INTERSECTION
Only add edge if it does not intersect.
Definition: navgraph.h:63
@ EDGE_FORCE
add nodes no matter what (be careful)
Definition: navgraph.h:62
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:142
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
Definition: navgraph.cpp:519
std::string name() const
Get graph name.
Definition: navgraph.cpp:124
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
Definition: navgraph.cpp:532
Fawkes library namespace.
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
static void operator>>(const YAML::Node &n, NavGraphNode &node)
Read topological map node from YAML iterator.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.
void read_default_properties(NavGraph *graph, YAML::Node &doc)
Read default properties for graph from YAML node.