acoustics.cc

机器人仿真平台,和stage配合运行

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  Audiodevice attempted written by Esben Ostergaard
 *
 * This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <sys/soundcard.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <netinet/in.h>
#include <gsl/gsl_fft_real.h>

#include "playercommon.h"
#include "drivertable.h"
#include "player.h"

#define DEFAULT_DEVICE "/dev/dsp"
#define MIN_FREQUENCY 800

class Acoustics : public CDevice
{
  public:
    // Constructor
    Acoustics(char* interface, ConfigFile* cf, int section);

    int Setup();
    int Shutdown();

    size_t GetCommand(void* dest, size_t maxsize);

  private:

    // Open or close the device
    int OpenDevice( int flag );
    int CloseDevice();

    // The main loop
    virtual void Main();

    // Get and set the configuration of the driver
    int SetConfiguration(int len, void* client, unsigned char buffer[]);
    int GetConfiguration(int len, void* client, unsigned char buffer[]);
   
    // Set various attributes of the audio device
    int SetSampleFormat(int _format);
    int SetSampleRate(int _rate);
    int SetChannels(unsigned short _channels);
    int SetBufferSize(int _size);

    // Functions for finding primary frequencies
    int ListenForTones();
    void InsertPeak(int f, int a);

    // Record and playback
    int Record();
    int PlayBuffer(char* buffer,unsigned int size);


    // Create a sine wave
    void CreateSine(unsigned short freq, unsigned short amp, 
        unsigned int duration, char* buffer, unsigned int bufferSize);

    // Create a BPSK chirp
    void CreateChirp(unsigned char mseq[],unsigned short mseqSize, 
        unsigned short freq, unsigned short amp, unsigned int pulseTime, 
        char* buffer, unsigned int bufSize );

    unsigned int CalcBuffSize( unsigned int duration );

    int audioFD; // File descriptor for the device
    const char* deviceName; // Name of the device( ex: "/dev/dsp" )
    int openFlag; // Flag for WRITE or READ mode
    int channels; // Number of channels( 1 || 2 )
    int sampleFormat; // Defines format: sample size, endian convention
    int sampleRate; // In Hertz
    int audioBuffSize; // The size of the buffer, should be a power of two
    char* audioBuffer; // The buffer used to hold audio data
    float bytesPerSample; // The number of bytes per sample

    char* waveCreatBuff; // A buffer used to create sound waves

    unsigned short* peakFreq; // List of peak frequencies
    unsigned short* peakAmp; // List of peak frequency amplitudes
    int N; // The length(in bytes) of the audio buffer to process
    short nHighestPeaks; // Number of peaks to find
    player_audiodsp_data_t data; // The data to return to the user
};

Acoustics::Acoustics(char* interface, ConfigFile* cf, int section)
  : CDevice(sizeof(player_audiodsp_data_t),sizeof(player_audiodsp_cmd_t),1,1),
  audioFD(-1),deviceName(NULL),openFlag(-1),channels(1),sampleFormat(16),
  sampleRate(8000),audioBuffSize(4096),audioBuffer(NULL),bytesPerSample(1),
  peakFreq(NULL),peakAmp(NULL),N(1024),nHighestPeaks(5)
{
  deviceName = cf->ReadString(section,"device",DEFAULT_DEVICE);
}

CDevice* Acoustics_Init(char* interface, ConfigFile* cf, int section)
{
  if(strcmp(interface, PLAYER_AUDIODSP_STRING))
  {
    PLAYER_ERROR1("driver \"acoustics\" does not support interface \"%s\"\n", 
                  interface);
    return(NULL);
  }
  else
    return((CDevice*)(new Acoustics(interface, cf, section)));
}

void Acoustics_Register(DriverTable* table)
{
  table->AddDriver("acoustics", PLAYER_ALL_MODE, Acoustics_Init );
}

int Acoustics::Setup()
{

  this->N=1024;
  this->nHighestPeaks = 5;
  this->peakFreq = new unsigned short[this->nHighestPeaks];
  this->peakAmp = new unsigned short[this->nHighestPeaks];

  puts("audio ready");

  StartThread();
  return 0;
}

int Acoustics::Shutdown()
{
  StopThread();
  this->CloseDevice();

  delete [] this->peakFreq;
  delete [] this->peakAmp;

  puts("Acoustics has been shutdown");
  return 0;
}

size_t Acoustics::GetCommand(void* dest, size_t maxsize)
{
  int retval = device_used_commandsize;

  if(device_used_commandsize)
  {
    memcpy(dest,device_command,device_used_commandsize);
    device_used_commandsize = 0;
  }
  return(retval);
}

int Acoustics::OpenDevice( int flag )
{
  assert(flag==O_RDONLY || flag==O_WRONLY);

  // We don't need to reopen the device if the flag is the same
  if( this->openFlag != flag )
  {
    this->openFlag = flag;

    // First close the device
    close(audioFD);

    // Then open it again with the new flag
    if( (audioFD = open(deviceName,this->openFlag)) == -1 )
    {
      PLAYER_ERROR1("failed to open audio device %s",deviceName);
      return -1;
    }

  }

  return 0;
}

