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") {
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") {
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.
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...
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.
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.
Only add edge if it does not intersect with any existing edge.
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.