YarpService.cpp
Go to the documentation of this file.
00001 #include "lo/YarpService.h"
00002 #include <yarp/os/all.h>
00003 
00004 using namespace yarp::os;
00005 
00006 
00007 volatile bool g_stopall = false;
00008 Port g_ListenPort;
00009 #ifdef _DEBUG
00010 #define PRINTF_DEBUG(A) printf(A)
00011 #define PRINTF_DEBUG_2(A,B) printf(A,B)
00012 #define PRINTF_DEBUG_3(A,B,C) printf(A,B,C)
00013 #else
00014 #define PRINTF_DEBUG(A)
00015 #define PRINTF_DEBUG_2(A,B)
00016 #define PRINTF_DEBUG_3(A,B,C)
00017 #endif 
00018 #include <signal.h>
00019 void CTRLC(int)
00020 {
00021   g_stopall = true;
00022   g_ListenPort.interrupt();
00023 }
00024 
00028 int main(int argc, char* argv[])
00029 {
00030   signal(SIGINT, CTRLC);
00031   if(argc > 2)
00032   {
00033     jlo::YarpService serv(argv[2], argv[1]);
00034     serv.SaveList()->WriteToFile(argv[1]);
00035   }
00036   else if(argc > 1)
00037   {
00038     jlo::YarpService serv("/lo/in", argv[1]);
00039     serv.SaveList()->WriteToFile(argv[1]);
00040   }
00041   else
00042   {
00043     PRINTF_DEBUG_2("Usage: %s configFile PortName\n", argv[0]);
00044     jlo::YarpService serv("/lo/in", "lo-config.ini");
00045     serv.SaveList()->WriteToFile("lo-config.ini");
00046   }
00047   
00048 }
00049 
00050 namespace jlo
00051 {
00052 YarpService::YarpService (const char* listeingport, const char* configFile) :
00053   ServiceInterface(configFile)
00054 {
00055   Network net;
00056   net.init();
00057   if(!net.queryName(listeingport).isValid())
00058   {
00059     if(g_ListenPort.open(listeingport))
00060     {
00061       while(!g_stopall)
00062       {
00063         Bottle botIn;
00064         g_ListenPort.read(botIn, true);
00065         Bottle bot;
00066         if(FillBottle(botIn, bot))
00067           g_ListenPort.reply(bot);
00068 #ifdef _DEBUG
00069         PRINTF_DEBUG_2("Got Bottle:%s\n", botIn.toString().c_str());
00070         PRINTF_DEBUG_2("Sending: %s\n", bot.toString().c_str());
00071 #endif /* _DEBUG*/
00072       }
00073     }
00074     else
00075     {
00076       printf("Opening of port %s failed! Check if the service already runs in your namespace or run yarp clean\n", listeingport);
00077     }
00078   }
00079   else
00080   {
00081     printf("Port %s already in use! Check if the service already runs in your namespace or run yarp clean\n", listeingport);
00082   }
00083 }
00084 
00085   bool YarpService::FillBottle(Bottle& botIn, Bottle& bot)
00086   {
00087         int num = botIn.size();
00088         switch(num)
00089         {
00090         case 1: 
00091           {
00092             Value id =  botIn.get(0);
00093             if(id.isInt())
00094             {
00095               ServiceLocatedObject* lo = YarpService::GetServiceLocatedObject(id.asInt());
00096               if (lo == NULL)
00097               {
00098                 PRINTF_DEBUG("Error Parsing Bottle: Id does not exist!\n");
00099                 bot.addString("Error Parsing Bottle: Id does not exist!\n");
00100                 break;
00101               }
00102               PutIntoBottle(lo, bot);
00103             }
00104             else
00105             {
00106               PRINTF_DEBUG("Error Parsing Bottle: Wrong Type in bottle for id query!\n");
00107               bot.addString("Error Parsing Bottle: Wrong Type in bottle for id query!\n");
00108             }
00109             break;
00110           }
00111         case 2: 
00112           {
00113             Value id =  botIn.get(0);
00114             if(id.isInt())
00115             {
00116               Value parent_id =  botIn.get(1);
00117               if(parent_id.isInt())
00118               {
00119                 int parentID = parent_id.asInt();
00120                 ServiceLocatedObject* lo = GetServiceLocatedObject(id.asInt());
00121                 if (lo == NULL)
00122                 {
00123                   PRINTF_DEBUG("Error Parsing Bottle: Id does not exist!\n");
00124                   bot.addString("Error Parsing Bottle: Id does not exist!\n");
00125                   break;
00126                 }
00127                 
00128                 bot.clear();
00129                 if(lo->m_uniqueID != ID_WORLD && lo->m_relation->m_uniqueID == parentID)
00130                 {
00131                   PutIntoBottle(lo, bot);
00132                 }
00133                 else
00134                 {
00135                   ServiceLocatedObject* parent = GetServiceLocatedObject(parentID);
00136                   if (parent == NULL)
00137                   {
00138                     PRINTF_DEBUG("Error Parsing Bottle: Parent Id does not exist!\n");
00139                     bot.addString("Error Parsing Bottle: Parent Id does not exist!\n");
00140                     break;
00141                   }
00142                   Matrix mat = lo->GetMatrix(*parent);
00143                   Matrix cov = lo->GetCovarianceMatrix(*parent);
00144                   PutIntoBottle(FServiceLocatedObject(parent, mat, cov), bot);
00145                 }
00146               }
00147               else
00148               {
00149                     PRINTF_DEBUG("Error Parsing Bottle: Parent Id was not an integer!\n");
00150                     bot.addString("Error Parsing Bottle: Parent Id was not an integer!\n");
00151                     break;
00152               }
00153             }
00154             else if(id.isString())
00155             {
00156               std::string  st(id.asString().c_str());
00157               if(st.compare("del")==0)
00158               {
00159                 if(botIn.get(1).isInt())
00160                 {
00161                   int id = botIn.get(1).asInt();
00162                   if(id != ID_WORLD)
00163                   {
00164                     bot.addInt(FreeServiceLocatedObject(GetServiceLocatedObject(id)));
00165                   }
00166                   else
00167                   {
00168                     PRINTF_DEBUG("Error: Can't delete ID_WORLD!\n");
00169                     bot.addString("Error: Can't delete ID_WORLD!\n");
00170                   }
00171                 }
00172               }
00173               else
00174               {
00175                 PRINTF_DEBUG("Error Parsing Bottle: Wrong Type in bottle for delete query!\n");
00176                 bot.addString("Error Parsing Bottle: Wrong Type in bottle for delete query!\n");
00177               }
00178             }
00179             else
00180             {
00181               PRINTF_DEBUG("Error Parsing Bottle: Wrong Type in bottle for parent_id query!\n");
00182               bot.addString("Error Parsing Bottle: Wrong Type in bottle for parent_id query!\n");
00183             }
00184             break;
00185           }
00186         case 0: 
00187           PRINTF_DEBUG("Empty Bottle!\n");
00188           return false;
00189         default: 
00190           {
00191              Value parent_id, id;
00192              int counter = 0;
00193              bool update = false;
00194               if(botIn.get(0).isInt() && botIn.get(1).isInt())
00195               {
00196                 id =  botIn.get(0);
00197                 parent_id =  botIn.get(1);
00198                 update = true;
00199                 counter = 2;
00200               }
00201               else
00202               {
00203                 parent_id =  botIn.get(0);
00204                 counter = 1;
00205               }
00206               if(parent_id.isInt())
00207               {
00208                 int parentID = parent_id.asInt();
00209                 Matrix mat(4,4), cov(6,6);
00210                 Value mat_in = botIn.get(counter);
00211                 Value cov_in = botIn.get(counter + 1);
00212                 int type = 1;
00213                 if(botIn.size() > counter + 1)
00214                   type = botIn.get(counter +2).asInt();
00215                 if(!mat_in.isList() || !cov_in.isList())
00216                 {
00217                   PRINTF_DEBUG("Error Parsing Bottle: Matrix or covariance not send as a list!\n");
00218                   bot.addString("Error Parsing Bottle: Matrix or covariance not send as a list!\n");
00219                   break;
00220                 }
00221                 yarp::os::Bottle* mat_in_bot = mat_in.asList();
00222                 if(mat_in_bot->size() < 16)
00223                 {
00224                   PRINTF_DEBUG("Error Parsing Bottle: Matrix list is too short!\n");
00225                   bot.addString("Error Parsing Bottle: Matrix list is too short!\n");
00226                   break;
00227                 }
00228 
00229                 int width = 4;
00230                 for(int r = 0; r < width; r++)
00231                 {
00232                   for(int c = 0; c < width; c++)
00233                   {
00234                     yarp::os::Value& val = mat_in_bot->get(r * width + c);
00235                     if(val.isDouble())
00236                       mat.element(r,c) = val.asDouble();
00237                     else if(val.isInt())
00238                       mat.element(r,c) = val.asInt();
00239                     else
00240                       mat.element(r,c) = 0;
00241                   }
00242                 }
00243                 width = 6;
00244                 yarp::os::Bottle* cov_in_bot = cov_in.asList();
00245                 if(cov_in_bot->size() < 36)
00246                 {
00247                   PRINTF_DEBUG("Error Parsing Bottle: Covariance list is too short!\n");
00248                   bot.addString("Error Parsing Bottle: Covariance list is too short!\n");
00249                   break;
00250                 }
00251                 for(int r = 0; r < width; r++)
00252                 {
00253                   for(int c = 0; c < width; c++)
00254                   {
00255                     yarp::os::Value& val = cov_in_bot->get(r * width + c);
00256                     if(val.isDouble())
00257                       cov.element(r,c) = val.asDouble();
00258                     else if(val.isInt())
00259                       cov.element(r,c) = val.asInt();
00260                     else
00261                       cov.element(r,c) = 0;
00262                   }
00263                 }
00264                 
00265                 if(update)
00266                 {
00267                   ServiceLocatedObject* pose = GetServiceLocatedObject(id.asInt());
00268                   if(pose == NULL)
00269                   {
00270                     ServiceLocatedObject* parent = GetServiceLocatedObject(parentID);
00271                     if(parent == NULL)
00272                     {
00273                       PRINTF_DEBUG("Error Parsing Bottle: Requested parent does not exist!\n");
00274                       bot.addString("Error Parsing Bottle: Requested parent does not exist!\n");
00275                       break;
00276                     }
00277                     PutIntoBottle(FServiceLocatedObject(parent, mat, cov, type), bot);
00278                   }
00279                   else
00280                   {
00281                     ServiceLocatedObject* parent = GetServiceLocatedObject(parentID);
00282                     if(parent == NULL || (parent->m_uniqueID == ID_WORLD && pose->m_uniqueID == ID_WORLD))
00283                     {
00284                       PRINTF_DEBUG("Error Parsing Bottle: Requested parent does not exist!\n");
00285                       bot.addString("Error Parsing Bottle: Requested parent does not exist!\n");
00286                       break;
00287                     }
00288                     pose->Update(mat, cov, ServiceInterface::FServiceLocatedObjectCopy);
00289                     PutIntoBottle(pose, bot);
00290                   }
00291                 }
00292                 else
00293                 {
00294                   ServiceLocatedObject* parent = GetServiceLocatedObject(parentID);
00295                   if(parent == NULL)
00296                   {
00297                     PRINTF_DEBUG("Error Parsing Bottle: Requested parent does not exist!\n");
00298                     bot.addString("Error Parsing Bottle: Requested parent does not exist!\n");
00299                     break;
00300                   }
00301                   PutIntoBottle(FServiceLocatedObject(parent, mat, cov, type), bot);
00302                 }
00303               }
00304               else
00305               {
00306                 PRINTF_DEBUG("Error Parsing Bottle: Wrong Type in bottle for setting lo!\n");
00307                 bot.addString("Error Parsing Bottle: Wrong Type in bottle for setting lo!\n");
00308               }
00309           }
00310           break;
00311         }
00312         return true;
00313   }
00314 
00315 YarpService::~YarpService()
00316 {
00317   g_ListenPort.close();
00318 }
00319 
00320 void YarpService::PutIntoBottle(ServiceLocatedObject* pose, Bottle& bot)
00321 {
00322   pose->IncreaseReferenceCounter();
00323   bot.addInt(pose->m_uniqueID);
00324   if(pose->m_uniqueID != ID_WORLD)
00325       bot.addInt(pose->m_parentID);
00326   else
00327     bot.addInt(pose->m_uniqueID);
00328   Matrix m = pose->GetMatrix();
00329   Matrix cov = pose->GetCovarianceMatrix();
00330   /* Bottle per matrix*/
00331   int width = 4;
00332   yarp::os::Bottle &mat_b = bot.addList();;
00333   for(int r = 0; r < width; r++)
00334   {
00335     for(int c = 0; c < width; c++)
00336     {
00337         mat_b.addDouble(m.element(r,c));
00338     }
00339   }
00340   
00341   width = 6;
00342   yarp::os::Bottle &cov_b = bot.addList();;
00343   for(int r = 0; r < width; r++)
00344   {
00345     for(int c = 0; c < width; c++)
00346     {
00347       cov_b.addDouble( cov.element(r,c));
00348     }
00349   }
00350 return;
00351 }
00352 
00353 } /* end namespace jlo*/
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


lo
Author(s): U. Klank
autogenerated on Thu May 23 2013 07:34:46