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) {
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") {
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
const std::string & from() const
Get edge originating node name.
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.
Fawkes library namespace.
void set_unconnected(bool unconnected)
Set unconnected state of the node.
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
bool is_directed() const
Check if edge is directed.
void add_node(const NavGraphNode &node)
Add a node.
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.
void set_from(const std::string &from)
Set originating node name.
const std::map< std::string, std::string > & properties() const
Get all properties.
void update_node(const NavGraphNode &node)
Update a given node.
std::string name() const
Get graph name.
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Base class for exceptions in Fawkes.
const std::string & to() const
Get edge target node name.
add nodes no matter what (be careful)
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.
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.
const std::string & name() const
Get name of node.
void set_directed(bool directed)
Set directed state.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
float y() const
Get Y coordinate in global frame.
Only add edge if it does not intersect.
void set_x(float x)
Set X position.
bool has_property(const std::string &property) const
Check if node has specified property.
void set_name(const std::string &name)
Set name of node.
bool unconnected() const
Check if this node shall be unconnected.
float x() const
Get X coordinate in global frame.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
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.