23#include <core/exception.h>
24#include <navgraph/navgraph.h>
25#include <navgraph/yaml_navgraph.h>
26#include <yaml-cpp/yaml.h>
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") {
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>();
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;
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());
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;
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) {
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) {
341 std::string mode = edge.
property(
"insert-mode");
342 if (mode ==
"force") {
344 }
else if (mode ==
"no-intersection") {
346 }
else if (mode ==
"split-intersection") {
356 const std::vector<NavGraphNode> &nodes = graph->
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") {
370 }
else if (ins_mode ==
"unconnected" || ins_mode ==
"UNCONNECTED") {
388 if (filename[0] !=
'/') {
389 filename = std::string(CONFDIR) +
"/" + filename;
393 out << YAML::TrueFalseBool << YAML::BeginMap << YAML::Key <<
"graph-name" << YAML::Value
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();
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();
430 out << YAML::LocalTag(
"dir");
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
Base class for exceptions in Fawkes.
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.
void set_from(const std::string &from)
Set originating node name.
const std::string & from() const
Get edge 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.
float x() const
Get X coordinate in global frame.
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.
const std::string & name() const
Get name of node.
float y() const
Get Y coordinate in global frame.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
void set_y(float y)
Set Y position.
void update_node(const NavGraphNode &node)
Update a given node.
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
void add_node(const NavGraphNode &node)
Add a node.
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
@ EDGE_SPLIT_INTERSECTION
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
@ EDGE_NO_INTERSECTION
Only add edge if it does not intersect.
@ EDGE_FORCE
add nodes no matter what (be careful)
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
std::string name() const
Get graph name.
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
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.