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
00055 pthread_mutex_unlock(&lock[source]);
00056 return urbi::URBI_CONTINUE;
00057 }
00058
00059
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 }