Fawkes API  Fawkes Development Version
rcsoft_map_graph.cpp
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