urbitalkie.cc

00001 #include <sys/time.h>
00002 #include <pthread.h>
00003 #include "libport/windows.hh"
00004 #include "urbi/uclient.hh"
00005 
00006 class SoundPipe
00007 {
00008 public:
00009   SoundPipe(urbi::UClient &r1, urbi::UClient &r2);
00010   urbi::UCallbackAction microNotify(int side, const urbi::UMessage &msg);
00011   urbi::UCallbackAction speakerNotify(int side, const urbi::UMessage &msg);
00012 
00013 private:
00014   static const size_t stackSize = 4;
00015   static const size_t minSendSize = 2048;
00016   class SoundStack
00017   {
00018   public:
00019     std::list<urbi::USound> stack;
00020     int serverStackPos;
00021   };
00022   urbi::UClient *robot[2];
00023   SoundStack stack[2];
00024   pthread_mutex_t lock[2];;
00025 
00026   void trySend(int source);
00027 };
00028 
00029 
00030 SoundPipe::SoundPipe(urbi::UClient &r1, urbi::UClient &r2)
00031 {
00032   pthread_mutex_init(&lock[0], 0);
00033   pthread_mutex_init(&lock[1], 0);
00034   robot[0]=&r1;
00035   robot[1]=&r2;
00036 
00037   stack[0].serverStackPos = 0;
00038   stack[1].serverStackPos = 0;
00039   r1.setCallback(*this, &SoundPipe::microNotify, 0, "mic");
00040   r2.setCallback(*this, &SoundPipe::microNotify, 1, "mic");
00041   r1.send("loop mic: micro.val,");
00042   r2.send("loop mic: micro.val,");
00043   r1.setCallback(*this, &SoundPipe::speakerNotify, 0, "speak");
00044   r2.setCallback(*this, &SoundPipe::speakerNotify, 1, "speak");
00045 }
00046 
00047 
00048 urbi::UCallbackAction
00049 SoundPipe::microNotify(int source, const urbi::UMessage &msg)
00050 {
00051   pthread_mutex_lock(&lock[source]);
00052   if (stack[source].stack.size() >= stackSize)
00053     {
00054       //drop it
00055       pthread_mutex_unlock(&lock[source]);
00056       return urbi::URBI_CONTINUE;
00057     }
00058 
00059   //convert it
00060   urbi::USound snd;
00061   snd.size = 0;
00062   snd.data = 0;
00063   snd.channels = 1;
00064   snd.rate = 16000;
00065   snd.sampleSize = 16;
00066   snd.sampleFormat = urbi::SAMPLE_SIGNED;
00067 
00068 
00069   urbi::USound &lastStacked = stack[source].stack.back();
00070   if (stack[source].stack.empty() || lastStacked.size > minSendSize)
00071     {
00072       snd.soundFormat = urbi::SOUND_RAW;
00073       convert(msg.value->binary->sound, snd);
00074       stack[source].stack.push_back(snd);
00075     }
00076   else
00077     {
00078       snd.soundFormat = urbi::SOUND_RAW;
00079       convert(msg.value->binary->sound, snd);
00080       lastStacked.data = (char *) realloc(lastStacked.data,
00081                                           lastStacked.size + snd.size);
00082       memcpy(lastStacked.data + lastStacked.size, snd.data, snd.size);
00083       lastStacked.size += snd.size;
00084       printf("%d queed %zd\n", source, lastStacked.size);
00085       free(snd.data);
00086       snd.data=0;
00087     }
00088 
00089   pthread_mutex_unlock(&lock[source]);
00090   trySend(1-source);
00091 
00092   return urbi::URBI_CONTINUE;
00093 }
00094 
00095 urbi::UCallbackAction
00096 SoundPipe::speakerNotify(int source, const urbi::UMessage &msg)
00097 {
00098   if (msg.type != urbi::MESSAGE_SYSTEM || !strstr(msg.message.c_str(),"stop"))
00099     return urbi::URBI_CONTINUE;
00100 
00101   pthread_mutex_lock(&lock[source]);
00102   stack[source].serverStackPos--;
00103   pthread_mutex_unlock(&lock[source]);
00104 
00105   trySend(1-source);
00106 
00107   return urbi::URBI_CONTINUE;
00108 }
00109 
00110 
00111 void
00112 SoundPipe::trySend(int source)
00113 {
00114   pthread_mutex_lock(&lock[0]);
00115   pthread_mutex_lock(&lock[1]);
00116 
00117   if (stack[source].stack.empty()
00118       || stack[1-source].serverStackPos>=3
00119       || stack[source].stack.front().size<minSendSize)
00120     {
00121       pthread_mutex_unlock(&lock[1]);
00122       pthread_mutex_unlock(&lock[0]);
00123       return ;
00124     }
00125 
00126   urbi::USound snd = stack[source].stack.front();
00127   stack[source].stack.pop_front();
00128   stack[1-source].serverStackPos++;
00129   pthread_mutex_unlock(&lock[source]);
00130   pthread_mutex_unlock(&lock[1-source]);
00131   printf("%d sent %zd\n", source, snd.size);
00132   robot[1-source]->sendSound("speaker", snd,"speak");
00133   free(snd.data);
00134   snd.data = 0;
00135 }
00136 
00137 
00138 int main(int argc, char * argv[])
00139 {
00140   if (argc<3)
00141   {
00142     fprintf(stderr,"usage: %s robot1 robot2\n\tplays what robot1 hears with robot2's speaker, and vice-versa\n",argv[0]);
00143     exit(1);
00144   }
00145   urbi::UClient * r[2];
00146   for (int i=0;i<2; ++i)
00147     {
00148       r[i]=new urbi::UClient(argv[i+1]);
00149       r[i]->start();
00150       if (r[i]->error())
00151         exit(1);
00152     }
00153   SoundPipe sp(*r[0], *r[1]);
00154   while (1)
00155     sleep(1);
00156 }

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