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