Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
rcsoft_map_graph.cpp
1 
2 /***************************************************************************
3  * rcosoft_map_graph.cpp - Map graph for storing pathplan information
4  *
5  * Created: Tue Jun 30 09:43:38 2009 (RoboCup 2009, Graz)
6  * Copyright 2009 Tim Niemueller [www.niemueller.de]
7  *
8  * $Id: rcsoft_map_graph.cpp 2826 2009-07-06 08:59:01Z tim $
9  *
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version. A runtime exception applies to
16  * this software (see LICENSE.GPL_WRE file mentioned below for details).
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU Library General Public License for more details.
22  *
23  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24  */
25 
26 #include "rcsoft_map_graph.h"
27 #include <utils/misc/string_conversions.h>
28 #include <core/exception.h>
29 
30 #include <libxml++/libxml++.h>
31 #include <cmath>
32 
33 using namespace xmlpp;
34 
35 namespace fawkes {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class RCSoftMapGraph <utils/graph/rcsoft_map_graph.h>
41  * Read RCSoft map graphs.
42  * This class can be used to read and search map graphs of our old software
43  * framework RCSoft.
44  * @author Tim Niemueller
45  */
46 
47 /** Constructor.
48  * @param filename path to the file to read
49  */
50 RCSoftMapGraph::RCSoftMapGraph(std::string filename)
51 {
52  __dom = new DomParser();
53  //dom->set_validate();
54  __dom->set_substitute_entities();
55  __dom->parse_file(filename);
56  __root = __dom->get_document()->get_root_node();
57  if ( __root == NULL ) {
58  throw Exception("Could not parse graph");
59  }
60 
61  parse_graph();
62 }
63 
64 
65 /** Destructor. */
66 RCSoftMapGraph::~RCSoftMapGraph()
67 {
68  delete __dom;
69 }
70 
71 
72 /** Get text content of a node.
73  * @param root node from where to start the search
74  * @param subnode optional subnode prefix, may not include the /text() suffix
75  * for retrieval of the text!
76  * @return text of the node
77  * @exception Exception thrown if the node does not have the desired sub-node
78  * or if the node is not a text node.
79  */
80 std::string
81 RCSoftMapGraph::get_node_text(xmlpp::Node *root, std::string subnode)
82 {
83  std::string sntext;
84  if (subnode == "") {
85  sntext = "text()";
86  } else {
87  sntext = subnode + "/text()";
88  }
89 
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());
93  }
94  const TextNode *value_node = dynamic_cast<const TextNode *>(set[0]);
95  if ( ! value_node ) {
96  throw Exception("Not a text node at %s sub-node of node %s", subnode.c_str(),
97  root->get_name().c_str());
98  }
99 
100  std::string s = value_node->get_content();
101  return StringConversions::trim(s);
102 }
103 
104 
105 /** Get a map node from a XML node.
106  * @param node XML node to parse as a map node.
107  * @return map node representation
108  */
109 RCSoftMapNode
110 RCSoftMapGraph::get_node(xmlpp::Node *node)
111 {
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));
119  }
120 
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));
125  }
126 
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));
131  }
132 
133  return RCSoftMapNode(name, x, y, children, properties, aliases);
134 }
135 
136 /** Get node content as a float.
137  * @param root node from where to start the search
138  * @param subnode optional subnode prefix, may not include the /text() suffix
139  * for retrieval of the text!
140  * @return text of node converted to a float
141  */
142 float
143 RCSoftMapGraph::get_node_float(xmlpp::Node *root, std::string subnode)
144 {
145  std::string s = get_node_text(root, subnode);
146  return StringConversions::to_float(s);
147 }
148 
149 
150 /** Parse the graph. */
151 void
152 RCSoftMapGraph::parse_graph()
153 {
154  // get graph name
155  __graph_name = get_node_text(__root, "/Graph/GraphName");
156 
157  // get nodes
158  NodeSet rootnode_set = __root->find("/Graph/Root/Node");
159  if ( rootnode_set.size() != 1 ) {
160  throw Exception("No root node defined");
161  }
162  __root_node = get_node(rootnode_set[0]);
163  __nodes.push_back(__root_node);
164 
165  // get nodes
166  NodeSet node_set = __root->find("/Graph/Node");
167  if ( node_set.size() == 0 ) {
168  throw Exception("No nodes defined");
169  }
170  for (NodeSet::iterator i = node_set.begin(); i != node_set.end(); ++i) {
171  __nodes.push_back(get_node(*i));
172  }
173 }
174 
175 
176 /** Get graph name.
177  * @return name of the graph.
178  */
179 std::string
180 RCSoftMapGraph::graph_name()
181 {
182  return __graph_name;
183 }
184 
185 
186 /** Get root node.
187  * @return root node
188  */
190 RCSoftMapGraph::root_node()
191 {
192  return __root_node;
193 }
194 
195 
196 /** Get all parsed nodes.
197  * @return vector of nodes
198  */
199 std::vector<fawkes::RCSoftMapNode>
200 RCSoftMapGraph::nodes()
201 {
202  return __nodes;
203 }
204 
205 
206 /** Get node with given name or alias.
207  * @param name_or_alias name or alias to search for
208  * @return node with the given name or alias, or an invalid node if the
209  * node could not be found.
210  */
212 RCSoftMapGraph::node(std::string name_or_alias)
213 {
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)) {
217  return *i;
218  }
219  }
220  return RCSoftMapNode();
221 }
222 
223 
224 /** Search nodes for specific property.
225  * Searches all nodes and returns the ones which have the specified property.
226  * @param property property to search for
227  * @return vector of nodes having the desired property
228  */
229 std::vector<fawkes::RCSoftMapNode>
230 RCSoftMapGraph::search_nodes(std::string property)
231 {
232  if (property == "") {
233  return nodes();
234  } else {
235  std::vector<fawkes::RCSoftMapNode> rv;
236 
237  std::vector<fawkes::RCSoftMapNode>::iterator i;
238  for (i = __nodes.begin(); i != __nodes.end(); ++i) {
239  if ( i->has_property(property) ) {
240  rv.push_back(*i);
241  }
242  }
243 
244  return rv;
245  }
246 }
247 
248 
249 /** Find node closest to a specified position.
250  * @param pos_x X world coordinate of close point
251  * @param pos_y Y world coordinate of close point
252  * @param property an optional property that nodes must have to be considered
253  * @return the closest node
254  */
256 RCSoftMapGraph::closest_node(float pos_x, float pos_y, std::string property)
257 {
258  std::vector<fawkes::RCSoftMapNode> nodes = search_nodes(property);
259 
260  float min_dist = HUGE;
261 
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) {
269  min_dist = dist;
270  elem = i;
271  }
272  }
273 
274  if (elem == nodes.end()) {
275  return RCSoftMapNode();
276  } else {
277  return *elem;
278  }
279 }
280 
281 } // end of namespace fawkes