Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rcsoft_map_graph.cpp - Access the annotated map. 00004 * 00005 * Created: Mon Mar 21 17:23:57 2011 00006 * Copyright 2011 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "rcsoft_map_graph.h" 00024 00025 #include <utils/graph/rcsoft_map_graph.h> 00026 #include <core/exception.h> 00027 #include <eclipseclass.h> 00028 #include <cstdio> 00029 #include <cstring> 00030 #include <cstdlib> 00031 00032 /** @class fawkes::EclExternalRCSoftMapGraph 00033 * Wrapper class for using the RCSoftMapGraph in the implementation of 00034 * the external predicates. 00035 * @author Daniel Beck 00036 */ 00037 namespace fawkes 00038 { 00039 class EclExternalRCSoftMapGraph 00040 { 00041 public: 00042 /** Cosntructor. */ 00043 EclExternalRCSoftMapGraph() : m_map_graph(0) {} 00044 /** Destructor. */ 00045 ~EclExternalRCSoftMapGraph() { delete m_map_graph; } 00046 00047 /** Load map file. 00048 * @param file the map file 00049 */ 00050 void load( const char* file ) 00051 { 00052 m_map_graph = new RCSoftMapGraph( std::string(file) ); 00053 } 00054 00055 /** Query status. 00056 * @return true if a map file is loaded; false otherwise 00057 */ 00058 bool loaded() 00059 { 00060 return m_map_graph ? true : false; 00061 } 00062 00063 /** Access the RCSoftMapGraph instance. 00064 * @return the RCSoftMapGraph instance 00065 */ 00066 RCSoftMapGraph* map_graph() 00067 { 00068 return m_map_graph; 00069 } 00070 00071 private: 00072 RCSoftMapGraph* m_map_graph; 00073 00074 }; 00075 00076 } 00077 00078 using namespace std; 00079 using namespace fawkes; 00080 00081 EclExternalRCSoftMapGraph g_map_graph; 00082 00083 int 00084 p_map_graph_load() 00085 { 00086 if ( g_map_graph.loaded() ) 00087 { 00088 printf( "p_map_load(): map already loaded\n" ); 00089 return EC_fail; 00090 } 00091 00092 char* mapfile; 00093 if ( EC_succeed != EC_arg( 1 ).is_string( &mapfile) ) 00094 { 00095 printf( "p_map_load(): first argument is not a string\n" ); 00096 return EC_fail; 00097 } 00098 00099 try 00100 { 00101 g_map_graph.load( mapfile ); 00102 } 00103 catch ( Exception& e ) 00104 { 00105 e.print_trace(); 00106 return EC_fail; 00107 } 00108 00109 return EC_succeed; 00110 } 00111 00112 int 00113 p_map_graph_get_node_coords3() 00114 { 00115 if ( !g_map_graph.loaded() ) 00116 { 00117 printf( "p_map_get_node(): map file not loaded\n" ); 00118 return EC_fail; 00119 } 00120 00121 char* nodename; 00122 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) ) 00123 { 00124 printf( "p_map_get_node(): first argument is not a string\n" ); 00125 return EC_fail; 00126 } 00127 00128 RCSoftMapNode node = g_map_graph.map_graph()->node( string(nodename) ); 00129 00130 // x-coordinate 00131 if ( EC_succeed != EC_arg( 2 ).unify( EC_word( (double) node.x() ) ) ) 00132 { 00133 printf( "p_map_get_node(): could not bind return value\n" ); 00134 return EC_fail; 00135 } 00136 00137 // y-coordinate 00138 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( (double) node.y() ) ) ) 00139 { 00140 printf( "p_map_get_node(): could not bind return value\n" ); 00141 return EC_fail; 00142 } 00143 00144 return EC_succeed; 00145 } 00146 00147 int 00148 p_map_graph_get_node_coords4() 00149 { 00150 if ( EC_succeed != p_map_graph_get_node_coords3() ) 00151 { return EC_fail; } 00152 00153 char* nodename; 00154 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) ) 00155 { 00156 printf( "p_map_get_node(): first argument is not a string\n" ); 00157 return EC_fail; 00158 } 00159 00160 RCSoftMapNode node = g_map_graph.map_graph()->node( string(nodename) ); 00161 00162 // check for orientation property 00163 int result = EC_succeed; 00164 vector< string >::iterator pit; 00165 for ( pit = node.properties().begin(); 00166 pit != node.properties().end(); 00167 ++pit ) 00168 { 00169 if ( 0 == strncmp( (*pit).c_str(), "Orientation", 11 ) ) 00170 { 00171 double ori = atof( (*pit).substr( 11 ).c_str() ); 00172 result = EC_arg( 4 ).unify( EC_word( ori ) ); 00173 break; 00174 } 00175 } 00176 00177 if ( node.properties().end() == pit ) 00178 { result = EC_arg( 4 ).unify( EC_atom( (char*) "false" ) ); } 00179 00180 if ( EC_succeed != result) 00181 { return EC_fail; } 00182 00183 return EC_succeed; 00184 } 00185 00186 00187 int 00188 p_map_graph_get_nodes() 00189 { 00190 if ( !g_map_graph.loaded() ) 00191 { 00192 printf( "p_map_get_nodes(): map file not loaded\n" ); 00193 return EC_fail; 00194 } 00195 00196 vector< RCSoftMapNode > nodes = g_map_graph.map_graph()->nodes(); 00197 EC_word tail = nil(); 00198 00199 for ( vector< RCSoftMapNode >::iterator nit = nodes.begin(); 00200 nit != nodes.end(); 00201 ++nit ) 00202 { 00203 EC_word n = list( nit->name().c_str(), 00204 list( (double) nit->x(), 00205 list( (double) nit->y(), nil() ) ) ); 00206 tail = list( n, tail ); 00207 } 00208 00209 if ( EC_succeed != EC_arg( 1 ).unify( tail ) ) 00210 { 00211 printf( "p_map_get_nodes(): could not bind return value\n" ); 00212 return EC_fail; 00213 } 00214 00215 return EC_succeed; 00216 } 00217 00218 int 00219 p_map_graph_get_closest_node() 00220 { 00221 if ( !g_map_graph.loaded() ) 00222 { 00223 printf( "p_map_search_nodes(): map file not loaded\n" ); 00224 return EC_fail; 00225 } 00226 00227 double x; 00228 double y; 00229 if ( EC_succeed != EC_arg( 1 ).is_double( &x ) ) 00230 { 00231 printf( "p_map_graph_get_closest_node(): no x-coordinate given\n" ); 00232 return EC_fail; 00233 } 00234 00235 if ( EC_succeed != EC_arg( 2 ).is_double( &y ) ) 00236 { 00237 printf( "p_map_graph_get_closest_node(): no y-coordinate given\n" ); 00238 return EC_fail; 00239 } 00240 00241 RCSoftMapNode node = g_map_graph.map_graph()->closest_node( (float) x, 00242 (float) y, 00243 "" ); 00244 00245 if ( EC_succeed != EC_arg( 3 ).unify( EC_word( node.name().c_str() ) ) ) 00246 { 00247 printf( "p_map_graph_get_closest_node(): could not bind return value\n" ); 00248 return EC_fail; 00249 } 00250 00251 return EC_succeed; 00252 } 00253 00254 int 00255 p_map_graph_search_nodes() 00256 { 00257 if ( !g_map_graph.loaded() ) 00258 { 00259 printf( "p_map_search_nodes(): map file not loaded\n" ); 00260 return EC_fail; 00261 } 00262 00263 char* property; 00264 if ( EC_succeed != EC_arg( 1 ).is_string( &property ) ) 00265 { 00266 printf( "p_map_search_nodes(): no property given\n" ); 00267 return EC_fail; 00268 } 00269 00270 vector< RCSoftMapNode > nodes = g_map_graph.map_graph()->search_nodes( string(property) ); 00271 EC_word tail = nil(); 00272 00273 for ( vector< RCSoftMapNode >::iterator nit = nodes.begin(); 00274 nit != nodes.end(); 00275 ++nit ) 00276 { 00277 EC_word n = list( nit->name().c_str(), 00278 list( (double) nit->x(), 00279 list( (double) nit->y(), nil() ) ) ); 00280 tail = list( n, tail ); 00281 } 00282 00283 if ( EC_succeed != EC_arg( 1 ).unify( tail ) ) 00284 { 00285 printf( "p_map_search_nodes(): could not bind return value\n" ); 00286 return EC_fail; 00287 } 00288 00289 return EC_succeed; 00290 } 00291 00292 int 00293 p_map_graph_get_children() 00294 { 00295 if ( !g_map_graph.loaded() ) 00296 { 00297 printf( "p_map_graph_get_children(): no map file loaded\n" ); 00298 return EC_fail; 00299 } 00300 00301 char* nodename; 00302 if ( EC_succeed != EC_arg( 1 ).is_string( &nodename ) ) 00303 { 00304 printf( "p_map_graph_get_children(): no node name given\n" ); 00305 return EC_fail; 00306 } 00307 00308 RCSoftMapNode node = g_map_graph.map_graph()->node( nodename ); 00309 vector< string > children = node.children(); 00310 EC_word tail = nil(); 00311 for ( vector< string >::iterator nit = children.begin(); 00312 nit != children.end(); 00313 ++nit ) 00314 { 00315 tail = list( EC_word( (*nit).c_str() ), tail ); 00316 } 00317 00318 if ( EC_succeed != EC_arg( 2 ).unify( tail ) ) 00319 { 00320 printf( "p_map_graph_get_children(): cannot bind return value\n" ); 00321 return EC_fail; 00322 } 00323 00324 return EC_succeed; 00325 }