Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

SpeakerServer.cc

Go to the documentation of this file.
00001 #include "SpeakerServer.h"
00002 
00003 #include "Shared/Buffer.h"
00004 #include "Shared/Config.h"
00005 #include "Shared/RobotInfo.h"
00006 #include "Wireless/Wireless.h"
00007 
00008 SpeakerServer* SpeakerServer::instance = 0;
00009 
00010 SpeakerServer* SpeakerServer::GetInstance() {
00011   if (instance == 0) {
00012     instance = new SpeakerServer();
00013   }
00014   return instance;
00015 }
00016 
00017 SpeakerServer::SpeakerServer()
00018   : BehaviorBase("Speaker Server"), socket(0),
00019     packet(), frame(0), resampled(0),
00020     channel(SoundManager::invalid_Play_ID) {
00021     
00022   AddReference();
00023 }
00024 
00025 SpeakerServer::~SpeakerServer() {
00026   delete frame;
00027   delete resampled;
00028   
00029   if (references == 1) {
00030     instance = 0;
00031   }
00032 }
00033 
00034 
00035 void SpeakerServer::DoStart() {
00036   BehaviorBase::DoStart();
00037   
00038   if (socket != 0) {
00039     wireless->setDaemon(socket, false);
00040     wireless->close(socket);
00041     socket = 0;
00042   }
00043   
00044   packet.header->SetPosition(0);
00045   channel = SoundManager::invalid_Play_ID;
00046     
00047   socket =
00048     wireless->socket(SocketNS::SOCK_STREAM, RECEIVE_BUFFER_SIZE + 8, 512);
00049   wireless->setReceiver(socket->sock, socket_callback);
00050   wireless->setDaemon(socket, true);
00051   wireless->listen(socket->sock, config->sound.streaming.speaker_port);
00052 }
00053 
00054 void SpeakerServer::DoStop() {
00055   if (socket != 0) {
00056     wireless->setDaemon(socket, false);
00057     wireless->close(socket);
00058     socket = 0;
00059   }
00060   
00061   sndman->StopPlay(channel);
00062   channel = SoundManager::invalid_Play_ID;
00063   
00064   packet.samples->SetCapacity(0);
00065   
00066   delete frame;
00067   delete resampled;
00068 
00069   BehaviorBase::DoStop();
00070 }
00071 
00072 int SpeakerServer::socket_callback(char *buf, int size) {
00073   if (instance == 0) {
00074     return 0;
00075   } else {
00076     return instance->GotSocketData(buf, size);
00077   }
00078 }
00079 
00080 SpeakerServer::Packet::Packet()
00081   : header(new Buffer(4)), size(0), type(0), skipped(false),
00082     pcmHeader(new Buffer(4)), sampleRate(0), sampleBits(0),
00083     samples(new Buffer(0)) {}
00084     
00085 SpeakerServer::Packet::~Packet() {
00086   delete header;
00087   delete pcmHeader;
00088   delete samples;
00089 }
00090 
00091 int SpeakerServer::GotSocketData(char* data, int dataSize) {
00092   while (dataSize > 0) {
00093     if (!packet.header->IsFull()) {
00094       if (!packet.header->Fill(data, dataSize)) {
00095         break;
00096       }
00097       packet.size =
00098         (unsigned short) GetShort(&packet.header->GetData()[0]);
00099       packet.type = GetShort(&packet.header->GetData()[2]);
00100       if ((packet.type == 0) && (packet.size >= 4)) {
00101         // PCM packet
00102         packet.pcmHeader->SetPosition(0);
00103         packet.skipped = false;
00104       } else {
00105         packet.skipped = true;
00106       }
00107     }
00108     
00109     if (packet.skipped) {
00110       if (packet.size > dataSize) {
00111         packet.size -= dataSize;
00112         break;
00113       } else {
00114         data += packet.size;
00115         dataSize -= packet.size;
00116         // Start reading next packet
00117         packet.header->SetPosition(0);
00118         continue;
00119       }
00120     }
00121     
00122     if (!packet.pcmHeader->IsFull()) {
00123       if (!packet.pcmHeader->Fill(data, dataSize)) {
00124         break;
00125       }
00126       packet.size -= packet.pcmHeader->GetLimit();
00127       packet.sampleRate =
00128         (unsigned short) GetShort(&packet.pcmHeader->GetData()[0]);
00129       packet.sampleBits = packet.pcmHeader->GetData()[2];
00130       
00131       const int resampledSize =
00132         packet.size *
00133           ((packet.sampleBits == 8) ? 2 : 1)
00134           * config->sound.sample_rate / packet.sampleRate;
00135       
00136       if ((packet.size > MAX_PACKET_SIZE)
00137             || (resampledSize > MAX_PACKET_SIZE)) {
00138         // Too many samples
00139         packet.skipped = true;
00140         continue;
00141       }
00142       packet.samples->SetCapacity(MAX_PACKET_SIZE);
00143       packet.samples->SetLimit(packet.size);
00144       packet.samples->SetPosition(0);
00145     }
00146         
00147     if (!packet.samples->IsFull()) {
00148       if (!packet.samples->Fill(data, dataSize)) {
00149         break;
00150       }
00151       AddPacket(
00152         packet.samples->GetData(),
00153         packet.samples->GetLimit(),
00154         packet.sampleRate,
00155         packet.sampleBits);
00156       // Start reading next packet
00157       packet.header->SetPosition(0);
00158       continue;
00159     }
00160   }
00161 
00162   return 0;
00163 }
00164 
00165 void SpeakerServer::AddPacket(
00166   const void* samples,
00167   int samplesSize,
00168   int sampleRate,
00169   byte sampleBits) {
00170 
00171   if (samplesSize < 1) {
00172     return;
00173   }
00174 
00175   samples =
00176     ResampleForSpeaker(
00177       samples, samplesSize, sampleRate, sampleBits, samplesSize);
00178   if (samples == 0) {
00179     return;
00180   }
00181   
00182   const int frameSize =
00183     config->sound.streaming.speaker_frame_length
00184       * config->sound.sample_rate
00185       * ((config->sound.sample_bits == 8) ? 1 : 2)
00186       / 1000;
00187   if (frame == 0) {
00188     frame = new Buffer(frameSize);
00189   } else if (frameSize != frame->GetLimit()) {
00190     if (frame->GetPosition() > frameSize) {
00191       QueueFrame(frame->GetData(), frame->GetPosition());
00192       frame->SetPosition(0);
00193     } else {
00194       frame->SetLimit(frameSize);
00195     }
00196     
00197     if (frame->GetLimit() < frameSize) {
00198       if (frame->GetCapacity() < frameSize) {
00199         frame->SetCapacity(frameSize);
00200       }
00201       frame->SetLimit(frameSize);
00202     }
00203   }
00204   
00205   if (frame->GetLimit() < 1) {
00206     return;
00207   }
00208   
00209   const char* buf = (const char*) samples;
00210   while (frame->Fill(buf, samplesSize)) {
00211     QueueFrame(frame->GetData(), frame->GetPosition());
00212     frame->SetPosition(0);
00213   }
00214 }
00215     
00216     
00217 void SpeakerServer::QueueFrame(const char* samples, int samplesSize) {
00218   if (channel != SoundManager::invalid_Play_ID) {
00219     const int remainingTime = sndman->GetRemainTime(channel) - RobotInfo::SoundBufferTime;
00220     if (remainingTime > (int) config->sound.streaming.speaker_max_delay) {
00221       // Queue too long
00222       sndman->StopPlay(channel);
00223       channel = SoundManager::invalid_Play_ID;
00224     } else if (remainingTime < 0) {
00225       // Queue underrun
00226     } else {
00227       // Try queueing
00228       SoundManager::Play_ID newChannel =
00229         sndman->ChainBuffer(channel, samples, (int) samplesSize);
00230       if (newChannel != SoundManager::invalid_Play_ID) {
00231         channel = newChannel;
00232         return;
00233       }
00234     }
00235   }
00236   
00237   // Start a new channel
00238   channel = sndman->PlayBuffer(samples, samplesSize);
00239 }
00240 
00241 const void* SpeakerServer::ResampleForSpeaker(
00242   const void* samples,
00243   int samplesSize,
00244   int sampleRate,
00245   byte bitsPerSample,
00246   int& newSamplesSize) {
00247     
00248   const int newSampleRate = config->sound.sample_rate;
00249   const int newBitsPerSample = config->sound.sample_bits;
00250   
00251   if ((sampleRate == newSampleRate) && (bitsPerSample == newBitsPerSample)) {
00252     newSamplesSize = samplesSize;
00253     return samples;
00254   }
00255   
00256   const int sampleCount = samplesSize / ((bitsPerSample == 16) ? 2 : 1);  
00257   const int newSampleCount = sampleCount * newSampleRate / sampleRate;
00258   newSamplesSize = newSampleCount * ((newBitsPerSample == 16) ? 2 : 1);
00259   
00260   if (newSampleCount == 0) {
00261     newSamplesSize = 0;
00262     return 0;
00263   }
00264   
00265   if (resampled == 0) {
00266     resampled = new Buffer(newSamplesSize);
00267   } else if (resampled->GetCapacity() < newSamplesSize) {
00268     resampled->SetCapacity(newSamplesSize);
00269   }
00270   
00271   // The code below is biased towards upsampling (normal case).
00272   // It does not average samples during downsampling.
00273   
00274   if (bitsPerSample == 16) {
00275     // 16-bit signed source
00276     const short* source = (const short*) samples; 
00277     if (newBitsPerSample == 16) {
00278       // 16-bit signed source, 16-bit signed destination
00279       short* dest = (short*) resampled->GetData();
00280       for (int i = 0; i < newSampleCount; i++) {  
00281         dest[i] = source[i * sampleRate / newSampleRate];
00282       }
00283     } else if (newBitsPerSample == 8) {
00284       // 16-bit signed source, 8-bit unsigned destination
00285       byte* dest = (byte*) resampled->GetData();
00286       for (int i = 0; i < newSampleCount; i++) {  
00287         dest[i] = ((byte) (source[i * sampleRate / newSampleRate] >> 8)) ^ 0x80;
00288       }
00289     } else {
00290       newSamplesSize = 0;
00291       return 0;
00292     }
00293   } else if (bitsPerSample == 8) {
00294     // 8-bit unsigned source
00295     const byte* source = (const byte*) samples;
00296     if (newBitsPerSample == 8) {
00297       // 8-bit unsigned source, 8-bit unsigned destination
00298       byte* dest = (byte*) resampled->GetData();
00299       for (int i = 0; i < newSampleCount; i++) {  
00300         dest[i] = source[i * sampleRate / newSampleRate];
00301       }
00302     } else if (newBitsPerSample == 16) {
00303       // 8-bit unsigned source, 16-bit signed destination
00304       short* dest = (short*) resampled->GetData();
00305       for (int i = 0; i < newSampleCount; i++) {  
00306         dest[i] = (source[i * sampleRate / newSampleRate] ^ 0x80) << 8;
00307       }
00308     } else {
00309       newSamplesSize = 0;
00310       return 0;
00311     }
00312   } else {
00313     newSamplesSize = 0;
00314     return 0;
00315   }
00316   
00317   return resampled->GetData();
00318 }

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4