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 <navgraph/yaml_navgraph.h>
24 #include <navgraph/navgraph.h>
25 #include <core/exception.h>
26 
27 #include <yaml-cpp/yaml.h>
28 #include <fstream>
29 
30 namespace fawkes {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 /** Read topological map node from YAML iterator.
36  * @param n iterator to node representing a topological map graph node
37  * @param node node to fill
38  */
39 static void
40 operator >> (const YAML::Node& n, NavGraphNode &node) {
41 #ifdef HAVE_YAMLCPP_0_5
42  const std::string name = n["name"].as<std::string>();
43 #else
44  std::string name;
45  n["name"] >> name;
46 #endif
47 
48 #ifdef HAVE_OLD_YAMLCPP
49  if (n.GetType() != YAML::CT_MAP) {
50 #else
51  if (n.Type() != YAML::NodeType::Map) {
52 #endif
53  throw Exception("Node %s is not a map!?", name.c_str());
54  }
55 
56  try {
57  if (n["pos"].size() != 2) {
58  throw Exception("Invalid position for node %s, "
59  "must be list of [x,y] coordinates", name.c_str());
60  }
61  float x, y;
62 #ifdef HAVE_YAMLCPP_0_5
63  x = n["pos"][0].as<float>();
64  y = n["pos"][1].as<float>();
65 #else
66  n["pos"][0] >> x;
67  n["pos"][1] >> y;
68 #endif
69 
70  node.set_x(x);
71  node.set_y(y);
72  } catch (YAML::Exception &e) {
73  throw fawkes::Exception("Failed to parse node: %s", e.what());
74  }
75 
76 #ifdef HAVE_OLD_YAMLCPP
77  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/unconnected") {
78 #else
79  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/unconnected") {
80 #endif
81  node.set_unconnected(true);
82  }
83 
84  bool has_properties = true;
85  try {
86 #ifdef HAVE_YAMLCPP_0_5
87  has_properties = n["properties"].IsDefined();
88 #else
89  has_properties = (n.FindValue("properties") != NULL);
90 #endif
91  } catch (YAML::Exception &e) {
92  has_properties = false;
93  }
94 
95  if (has_properties) {
96  try {
97  const YAML::Node &props = n["properties"];
98  if (props.Type() != YAML::NodeType::Sequence) {
99  throw Exception("Properties must be a list");
100  }
101 
102  std::map<std::string, std::string> properties;
103 
104 #ifdef HAVE_YAMLCPP_0_5
105  YAML::const_iterator p;
106 #else
107  YAML::Iterator p;
108 #endif
109  for (p = props.begin(); p != props.end(); ++p) {
110 #ifdef HAVE_OLD_YAMLCPP
111  if (p->GetType() == YAML::CT_SCALAR) {
112 #else
113  if (p->Type() == YAML::NodeType::Scalar) {
114 #endif
115 #ifdef HAVE_YAMLCPP_0_5
116  std::string key = p->as<std::string>();
117 #else
118  std::string key;
119  *p >> key;
120 #endif
121  node.set_property(key, "true");
122 #ifdef HAVE_OLD_YAMLCPP
123  } else if (p->GetType() == YAML::CT_MAP) {
124 #else
125  } else if (p->Type() == YAML::NodeType::Map) {
126 #endif
127 #ifdef HAVE_YAMLCPP_0_5
128  for (YAML::const_iterator i = p->begin(); i != p->end(); ++i) {
129  std::string key = i->first.as<std::string>();
130  std::string value = i->second.as<std::string>();
131 #else
132  for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
133  std::string key, value;
134  i.first() >> key;
135  i.second() >> value;
136 #endif
137  node.set_property(key, value);
138  }
139  } else {
140  throw Exception("Invalid property for node '%s'", name.c_str());
141  }
142  }
143  } catch (YAML::Exception &e) {
144  throw Exception("Failed to read propery of %s: %s",
145  name.c_str(), e.what());
146  } // ignored
147  }
148 
149  node.set_name(name);
150 }
151 
152 /** Read topological map edge from YAML iterator.
153  * @param n iterator to node representing a topological map graph edge
154  * @param edge edge to fill
155  */
156 static void
157 operator >> (const YAML::Node& n, NavGraphEdge &edge) {
158 #ifdef HAVE_OLD_YAMLCPP
159  if (n.GetType() != YAML::CT_SEQUENCE || n.size() != 2) {
160 #else
161  if (n.Type() != YAML::NodeType::Sequence || n.size() != 2) {
162 #endif
163  throw Exception("Invalid edge");
164  }
165  std::string from, to;
166 #ifdef HAVE_YAMLCPP_0_5
167  from = n[0].as<std::string>();
168  to = n[1].as<std::string>();
169 #else
170  n[0] >> from;
171  n[1] >> to;
172 #endif
173 
174  edge.set_from(from);
175  edge.set_to(to);
176 
177 #ifdef HAVE_OLD_YAMLCPP
178  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/dir") {
179 #else
180  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/dir") {
181 #endif
182  edge.set_directed(true);
183  }
184 
185 #ifdef HAVE_OLD_YAMLCPP
186  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/allow-intersection") {
187 #else
188  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/allow-intersection") {
189 #endif
190  edge.set_property("insert-mode", "force");
191  }
192 
193 #ifdef HAVE_OLD_YAMLCPP
194  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/no-intersection") {
195 #else
196  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/no-intersection") {
197 #endif
198  edge.set_property("insert-mode", "no-intersection");
199  }
200 
201 #ifdef HAVE_OLD_YAMLCPP
202  if (n.GetTag() == "tag:fawkesrobotics.org,navgraph/split-intersection") {
203 #else
204  if (n.Tag() == "tag:fawkesrobotics.org,navgraph/split-intersection") {
205 #endif
206  edge.set_property("insert-mode", "split-intersection");
207  }
208 
209 }
210 
211 /** Read default properties for graph from YAML node.
212  * @param graph the graph to assign the properties to
213  * @param doc the root document of the YAML graph definition
214  */
215 void
216 read_default_properties(NavGraph *graph, YAML::Node &doc)
217 {
218  bool has_properties = true;
219  try {
220 #ifdef HAVE_YAMLCPP_0_5
221  has_properties = doc["default-properties"].IsDefined();
222 #else
223  has_properties = (doc.FindValue("default-properties") != NULL);
224 #endif
225  } catch (YAML::Exception &e) {
226  has_properties = false;
227  }
228 
229  if (has_properties) {
230  try {
231  const YAML::Node &props = doc["default-properties"];
232  if (props.Type() != YAML::NodeType::Sequence) {
233  throw Exception("Default properties must be a list");
234  }
235 
236  std::map<std::string, std::string> properties;
237 
238 #ifdef HAVE_YAMLCPP_0_5
239  YAML::const_iterator p;
240 #else
241  YAML::Iterator p;
242 #endif
243  for (p = props.begin(); p != props.end(); ++p) {
244 #ifdef HAVE_OLD_YAMLCPP
245  if (p->GetType() == YAML::CT_SCALAR) {
246 #else
247  if (p->Type() == YAML::NodeType::Scalar) {
248 #endif
249 #ifdef HAVE_YAMLCPP_0_5
250  std::string key = p->as<std::string>();
251 #else
252  std::string key;
253  *p >> key;
254 #endif
255  properties[key] = "true";
256 #ifdef HAVE_OLD_YAMLCPP
257  } else if (p->GetType() == YAML::CT_MAP) {
258 #else
259  } else if (p->Type() == YAML::NodeType::Map) {
260 #endif
261 #ifdef HAVE_YAMLCPP_0_5
262  for (YAML::const_iterator i = p->begin(); i != p->end(); ++i) {
263  std::string key = i->first.as<std::string>();
264  std::string value = i->second.as<std::string>();
265 #else
266  for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
267  std::string key, value;
268  i.first() >> key;
269  i.second() >> value;
270 #endif
271  properties[key] = value;
272  }
273  } else {
274  throw Exception("Invalid default property for graph %s", graph->name().c_str());
275  }
276  }
277 
278  graph->set_default_properties(properties);
279  } catch (YAML::Exception &e) {
280  throw Exception("Failed to read default property of graph %s: %s",
281  graph->name().c_str(), e.what());
282  }
283  }
284 }
285 
286 /** Load topological map graph stored in RCSoft format.
287  * @param filename path to the file to read
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)
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();
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 
382 /** Save navgraph to YAML file.
383  * @param filename name of file to save to
384  * @param graph graph to save to
385  */
386 void
387 save_yaml_navgraph(std::string filename, NavGraph *graph)
388 {
389  if (filename[0] != '/') {
390  filename = std::string(CONFDIR) + "/" + filename;
391  }
392 
393  YAML::Emitter out;
394  out << YAML::TrueFalseBool
395  << YAML::BeginMap
396  << YAML::Key << "graph-name"
397  << YAML::Value << graph->name();
398 
399  const std::map<std::string, std::string> &def_props = graph->default_properties();
400  if (! def_props.empty()) {
401  out << YAML::Key << "default-properties"
402  << YAML::Value << YAML::BeginSeq;
403  for (auto &p : def_props) {
404  out << YAML::BeginMap
405  << YAML::Key << p.first
406  << YAML::Value << p.second
407  << YAML::EndMap;
408  }
409  out << YAML::EndSeq;
410  }
411 
412  out << YAML::Key << "nodes"
413  << YAML::Value << YAML::BeginSeq;
414 
415  const std::vector<NavGraphNode> &nodes = graph->nodes();
416  for (const NavGraphNode &node : nodes) {
417  out << YAML::BeginMap
418  << YAML::Key << "name"
419  << YAML::Value << node.name()
420  << YAML::Key << "pos"
421  << YAML::Value << YAML::Flow << YAML::BeginSeq << node.x() << node.y() << YAML::EndSeq;
422 
423  const std::map<std::string, std::string> &props = node.properties();
424  if (! props.empty()) {
425  out << YAML::Key << "properties"
426  << YAML::Value << YAML::BeginSeq;
427  for (auto &p : props) {
428  out << YAML::BeginMap
429  << YAML::Key << p.first
430  << YAML::Value << p.second
431  << YAML::EndMap;
432  }
433  out << YAML::EndSeq;
434  }
435 
436  out << YAML::EndMap;
437  }
438  out << YAML::EndSeq
439  << YAML::Key << "connections"
440  << YAML::Value << YAML::BeginSeq;
441 
442  const std::vector<NavGraphEdge> &edges = graph->edges();
443  for (const NavGraphEdge &edge : edges) {
444  if (edge.is_directed()) out << YAML::LocalTag("dir");
445  out << YAML::Flow << YAML::BeginSeq << edge.from() << edge.to() << YAML::EndSeq;
446  }
447 
448  out << YAML::EndSeq
449  << YAML::EndMap;
450 
451  std::ofstream s(filename);
452  s << "%YAML 1.2" << std::endl
453  << "%TAG ! tag:fawkesrobotics.org,navgraph/" << std::endl
454  << "---" << std::endl
455  << out.c_str();
456 }
457 
458 } // 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.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
Definition: navgraph.cpp:743
Fawkes library namespace.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
Topological map graph.
Definition: navgraph.h:57
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.
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:461
void save_yaml_navgraph(std::string filename, NavGraph *graph)
Save navgraph to YAML file.
void set_y(float y)
Set Y position.
std::string property(const std::string &prop) const
Get specified property as string.
void set_to(const std::string &to)
Set target node name.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:124
void set_from(const std::string &from)
Set originating node name.
const std::map< std::string, std::string > & properties() const
Get all properties.
Definition: navgraph_node.h:88
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
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:592
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
add nodes no matter what (be careful)
Definition: navgraph.h:70
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
Definition: navgraph.h:73
void set_property(const std::string &property, const std::string &value)
Set property.
static void operator>>(const YAML::Node &n, NavGraphNode &node)
Read topological map node from YAML iterator.
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
Definition: navgraph.cpp:540
NavGraph * load_yaml_navgraph(std::string filename)
Load topological map graph stored in RCSoft format.
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:49
void set_directed(bool directed)
Set directed state.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:134
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:59
Only add edge if it does not intersect with any existing edge.
Definition: navgraph.h:71
void set_x(float x)
Set X position.
Topological graph edge.
Definition: navgraph_edge.h:39
void set_name(const std::string &name)
Set name of node.
Topological graph node.
Definition: navgraph_node.h:38
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:54
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1387
void read_default_properties(NavGraph *graph, YAML::Node &doc)
Read default properties for graph from YAML node.
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
Definition: navgraph.cpp:526