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
30namespace 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 */
36static void
37operator>>(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 */
154static void
155operator>>(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 */
213void
214read_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 */
291NavGraph *
292load_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 */
385void
386save_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.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
void set_from(const std::string &from)
Set originating node name.
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
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.
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.
const std::map< std::string, std::string > & properties() const
Get all properties.
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
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.