00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "fawkes_bb_interface.h"
00024 #include <plugins/readylogagent/eclipse_thread.h>
00025
00026 #include <core/exceptions/software.h>
00027 #include <interface/interface.h>
00028 #include <interfaces/ObjectPositionInterface.h>
00029
00030 #include <eclipseclass.h>
00031
00032 #include <cstdlib>
00033 #include <cstring>
00034 #include <map>
00035 #include <string>
00036 #include <list>
00037
00038 using namespace std;
00039 using namespace fawkes;
00040
00041 EC_word construct_iface_struct( Interface* iface );
00042 EC_word construct_msg_struct( const char* iface_type, Message* msg );
00043 EC_word* construct_struct_args( const InterfaceFieldIterator& begin,
00044 const InterfaceFieldIterator& end,
00045 unsigned int num_fields );
00046 void parse_struct_args( EC_word iface_struct,
00047 const InterfaceFieldIterator& begin,
00048 const InterfaceFieldIterator& end,
00049 unsigned int num_fields );
00050
00051 int
00052 p_read_interface()
00053 {
00054
00055
00056
00057
00058
00059
00060 Interface* interface;
00061 char* id;
00062
00063
00064 if ( EC_succeed != EC_arg( 1 ).is_string( &id ) )
00065 {
00066 printf( "First argument of read_interface/2 is not a string\n" );
00067 return EC_fail;
00068 }
00069
00070
00071 interface = EclipseAgentThread::instance()->get_registered_interface( id );
00072 if ( !interface )
00073 {
00074 printf( "Interface with id %s is not available to the agent\n", id );
00075 return EC_fail;
00076 }
00077
00078
00079 EC_word iface_struct;
00080 try
00081 {
00082 iface_struct = construct_iface_struct( interface );
00083 }
00084 catch ( Exception& e )
00085 {
00086 e.print_trace();
00087 return EC_fail;
00088 }
00089
00090
00091 return unify( EC_arg(2), iface_struct );
00092 }
00093
00094 int
00095 p_write_interface()
00096 {
00097
00098
00099
00100
00101
00102 char* id;
00103 Interface* interface;
00104
00105
00106 if ( EC_succeed != EC_arg( 1 ).is_string( &id ) )
00107 {
00108 printf( "Firste argument of write_interface/2 is not a string\n" );
00109 return EC_fail;
00110 }
00111
00112
00113 interface = EclipseAgentThread::instance()->get_registered_interface( id );
00114 if ( !interface )
00115 {
00116 printf( "Interface with id %s is not available to the agent\n", id );
00117 return EC_fail;
00118 }
00119
00120
00121 EC_functor fctor;
00122 if ( EC_succeed != EC_arg( 2 ).functor( &fctor ) )
00123 {
00124 printf( "Second argument of writer_interface/2 is not a compound term\n" );
00125 return EC_fail;
00126 }
00127
00128
00129 char* iface_type;
00130 if ( 1 != sscanf( fctor.name(), "data_%s", iface_type ) ||
00131 0 != strcmp( interface->type(), iface_type ) )
00132 {
00133 printf( "Second argument of write_interface/2 is not of type data_%s but of type %s\n",
00134 interface->type(), fctor.name() );
00135 free( iface_type );
00136 return EC_fail;
00137 }
00138 free( iface_type );
00139
00140
00141 unsigned int arity = (unsigned int) EC_arg( 2 ).arity();
00142 if ( interface->num_fields() != arity )
00143 {
00144 printf( "Second argument of write_interface/2 has wrong arity\n" );
00145 return EC_fail;
00146 }
00147
00148
00149 try
00150 {
00151 parse_struct_args( EC_word( 2 ),
00152 interface->fields(),
00153 interface->fields_end(),
00154 interface->num_fields() );
00155 }
00156 catch ( Exception& e )
00157 {
00158 e.print_trace();
00159 return EC_fail;
00160 }
00161
00162 return EC_succeed;
00163 }
00164
00165 int
00166 p_send_message()
00167 {
00168
00169
00170
00171 char* iface_id;
00172 Interface* interface;
00173
00174
00175 if ( EC_succeed != EC_arg( 1 ).is_string( &iface_id ) )
00176 {
00177 printf( "First argument of send_message/2 is not an atom\n" );
00178 return EC_fail;
00179 }
00180
00181
00182 interface = EclipseAgentThread::instance()->get_registered_interface( iface_id );
00183 if ( !interface )
00184 {
00185 printf( "Interface with id %s is not available to the agent\n", iface_id );
00186 return EC_fail;
00187 }
00188
00189
00190 EC_functor fctor;
00191 if ( EC_succeed != EC_arg( 2 ).functor( &fctor ) )
00192 {
00193 printf( "Second argument of send_message/2 has no functor\n" );
00194 return EC_fail;
00195 }
00196
00197 char* fctor_name = strdup( fctor.name() );
00198 strtok( fctor_name, "_" );
00199 char* iface_type = strtok( NULL, "_" );
00200 char* msg_type = strtok( NULL, "_" );
00201
00202 if ( !iface_type ||
00203 !msg_type ||
00204 0 != strcmp( interface->type(), iface_type ) )
00205 {
00206 printf( "Malformed functor: %s\n", fctor.name() );
00207 return EC_fail;
00208 }
00209
00210
00211 Message* msg;
00212 try
00213 {
00214 msg = interface->create_message( msg_type );
00215 }
00216 catch ( UnknownTypeException& e )
00217 {
00218 printf( "Message type %s is not available for interfaces of type %s\n",
00219 msg_type, interface->type() );
00220 return EC_fail;
00221 }
00222
00223
00224 free( fctor_name );
00225
00226
00227 try
00228 {
00229 parse_struct_args( EC_arg( 2 ), msg->fields(), msg->fields_end(), msg->num_fields() );
00230 }
00231 catch ( Exception& e )
00232 {
00233 e.print_trace();
00234 return EC_fail;
00235 }
00236
00237
00238 interface->msgq_enqueue( msg );
00239
00240 return EC_succeed;
00241 }
00242
00243 int
00244 p_recv_messages()
00245 {
00246
00247
00248
00249
00250
00251 char* iface_id;
00252 Interface* interface;
00253
00254 if ( EC_succeed != EC_arg( 1 ).is_string( &iface_id ) )
00255 {
00256 printf( "First argument of send_message/2 is not an atom\n" );
00257 return EC_fail;
00258 }
00259
00260 interface = EclipseAgentThread::instance()->get_registered_interface( iface_id );
00261 if ( !interface )
00262 {
00263 printf( "Interface with id %s is not available to the agent\n", iface_id );
00264 return EC_fail;
00265 }
00266
00267 std::list< EC_word > messages;
00268
00269 while ( !interface-> msgq_empty() )
00270 {
00271 Message* msg = interface->msgq_first();
00272 EC_word msg_struct;
00273
00274 try
00275 {
00276 msg_struct = construct_msg_struct( interface->type(), msg );
00277 }
00278 catch( Exception& e )
00279 {
00280 e.print_trace();
00281 return EC_fail;
00282 }
00283
00284 messages.push_back( msg_struct );
00285
00286 interface->msgq_pop();
00287 }
00288
00289 EC_word l = nil();
00290 while( !messages.empty() )
00291 {
00292 l = ::list( messages.front(), l );
00293 messages.pop_front();
00294 }
00295
00296 return unify( EC_arg( 2 ), l );
00297 }
00298
00299 EC_word
00300 construct_iface_struct( Interface* iface )
00301 {
00302 EC_word* args = construct_struct_args( iface->fields(),
00303 iface->fields_end(),
00304 iface->num_fields() );
00305
00306 char* fctor;
00307 asprintf( &fctor, "data_%s", iface->type() );
00308
00309 EC_word ret = term( EC_functor( fctor, iface->num_fields() ), args );
00310
00311 delete[] args;
00312 free( fctor );
00313
00314 return ret;
00315 }
00316
00317 EC_word
00318 construct_msg_struct( const char* iface_type, Message* msg )
00319 {
00320 EC_word* args = construct_struct_args( msg->fields(), msg->fields_end(), msg->num_fields() );
00321
00322 char* fctor;
00323 asprintf( &fctor, "data_%s_%s", iface_type, msg->type() );
00324
00325 EC_word ret = term( EC_functor( fctor, msg->num_fields() ), args );
00326
00327 delete[] args;
00328 free( fctor );
00329
00330 return ret;
00331 }
00332
00333 EC_word*
00334 construct_struct_args( const InterfaceFieldIterator& begin,
00335 const InterfaceFieldIterator& end,
00336 unsigned int num_fields )
00337 {
00338 EC_word* args = new EC_word[ num_fields ];
00339
00340 InterfaceFieldIterator field_iter;
00341 unsigned int field_idx;
00342 for ( field_iter = begin, field_idx = 0;
00343 field_iter != end;
00344 ++field_iter, ++field_idx )
00345 {
00346 interface_fieldtype_t type = field_iter.get_type();
00347
00348 switch ( type )
00349 {
00350 case IFT_BOOL: {
00351 char* t = strdup( "true" );
00352 char* f = strdup( "fail" );
00353
00354 args[ field_idx ] = field_iter.get_bool() ? EC_atom( t ) : EC_atom( f );
00355
00356 free( t );
00357 free( f );
00358
00359 break;
00360 }
00361
00362 case IFT_INT:
00363 args[ field_idx ] = EC_word( field_iter.get_int() );
00364 break;
00365
00366 case IFT_UINT:
00367 args[ field_idx ] = EC_word( (long) field_iter.get_uint() );
00368 break;
00369
00370 case IFT_LONGINT:
00371 args[ field_idx ] = EC_word( field_iter.get_longint() );
00372 break;
00373
00374 case IFT_LONGUINT:
00375 args[ field_idx ] = EC_word( (long) field_iter.get_longuint() );
00376 break;
00377
00378 case IFT_FLOAT:
00379 args[ field_idx ] = EC_word( field_iter.get_float() );
00380 break;
00381
00382 case IFT_STRING:
00383 args[ field_idx ] = EC_word( field_iter.get_string() );
00384 break;
00385
00386 case IFT_BYTE:
00387 args[ field_idx ] = EC_word( field_iter.get_byte() );
00388 break;
00389
00390 default:
00391 throw UnknownTypeException( "Field of unknown type %s", field_iter.get_typename() );
00392 }
00393 }
00394
00395 return args;
00396 }
00397
00398 void
00399 parse_struct_args( EC_word data_struct,
00400 const InterfaceFieldIterator& begin,
00401 const InterfaceFieldIterator& end,
00402 unsigned int num_fields )
00403 {
00404 unsigned int field_idx;
00405 InterfaceFieldIterator field_iter;
00406
00407 for ( field_iter = begin, field_idx = 1;
00408 field_iter != end;
00409 ++field_iter, ++field_idx )
00410 {
00411 interface_fieldtype_t type = field_iter.get_type();
00412
00413 EC_word arg;
00414 if ( EC_succeed != data_struct.arg( field_idx, arg ) )
00415 { throw Exception( "Failed to parse interface data. Couldn't read %d-th parameter.\n", field_idx ); }
00416
00417 switch( type )
00418 {
00419 case IFT_BOOL: {
00420 char* t = strdup( "true" );
00421 char* f = strdup( "fail" );
00422 if ( EC_succeed == arg.unify( EC_atom( t ) ) )
00423 { field_iter.set_bool( true ); }
00424 else if ( EC_succeed == arg.unify( EC_atom( f ) ) )
00425 { field_iter.set_bool( false ); }
00426 else
00427 {
00428 free( t );
00429 free( f );
00430 throw TypeMismatchException( "Wrong data type for %d-th argument\n", field_idx );
00431 }
00432
00433 free( t );
00434 free( f );
00435
00436 break;
00437 }
00438
00439 case IFT_INT: {
00440 long val;
00441 if ( EC_succeed == arg.is_long( &val ) )
00442 { field_iter.set_int( (int) val ); }
00443 else
00444 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00445
00446 break;
00447 }
00448
00449 case IFT_UINT: {
00450 long val;
00451 if ( EC_succeed == arg.is_long( &val ) )
00452 { field_iter.set_uint( (unsigned int) val ); }
00453 else
00454 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00455
00456 break;
00457 }
00458
00459 case IFT_LONGINT: {
00460 long val;
00461 if ( EC_succeed == arg.is_long( &val ) )
00462 { field_iter.set_longint( val ); }
00463 else
00464 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00465
00466 break;
00467 }
00468
00469 case IFT_LONGUINT: {
00470 long val;
00471 if ( EC_succeed == arg.is_long( &val ) )
00472 { field_iter.set_longuint( (long unsigned int) val ); }
00473 else
00474 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00475
00476 break;
00477 }
00478
00479 case IFT_FLOAT: {
00480 double val;
00481 if ( EC_succeed == arg.is_double( &val ) )
00482 { field_iter.set_float( (float) val ); }
00483 else
00484 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00485
00486 break;
00487 }
00488
00489 case IFT_STRING: {
00490 char* val;
00491 if ( EC_succeed == arg.is_string( &val ) )
00492 { field_iter.set_string( val ); }
00493 else
00494 { throw TypeMismatchException( "Wrong data type for %d-the argument\n", field_idx ); }
00495
00496 break;
00497 }
00498
00499 default:
00500 break;
00501 }
00502 }
00503 }
