Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rcosoft_map_graph.cpp - Map graph for storing pathplan information 00004 * 00005 * Created: Tue Jun 30 09:43:38 2009 (RoboCup 2009, Graz) 00006 * Copyright 2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 * $Id: rcsoft_map_graph.cpp 2826 2009-07-06 08:59:01Z tim $ 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include "rcsoft_map_graph.h" 00027 #include <utils/misc/string_conversions.h> 00028 #include <core/exception.h> 00029 00030 #include <libxml++/libxml++.h> 00031 #include <cmath> 00032 00033 using namespace xmlpp; 00034 00035 namespace fawkes { 00036 #if 0 /* just to make Emacs auto-indent happy */ 00037 } 00038 #endif 00039 00040 /** @class RCSoftMapGraph <utils/graph/rcsoft_map_graph.h> 00041 * Read RCSoft map graphs. 00042 * This class can be used to read and search map graphs of our old software 00043 * framework RCSoft. 00044 * @author Tim Niemueller 00045 */ 00046 00047 /** Constructor. 00048 * @param filename path to the file to read 00049 */ 00050 RCSoftMapGraph::RCSoftMapGraph(std::string filename) 00051 { 00052 __dom = new DomParser(); 00053 //dom->set_validate(); 00054 __dom->set_substitute_entities(); 00055 __dom->parse_file(filename); 00056 __root = __dom->get_document()->get_root_node(); 00057 if ( __root == NULL ) { 00058 throw Exception("Could not parse graph"); 00059 } 00060 00061 parse_graph(); 00062 } 00063 00064 00065 /** Destructor. */ 00066 RCSoftMapGraph::~RCSoftMapGraph() 00067 { 00068 delete __dom; 00069 } 00070 00071 00072 /** Get text content of a node. 00073 * @param root node from where to start the search 00074 * @param subnode optional subnode prefix, may not include the /text() suffix 00075 * for retrieval of the text! 00076 * @return text of the node 00077 * @exception Exception thrown if the node does not have the desired sub-node 00078 * or if the node is not a text node. 00079 */ 00080 std::string 00081 RCSoftMapGraph::get_node_text(xmlpp::Node *root, std::string subnode) 00082 { 00083 std::string sntext; 00084 if (subnode == "") { 00085 sntext = "text()"; 00086 } else { 00087 sntext = subnode + "/text()"; 00088 } 00089 00090 NodeSet set = root->find(sntext); 00091 if ( set.size() == 0 ) { 00092 throw Exception("No %s sub-node for node %s", subnode.c_str(), root->get_name().c_str()); 00093 } 00094 const TextNode *value_node = dynamic_cast<const TextNode *>(set[0]); 00095 if ( ! value_node ) { 00096 throw Exception("Not a text node at %s sub-node of node %s", subnode.c_str(), 00097 root->get_name().c_str()); 00098 } 00099 00100 std::string s = value_node->get_content(); 00101 return StringConversions::trim(s); 00102 } 00103 00104 00105 /** Get a map node from a XML node. 00106 * @param node XML node to parse as a map node. 00107 * @return map node representation 00108 */ 00109 RCSoftMapNode 00110 RCSoftMapGraph::get_node(xmlpp::Node *node) 00111 { 00112 std::string name = get_node_text(node, "NodeName"); 00113 float x = get_node_float(node, "x"); 00114 float y = get_node_float(node, "y"); 00115 std::vector<std::string> properties; 00116 NodeSet prop_set = node->find("Property"); 00117 for (NodeSet::iterator i = prop_set.begin(); i != prop_set.end(); ++i) { 00118 properties.push_back(get_node_text(*i)); 00119 } 00120 00121 std::vector<std::string> aliases; 00122 NodeSet alias_set = node->find("Alias"); 00123 for (NodeSet::iterator i = alias_set.begin(); i != alias_set.end(); ++i) { 00124 aliases.push_back(get_node_text(*i)); 00125 } 00126 00127 std::vector<std::string> children; 00128 NodeSet child_set = node->find("Child"); 00129 for (NodeSet::iterator i = child_set.begin(); i != child_set.end(); ++i) { 00130 children.push_back(get_node_text(*i)); 00131 } 00132 00133 return RCSoftMapNode(name, x, y, children, properties, aliases); 00134 } 00135 00136 /** Get node content as a float. 00137 * @param root node from where to start the search 00138 * @param subnode optional subnode prefix, may not include the /text() suffix 00139 * for retrieval of the text! 00140 * @return text of node converted to a float 00141 */ 00142 float 00143 RCSoftMapGraph::get_node_float(xmlpp::Node *root, std::string subnode) 00144 { 00145 std::string s = get_node_text(root, subnode); 00146 return StringConversions::to_float(s); 00147 } 00148 00149 00150 /** Parse the graph. */ 00151 void 00152 RCSoftMapGraph::parse_graph() 00153 { 00154 // get graph name 00155 __graph_name = get_node_text(__root, "/Graph/GraphName"); 00156 00157 // get nodes 00158 NodeSet rootnode_set = __root->find("/Graph/Root/Node"); 00159 if ( rootnode_set.size() != 1 ) { 00160 throw Exception("No root node defined"); 00161 } 00162 __root_node = get_node(rootnode_set[0]); 00163 __nodes.push_back(__root_node); 00164 00165 // get nodes 00166 NodeSet node_set = __root->find("/Graph/Node"); 00167 if ( node_set.size() == 0 ) { 00168 throw Exception("No nodes defined"); 00169 } 00170 for (NodeSet::iterator i = node_set.begin(); i != node_set.end(); ++i) { 00171 __nodes.push_back(get_node(*i)); 00172 } 00173 } 00174 00175 00176 /** Get graph name. 00177 * @return name of the graph. 00178 */ 00179 std::string 00180 RCSoftMapGraph::graph_name() 00181 { 00182 return __graph_name; 00183 } 00184 00185 00186 /** Get root node. 00187 * @return root node 00188 */ 00189 fawkes::RCSoftMapNode 00190 RCSoftMapGraph::root_node() 00191 { 00192 return __root_node; 00193 } 00194 00195 00196 /** Get all parsed nodes. 00197 * @return vector of nodes 00198 */ 00199 std::vector<fawkes::RCSoftMapNode> 00200 RCSoftMapGraph::nodes() 00201 { 00202 return __nodes; 00203 } 00204 00205 00206 /** Get node with given name or alias. 00207 * @param name_or_alias name or alias to search for 00208 * @return node with the given name or alias, or an invalid node if the 00209 * node could not be found. 00210 */ 00211 RCSoftMapNode 00212 RCSoftMapGraph::node(std::string name_or_alias) 00213 { 00214 std::vector<fawkes::RCSoftMapNode>::iterator i; 00215 for (i = __nodes.begin(); i != __nodes.end(); ++i) { 00216 if ( (i->name() == name_or_alias) || i->has_alias(name_or_alias)) { 00217 return *i; 00218 } 00219 } 00220 return RCSoftMapNode(); 00221 } 00222 00223 00224 /** Search nodes for specific property. 00225 * Searches all nodes and returns the ones which have the specified property. 00226 * @param property property to search for 00227 * @return vector of nodes having the desired property 00228 */ 00229 std::vector<fawkes::RCSoftMapNode> 00230 RCSoftMapGraph::search_nodes(std::string property) 00231 { 00232 if (property == "") { 00233 return nodes(); 00234 } else { 00235 std::vector<fawkes::RCSoftMapNode> rv; 00236 00237 std::vector<fawkes::RCSoftMapNode>::iterator i; 00238 for (i = __nodes.begin(); i != __nodes.end(); ++i) { 00239 if ( i->has_property(property) ) { 00240 rv.push_back(*i); 00241 } 00242 } 00243 00244 return rv; 00245 } 00246 } 00247 00248 00249 /** Find node closest to a specified position. 00250 * @param pos_x X world coordinate of close point 00251 * @param pos_y Y world coordinate of close point 00252 * @param property an optional property that nodes must have to be considered 00253 * @return the closest node 00254 */ 00255 fawkes::RCSoftMapNode 00256 RCSoftMapGraph::closest_node(float pos_x, float pos_y, std::string property) 00257 { 00258 std::vector<fawkes::RCSoftMapNode> nodes = search_nodes(property); 00259 00260 float min_dist = HUGE; 00261 00262 std::vector<fawkes::RCSoftMapNode>::iterator i; 00263 std::vector<fawkes::RCSoftMapNode>::iterator elem = nodes.begin(); 00264 for (i = nodes.begin(); i != nodes.end(); ++i) { 00265 float dx = i->x() - pos_x; 00266 float dy = i->y() - pos_y; 00267 float dist = sqrtf(dx * dx + dy * dy); 00268 if (sqrtf(dx * dx + dy * dy) < min_dist) { 00269 min_dist = dist; 00270 elem = i; 00271 } 00272 } 00273 00274 if (elem == nodes.end()) { 00275 return RCSoftMapNode(); 00276 } else { 00277 return *elem; 00278 } 00279 } 00280 00281 } // end of namespace fawkes