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
00014
00015
00016
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
00024 #endif
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
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
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
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;
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
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
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
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 }