int Acoustics::CloseDevice()
{
  this->openFlag = -1;
  return close(audioFD);
}

void Acoustics::Main()
{
  int len=0;
  void *client=NULL;
  unsigned char configBuffer[PLAYER_MAX_REQREP_SIZE];
  unsigned char cmdBuffer[sizeof(player_audiodsp_cmd)];
  char* playBuffer = NULL;
  unsigned int playBufferSize = 0;

  player_audiodsp_cmd_t audioCmd;

  sleep(1);

  cmdBuffer[0]=0x0;

  while(true)
  {
    // test if we are suppose to cancel
    pthread_testcancel();

    // Set/Get the configuration
    while((len = GetConfig(&client, &configBuffer, sizeof(configBuffer))) > 0)
    {

      switch(configBuffer[0])
      {
        case PLAYER_AUDIODSP_SET_CONFIG:
          this->SetConfiguration(len,client,configBuffer);
          break;

        case PLAYER_AUDIODSP_GET_CONFIG:
          this->GetConfiguration(len,client,configBuffer);
          break;

        default:
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
            PLAYER_ERROR("PutReply() failed");
          break;
      }

    }

    // Get the next command
    memset(&cmdBuffer,0,sizeof(cmdBuffer));
    len = this->GetCommand(&cmdBuffer,sizeof(cmdBuffer));

    // Process the command
    switch(cmdBuffer[0])
    {
      case PLAYER_AUDIODSP_PLAY_TONE:
        {
          memcpy(&audioCmd, cmdBuffer, sizeof(audioCmd));

          playBufferSize = this->CalcBuffSize( ntohl(audioCmd.duration) );
          if( playBuffer ) delete [] playBuffer;
          playBuffer = new char[playBufferSize];

          // Create a tone
          this->CreateSine(ntohs(audioCmd.frequency), ntohs(audioCmd.amplitude), 
              ntohl(audioCmd.duration), playBuffer, playBufferSize);

          // Play the sound
          this->PlayBuffer(playBuffer,playBufferSize);
          break;
        }

      case PLAYER_AUDIODSP_PLAY_CHIRP:
        {
          memcpy(&audioCmd, cmdBuffer, sizeof(audioCmd));

          playBufferSize = ntohs(audioCmd.bitStringLen)*
            this->CalcBuffSize(ntohl(audioCmd.duration));

          if( playBuffer ) delete [] playBuffer;
          playBuffer = new char[playBufferSize];

          // Create a chirp
          this->CreateChirp(audioCmd.bitString,ntohs(audioCmd.bitStringLen),
              ntohs(audioCmd.frequency), ntohs(audioCmd.amplitude), 
              ntohl(audioCmd.duration), playBuffer, playBufferSize);

          // Play the chirp
          this->PlayBuffer(playBuffer,playBufferSize);
          break;
        }

        // Replay the last buffer
      case PLAYER_AUDIODSP_REPLAY:
        {
          memcpy(&audioCmd, cmdBuffer, sizeof(audioCmd));
          this->PlayBuffer(playBuffer,playBufferSize);
          break;
        }

        // The default is to listen for tones and report the findings
      default:
        {

          // Get the most significant frequencies
          if( !ListenForTones() )
          {
            for (int i=0; i<this->nHighestPeaks; i++) 
            {
              this->data.freq[i]=htons((unsigned short)((this->peakFreq[i]*this->sampleRate)/this->N));
              this->data.amp[i]=htons((unsigned short)this->peakAmp[i]);
            }

            // Return the data to the user
            PutData((uint8_t*)&this->data, sizeof(this->data),0,0);
          }

          break;
        }
    }
  }

  if( playBuffer ) 
    delete [] playBuffer;
}

int Acoustics::SetConfiguration(int len, void* client, unsigned char buffer[])
{

  player_audiodsp_config_t config;

  if( len != sizeof(player_audiodsp_config_t))
  {
    PLAYER_ERROR2("config request len is invalid (%d != %d)", len, 
        sizeof(config));
    if( PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return 1;
  }

  memcpy(&config, buffer, sizeof(config));
  this->channels = config.channels;

  // Must open the device for write in order to configure it
  this->OpenDevice(O_WRONLY);

  // Attempts to set the format and rate of each sample along with
  // the number of channels to use.
  if( this->SetSampleFormat( ntohs(config.sampleFormat)) == 0 &&
      this->SetChannels( ntohs(config.channels)) == 0 && 
      this->SetSampleRate( ntohs(config.sampleRate)) == 0 )
  {
    // Create the audio buffer
    this->SetBufferSize(0);

    if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &config, 
          sizeof(config)) != 0)
      PLAYER_ERROR("PutReply() failed");

    return -1;

  } else {
    if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
  }

  return 0;
}

int Acoustics::GetConfiguration(int len, void* client, unsigned char buffer[])