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
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
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 }