23 #include <navgraph/yaml_navgraph.h> 24 #include <navgraph/navgraph.h> 25 #include <core/exception.h> 27 #include <yaml-cpp/yaml.h> 41 #ifdef HAVE_YAMLCPP_0_5 42 const std::string name = n[
"name"].as<std::string>();
48 #ifdef HAVE_OLD_YAMLCPP 49 if (n.GetType() != YAML::CT_MAP) {
51 if (n.Type() != YAML::NodeType::Map) {
53 throw Exception(
"Node %s is not a map!?", name.c_str());
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());
62 #ifdef HAVE_YAMLCPP_0_5 63 x = n[
"pos"][0].as<
float>();
64 y = n[
"pos"][1].as<
float>();
72 }
catch (YAML::Exception &e) {
76 #ifdef HAVE_OLD_YAMLCPP 77 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/unconnected") {
79 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/unconnected") {
84 bool has_properties =
true;
86 #ifdef HAVE_YAMLCPP_0_5 87 has_properties = n[
"properties"].IsDefined();
89 has_properties = (n.FindValue(
"properties") != NULL);
91 }
catch (YAML::Exception &e) {
92 has_properties =
false;
97 const YAML::Node &props = n[
"properties"];
98 if (props.Type() != YAML::NodeType::Sequence) {
99 throw Exception(
"Properties must be a list");
102 std::map<std::string, std::string> properties;
104 #ifdef HAVE_YAMLCPP_0_5 105 YAML::const_iterator p;
109 for (p = props.begin(); p != props.end(); ++p) {
110 #ifdef HAVE_OLD_YAMLCPP 111 if (p->GetType() == YAML::CT_SCALAR) {
113 if (p->Type() == YAML::NodeType::Scalar) {
115 #ifdef HAVE_YAMLCPP_0_5 116 std::string key = p->as<std::string>();
122 #ifdef HAVE_OLD_YAMLCPP 123 }
else if (p->GetType() == YAML::CT_MAP) {
125 }
else if (p->Type() == YAML::NodeType::Map) {
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>();
132 for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
133 std::string key, value;
140 throw Exception(
"Invalid property for node '%s'", name.c_str());
143 }
catch (YAML::Exception &e) {
144 throw Exception(
"Failed to read propery of %s: %s",
145 name.c_str(), e.what());
158 #ifdef HAVE_OLD_YAMLCPP 159 if (n.GetType() != YAML::CT_SEQUENCE || n.size() != 2) {
161 if (n.Type() != YAML::NodeType::Sequence || n.size() != 2) {
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>();
177 #ifdef HAVE_OLD_YAMLCPP 178 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/dir") {
180 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/dir") {
185 #ifdef HAVE_OLD_YAMLCPP 186 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/allow-intersection") {
188 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/allow-intersection") {
193 #ifdef HAVE_OLD_YAMLCPP 194 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/no-intersection") {
196 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/no-intersection") {
201 #ifdef HAVE_OLD_YAMLCPP 202 if (n.GetTag() ==
"tag:fawkesrobotics.org,navgraph/split-intersection") {
204 if (n.Tag() ==
"tag:fawkesrobotics.org,navgraph/split-intersection") {
218 bool has_properties =
true;
220 #ifdef HAVE_YAMLCPP_0_5 221 has_properties = doc[
"default-properties"].IsDefined();
223 has_properties = (doc.FindValue(
"default-properties") != NULL);
225 }
catch (YAML::Exception &e) {
226 has_properties =
false;
229 if (has_properties) {
231 const YAML::Node &props = doc[
"default-properties"];
232 if (props.Type() != YAML::NodeType::Sequence) {
233 throw Exception(
"Default properties must be a list");
236 std::map<std::string, std::string> properties;
238 #ifdef HAVE_YAMLCPP_0_5 239 YAML::const_iterator p;
243 for (p = props.begin(); p != props.end(); ++p) {
244 #ifdef HAVE_OLD_YAMLCPP 245 if (p->GetType() == YAML::CT_SCALAR) {
247 if (p->Type() == YAML::NodeType::Scalar) {
249 #ifdef HAVE_YAMLCPP_0_5 250 std::string key = p->as<std::string>();
255 properties[key] =
"true";
256 #ifdef HAVE_OLD_YAMLCPP 257 }
else if (p->GetType() == YAML::CT_MAP) {
259 }
else if (p->Type() == YAML::NodeType::Map) {
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>();
266 for (YAML::Iterator i = p->begin(); i != p->end(); ++i) {
267 std::string key, value;
271 properties[key] = value;
274 throw Exception(
"Invalid default property for graph %s", graph->
name().c_str());
279 }
catch (YAML::Exception &e) {
280 throw Exception(
"Failed to read default property of graph %s: %s",
281 graph->
name().c_str(), e.what());
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") {
343 graph->
add_edge(edge, NavGraph::EDGE_FORCE);
344 }
else if (mode ==
"no-intersection") {
345 graph->
add_edge(edge, NavGraph::EDGE_NO_INTERSECTION);
346 }
else if (mode ==
"split-intersection") {
347 graph->
add_edge(edge, NavGraph::EDGE_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") {
389 if (filename[0] !=
'/') {
390 filename = std::string(CONFDIR) +
"/" + filename;
394 out << YAML::TrueFalseBool
396 << YAML::Key <<
"graph-name" 397 << YAML::Value << graph->
name();
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
412 out << YAML::Key <<
"nodes" 413 << YAML::Value << YAML::BeginSeq;
415 const std::vector<NavGraphNode> &nodes = graph->
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;
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
439 << YAML::Key <<
"connections" 440 << YAML::Value << YAML::BeginSeq;
442 const std::vector<NavGraphEdge> &edges = graph->
edges();
444 if (edge.
is_directed()) out << YAML::LocalTag(
"dir");
445 out << YAML::Flow << YAML::BeginSeq << edge.
from() << edge.
to() << YAML::EndSeq;
451 std::ofstream s(filename);
452 s <<
"%YAML 1.2" << std::endl
453 <<
"%TAG ! tag:fawkesrobotics.org,navgraph/" << std::endl
454 <<
"---" << 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.
void set_property(const std::string &property, const std::string &value)
Set property.
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
NavGraph * load_yaml_navgraph(std::string filename)
Load topological map graph stored in RCSoft format.
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.
void set_x(float x)
Set X position.
void set_name(const std::string &name)
Set name of node.
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.