usyncclient.cc

00001 #ifndef _MSC_VER
00002 # include <unistd.h>
00003 #else
00004 # include <io.h>
00005 #endif
00006 #include <fcntl.h>
00007 
00008 #include "libport/thread.hh"
00009 
00010 #include "urbi/usyncclient.hh"
00011 #include "urbi/uconversion.hh"
00012 
00013 /* "min" shouldn't be defined as a preprocessor macro. On windows, it is. So
00014  * using std::min leads to a parse error because CPP expands it as
00015  *   std::((a) < (b) ? (a) : (b))
00016  * However, this should only occur on windows with the STL of MS VC++.
00017  */
00018 #ifdef min
00019 # ifdef WIN32
00020 #  undef min
00021 # else
00022 #  error "min is defined as a CPP macro and we're not on WIN32. Report this!"
00023 # endif /* !WIN32 */
00024 #endif /* !min */
00025 
00026 namespace urbi
00027 {
00028   USyncClient::USyncClient(const char *_host, int _port, int _buflen)
00029     : UClient(_host, _port, _buflen),
00030       sem_(),
00031       queueLock_(),
00032       msg(0),
00033       syncLock_(),
00034       syncTag ("")
00035   {
00036     libport::startThread(this, &USyncClient::callbackThread);
00037     if (!defaultClient)
00038       defaultClient = this;
00039   }
00040 
00041   void USyncClient::callbackThread()
00042   {
00043     while (true)
00044     {
00045       sem_--;
00046       queueLock_.lock();
00047       if (queue.empty())
00048       {
00049         //we got mysteriously interrupted
00050         queueLock_.unlock();
00051         continue;
00052       }
00053       UMessage *m = queue.front();
00054       queue.pop_front();
00055       queueLock_.unlock();
00056       UAbstractClient::notifyCallbacks(*m);
00057       delete m;
00058     }
00059   }
00060 
00061   void USyncClient::notifyCallbacks(const UMessage &msg)
00062   {
00063     queueLock_.lock();
00064     if (syncTag == msg.tag)
00065     {
00066       this->msg = new UMessage(msg);
00067       syncLock_++;
00068       syncTag = "";
00069     }
00070     else
00071     {
00072       queue.push_back(new UMessage(msg));
00073       sem_++;
00074     }
00075     queueLock_.unlock();
00076   }
00077 
00078   UMessage* USyncClient::waitForTag(const char* tag)
00079   {
00080     syncTag = tag;
00081     queueLock_.unlock();
00082     syncLock_--;
00083     syncTag = "";
00084     return msg;
00085   }
00086 
00087 
00088   UMessage* USyncClient::syncGet(const char* format, ...)
00089   {
00090     //check there is no tag
00091     int p = 0;
00092     while (format[p] ==' ')
00093       ++p;
00094     while (isalpha(format[p]))
00095       ++p;
00096     while (format[p] == ' ')
00097       ++p;
00098     if (format[p] == ':')
00099     {
00100       std::cerr << "FATAL: passing a taged command to syncGet:'" << format <<
00101         "'\n";
00102       ::exit(1);
00103     }
00104     //check if there is a command separator
00105     p = strlen(format) - 1;
00106     while (format[p] == ' ')
00107       --p;
00108     bool hasSep = (format[p] == ';' || format[p] == ',');
00109     va_list arg;
00110     va_start(arg, format);
00111     sendBufferLock.lock();
00112     rc = vpack(format, arg);
00113     va_end(arg);
00114     if (rc < 0)
00115     {
00116       sendBufferLock.unlock();
00117       return 0;
00118     }
00119     if (!hasSep)
00120       strcat(sendBuffer, ",");
00121     char tag[70];
00122     makeUniqueTag(tag);
00123     strcat(tag, ":");
00124     effectiveSend(tag, strlen(tag));
00125     tag[strlen(tag) - 1] = 0; //restore tag
00126     queueLock_.lock();
00127     rc = effectiveSend(sendBuffer, strlen(sendBuffer));
00128     sendBuffer[0] = 0;
00129     sendBufferLock.unlock();
00130     UMessage* m = waitForTag(tag);
00131     return m;
00132   }
00133 
00134   int
00135   USyncClient::syncGetImage(const char* camera,
00136                             void* buffer, int& buffersize,
00137                             int format, int transmitFormat,
00138                             int& width, int& height)
00139   {
00140     int f;
00141     if (format == IMAGE_JPEG  || transmitFormat == URBI_TRANSMIT_JPEG)
00142       f = 1;
00143     else
00144       f = 0;
00145     //XXX required to ensure format change is applied
00146     send("%s.format = %d; noop; noop;", camera, f);
00147     UMessage *m = syncGet("%s.val;", camera);
00148     if (m->value->binary->type != BINARY_IMAGE)
00149     {
00150       delete m;
00151       return 0;
00152     }
00153     width = m->value->binary->image.width;
00154     height = m->value->binary->image.height;
00155 
00156     int osize = buffersize;
00157     if (f == 1  &&  format != IMAGE_JPEG)
00158     {
00159       //uncompress jpeg
00160       if (format == IMAGE_YCbCr)
00161         convertJPEGtoYCrCb((const byte*) m->value->binary->image.data,
00162                            m->value->binary->image.size, (byte*) buffer,
00163                            buffersize);
00164       else
00165         convertJPEGtoRGB((const byte*) m->value->binary->image.data,
00166                          m->value->binary->image.size, (byte*) buffer,
00167                          buffersize);
00168     }
00169     else if (format == IMAGE_RGB || format == IMAGE_PPM)
00170     {
00171       buffersize = std::min(m->value->binary->image.size,
00172                             static_cast<size_t> (buffersize));
00173       if (m->value->binary->image.imageFormat == IMAGE_YCbCr)
00174         convertYCrCbtoRGB((const byte*) m->value->binary->image.data,
00175                           buffersize, (byte*) buffer);
00176       else
00177         memcpy(buffer, m->value->binary->image.data, buffersize);
00178 
00179     }
00180     else
00181     {
00182       //jpeg jpeg, or ycrcb ycrcb
00183       buffersize = std::min(m->value->binary->image.size,
00184                             static_cast<size_t> (buffersize));
00185       memcpy(buffer, m->value->binary->image.data, buffersize);
00186     }
00187     if (format == IMAGE_PPM)
00188     {
00189       char p6h[20];
00190       sprintf(p6h, "P6\n%d %d\n255\n", width, height);
00191       int p6len = strlen(p6h);
00192       int mlen = osize > buffersize + p6len ? buffersize : osize - p6len;
00193       memmove((void *) (((long) buffer) + p6len), buffer, mlen);
00194       memcpy(buffer, p6h, p6len);
00195       buffersize += p6len;
00196     }
00197     delete m;
00198     return 1;
00199   }
00200 
00201 
00202   int
00203   USyncClient::syncGetNormalizedDevice(const char* device, double& val)
00204   {
00205     UMessage *m = syncGet("%s.valn;", device);
00206 
00207     if (m->type != MESSAGE_DATA || m->value->type != DATA_DOUBLE)
00208     {
00209       delete m;
00210       return 0;
00211     }
00212     val = (double)(*(m->value));
00213     delete m;
00214     return 1;
00215   }
00216 
00217   int
00218   USyncClient::syncGetDevice(const char* device, double& val)
00219   {
00220     UMessage *m = syncGet("%s.val;", device);
00221 
00222     if (m->type != MESSAGE_DATA || m->value->type != DATA_DOUBLE)
00223     {
00224       delete m;
00225       return 0;
00226     }
00227     val = (double)(*(m->value));
00228     delete m;
00229     return 1;
00230   }
00231 
00232   int
00233   USyncClient::syncGetResult(const char* command, double& val)
00234   {
00235     UMessage *m = syncGet("%s", command);
00236 
00237     if (m->type != MESSAGE_DATA || m->value->type != DATA_DOUBLE)
00238     {
00239       delete m;
00240       return 0;
00241     }
00242     val = (double)(*(m->value));
00243     delete m;
00244     return 1;
00245 
00246   }
00247 
00248 
00249   int
00250   USyncClient::syncGetDevice(const char* device, const char* access,
00251                              double& val)
00252   {
00253     UMessage *m = syncGet("%s.%s;", device, access);
00254 
00255     if (m->type != MESSAGE_DATA || m->value->type != DATA_DOUBLE)
00256     {
00257       delete m;
00258       return 0;
00259     }
00260     val = (double)(*(m->value));
00261     delete m;
00262     return 1;
00263 
00264 
00265   }
00266 
00267 
00268   int
00269   USyncClient::syncGetSound(const char* device, int duration, USound& sound)
00270   {
00271     send("syncgetsound = BIN 0;"
00272          " loopsound: loop syncgetsound = syncgetsound +  %s.val,"
00273          " { "
00274          "   wait(%d);"
00275          "   stop loopsound;"
00276          "   noop;"
00277          "   noop;"
00278          " };", device, duration);
00279     UMessage* m = syncGet("syncgetsound;");
00280     if (m->type != MESSAGE_DATA
00281         || m->value->type != DATA_BINARY
00282         || m->value->binary->type != BINARY_SOUND)
00283         {
00284       delete m;
00285       return 0;
00286     }
00287     convert(m->value->binary->sound, sound);
00288     delete m;
00289     return 1;
00290   }
00291 
00292   int
00293   USyncClient::syncSend(const void* buffer, int length)
00294   {
00295     if (rc != 0)
00296       return -1;
00297     sendBufferLock.lock();
00298     int sent = 0;
00299     while (sent < length)
00300     {
00301       int res = ::write(sd, (char*) buffer + sent, length - sent);
00302       if (res < 0)
00303       {
00304         rc = res;
00305         sendBufferLock.unlock();
00306         return res;
00307       }
00308       sent += res;
00309     }
00310     sendBufferLock.unlock();
00311     return 0;
00312   }
00313 } // namespace urbi

Generated on Tue Apr 10 17:45:45 2007 for URBISDK by  doxygen 1.5.1