26 #include "rcsoft_map_graph.h" 27 #include <utils/misc/string_conversions.h> 28 #include <core/exception.h> 30 #include <libxml++/libxml++.h> 33 using namespace xmlpp;
50 RCSoftMapGraph::RCSoftMapGraph(std::string filename)
52 __dom =
new DomParser();
54 __dom->set_substitute_entities();
55 __dom->parse_file(filename);
56 __root = __dom->get_document()->get_root_node();
57 if ( __root == NULL ) {
66 RCSoftMapGraph::~RCSoftMapGraph()
81 RCSoftMapGraph::get_node_text(xmlpp::Node *root, std::string subnode)
87 sntext = subnode +
"/text()";
90 NodeSet
set = root->find(sntext);
91 if (
set.size() == 0 ) {
92 throw Exception(
"No %s sub-node for node %s", subnode.c_str(), root->get_name().c_str());
94 const TextNode *value_node =
dynamic_cast<const TextNode *
>(
set[0]);
96 throw Exception(
"Not a text node at %s sub-node of node %s", subnode.c_str(),
97 root->get_name().c_str());
100 std::string s = value_node->get_content();
101 return StringConversions::trim(s);
110 RCSoftMapGraph::get_node(xmlpp::Node *node)
112 std::string name = get_node_text(node,
"NodeName");
113 float x = get_node_float(node,
"x");
114 float y = get_node_float(node,
"y");
115 std::vector<std::string> properties;
116 NodeSet prop_set = node->find(
"Property");
117 for (NodeSet::iterator i = prop_set.begin(); i != prop_set.end(); ++i) {
118 properties.push_back(get_node_text(*i));
121 std::vector<std::string> aliases;
122 NodeSet alias_set = node->find(
"Alias");
123 for (NodeSet::iterator i = alias_set.begin(); i != alias_set.end(); ++i) {
124 aliases.push_back(get_node_text(*i));
127 std::vector<std::string> children;
128 NodeSet child_set = node->find(
"Child");
129 for (NodeSet::iterator i = child_set.begin(); i != child_set.end(); ++i) {
130 children.push_back(get_node_text(*i));
133 return RCSoftMapNode(name, x, y, children, properties, aliases);
143 RCSoftMapGraph::get_node_float(xmlpp::Node *root, std::string subnode)
145 std::string s = get_node_text(root, subnode);
146 return StringConversions::to_float(s);
152 RCSoftMapGraph::parse_graph()
155 __graph_name = get_node_text(__root,
"/Graph/GraphName");
158 NodeSet rootnode_set = __root->find(
"/Graph/Root/Node");
159 if ( rootnode_set.size() != 1 ) {
162 __root_node = get_node(rootnode_set[0]);
163 __nodes.push_back(__root_node);
166 NodeSet node_set = __root->find(
"/Graph/Node");
167 if ( node_set.size() == 0 ) {
170 for (NodeSet::iterator i = node_set.begin(); i != node_set.end(); ++i) {
171 __nodes.push_back(get_node(*i));
180 RCSoftMapGraph::graph_name()
190 RCSoftMapGraph::root_node()
199 std::vector<fawkes::RCSoftMapNode>
200 RCSoftMapGraph::nodes()
212 RCSoftMapGraph::node(std::string name_or_alias)
214 std::vector<fawkes::RCSoftMapNode>::iterator i;
215 for (i = __nodes.begin(); i != __nodes.end(); ++i) {
216 if ( (i->name() == name_or_alias) || i->has_alias(name_or_alias)) {
229 std::vector<fawkes::RCSoftMapNode>
230 RCSoftMapGraph::search_nodes(std::string property)
232 if (property ==
"") {
235 std::vector<fawkes::RCSoftMapNode> rv;
237 std::vector<fawkes::RCSoftMapNode>::iterator i;
238 for (i = __nodes.begin(); i != __nodes.end(); ++i) {
239 if ( i->has_property(property) ) {
256 RCSoftMapGraph::closest_node(
float pos_x,
float pos_y, std::string property)
258 std::vector<fawkes::RCSoftMapNode> nodes = search_nodes(property);
260 float min_dist = HUGE;
262 std::vector<fawkes::RCSoftMapNode>::iterator i;
263 std::vector<fawkes::RCSoftMapNode>::iterator elem = nodes.begin();
264 for (i = nodes.begin(); i != nodes.end(); ++i) {
265 float dx = i->x() - pos_x;
266 float dy = i->y() - pos_y;
267 float dist = sqrtf(dx * dx + dy * dy);
268 if (sqrtf(dx * dx + dy * dy) < min_dist) {
274 if (elem == nodes.end()) {
RCSoft map node representation.
Fawkes library namespace.
Base class for exceptions in Fawkes.