23 #include <core/exception.h>
24 #include <navgraph/navgraph.h>
25 #include <navgraph/yaml_navgraph.h>
26 #include <yaml-cpp/yaml.h>
37 operator>>(
const YAML::Node &n, NavGraphNode &node)
39 #ifdef HAVE_YAMLCPP_0_5
40 const std::string name = n[
"name"].as<std::string>();
46 #ifdef HAVE_OLD_YAMLCPP
47 if (n.GetType() != YAML::CT_MAP) {
49 if (n.Type() != YAML::NodeType::Map) {
51 throw Exception(
"Node %s is not a map!?", name.c_str());
55 if (n[
"pos"].size() != 2) {
56 throw Exception(
"Invalid position for node %s, "
57 "must be list of [x,y] coordinates",
61 #ifdef HAVE_YAMLCPP_0_5
62 x = n[
"pos"][0].as<
float>();
63 y = n[
"pos"][1].as<
float>();
71 }
catch (YAML::Exception &e) {
75 #ifdef HAVE_OLD_YAMLCPP
76 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/unconnected") {
78 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/unconnected") {
80 node.set_unconnected(
true);
83 bool has_properties =
true;
85 #ifdef HAVE_YAMLCPP_0_5
86 has_properties = n[
"properties"].IsDefined();
88 has_properties = (n.FindValue(
"properties") != NULL);
90 }
catch (YAML::Exception &e) {
91 has_properties =
false;
96 const YAML::Node &props = n[
"properties"];
97 if (props.Type() != YAML::NodeType::Sequence) {
98 throw Exception(
"Properties must be a list");
101 std::map<std::string, std::string> properties;
103 #ifdef HAVE_YAMLCPP_0_5
104 YAML::const_iterator p;
108 for (p = props.begin(); p != props.end(); ++p) {
109 #ifdef HAVE_OLD_YAMLCPP
110 if (p->GetType() == YAML::CT_SCALAR) {
112 if (p->Type() == YAML::NodeType::Scalar) {
114 #ifdef HAVE_YAMLCPP_0_5
115 std::string key = p->as<std::string>();
120 node.set_property(key,
"true");
121 #ifdef HAVE_OLD_YAMLCPP
122 }
else if (p->GetType() == YAML::CT_MAP) {
124 }
else if (p->Type() == YAML::NodeType::Map) {
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>();
131 for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
132 std::string key, value;
136 node.set_property(key, value);
139 throw Exception(
"Invalid property for node '%s'", name.c_str());
142 }
catch (YAML::Exception &e) {
143 throw Exception(
"Failed to read propery of %s: %s", name.c_str(), e.what());
157 #ifdef HAVE_OLD_YAMLCPP
158 if (n.GetType() != YAML::CT_SEQUENCE || n.size() != 2) {
160 if (n.Type() != YAML::NodeType::Sequence || n.size() != 2) {
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>();
176 #ifdef HAVE_OLD_YAMLCPP
177 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/dir") {
179 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/dir") {
184 #ifdef HAVE_OLD_YAMLCPP
185 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/allow-intersection") {
187 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/allow-intersection") {
192 #ifdef HAVE_OLD_YAMLCPP
193 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/no-intersection") {
195 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/no-intersection") {
200 #ifdef HAVE_OLD_YAMLCPP
201 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/split-intersection") {
203 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/split-intersection") {
216 bool has_properties =
true;
218 #ifdef HAVE_YAMLCPP_0_5
219 has_properties = doc[
"default-properties"].IsDefined();
221 has_properties = (doc.FindValue(
"default-properties") != NULL);
223 }
catch (YAML::Exception &e) {
224 has_properties =
false;
227 if (has_properties) {
229 const YAML::Node &props = doc[
"default-properties"];
230 if (props.Type() != YAML::NodeType::Sequence) {
231 throw Exception(
"Default properties must be a list");
234 std::map<std::string, std::string> properties;
236 #ifdef HAVE_YAMLCPP_0_5
237 YAML::const_iterator p;
241 for (p = props.begin(); p != props.end(); ++p) {
242 #ifdef HAVE_OLD_YAMLCPP
243 if (p->GetType() == YAML::CT_SCALAR) {
245 if (p->Type() == YAML::NodeType::Scalar) {
247 #ifdef HAVE_YAMLCPP_0_5
248 std::string key = p->as<std::string>();
253 properties[key] =
"true";
254 #ifdef HAVE_OLD_YAMLCPP
255 }
else if (p->GetType() == YAML::CT_MAP) {
257 }
else if (p->Type() == YAML::NodeType::Map) {
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>();
264 for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
265 std::string key, value;
269 properties[key] = value;
272 throw Exception(
"Invalid default property for graph %s", graph->name().c_str());
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(),
295 if (filename[0] !=
'/') {
296 filename = std::string(CONFDIR) +
"/" + filename;
300 #ifdef HAVE_YAMLCPP_0_5
301 if (!(doc = YAML::LoadFile(filename))) {
303 std::ifstream fin(filename.c_str());
304 YAML::Parser parser(fin);
305 if (!parser.GetNextDocument(doc)) {
310 #ifdef HAVE_YAMLCPP_0_5
311 std::string graph_name = doc[
"graph-name"].as<std::string>();
313 std::string graph_name;
314 doc[
"graph-name"] >> graph_name;
317 NavGraph *graph =
new NavGraph(graph_name);
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) {
325 for (YAML::Iterator n = ynodes.begin(); n != ynodes.end(); ++n) {
329 graph->add_node(node);
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) {
336 for (YAML::Iterator e = yedges.begin(); e != yedges.end(); ++e) {
340 if (edge.has_property(
"insert-mode")) {
341 std::string mode = edge.
property(
"insert-mode");
342 if (mode ==
"force") {
344 }
else if (mode ==
"no-intersection") {
346 }
else if (mode ==
"split-intersection") {
350 graph->add_edge(edge);
354 graph->calc_reachability(allow_multi_graph);
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") {
361 graph->connect_node_to_closest_node(n);
362 }
else if (ins_mode ==
"closest-edge" || ins_mode ==
"CLOSEST_EDGE") {
363 graph->connect_node_to_closest_edge(n);
364 }
else if (ins_mode ==
"closest-edge-or-node" || ins_mode ==
"CLOSEST_EDGE_OR_NODE") {
366 graph->connect_node_to_closest_edge(n);
367 }
catch (Exception &e) {
368 graph->connect_node_to_closest_node(n);
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);
388 if (filename[0] !=
'/') {
389 filename = std::string(CONFDIR) +
"/" + filename;
393 out << YAML::TrueFalseBool << YAML::BeginMap << YAML::Key <<
"graph-name" << YAML::Value
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;
405 out << YAML::Key <<
"nodes" << YAML::Value << YAML::BeginSeq;
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;
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;
425 out << YAML::EndSeq << YAML::Key <<
"connections" << YAML::Value << YAML::BeginSeq;
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");
441 out << YAML::Flow << YAML::BeginSeq << edge.from() << edge.to() << YAML::EndSeq;
444 out << YAML::EndSeq << YAML::EndMap;
446 std::ofstream s(filename);
447 s <<
"%YAML 1.2" << std::endl
448 <<
"%TAG ! tag:fawkesrobotics.org,navgraph/" << std::endl
449 <<
"---" << std::endl