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