acoustics.cc

机器人仿真软件

/*
 *  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
 *
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_acoustics acoustics
@brief Analyze and produce audible tones with sound hardware

@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

@todo Write a full description

@par Compile-time dependencies

- GSL
- <sys/soundcard.h>

@par Provides

- @ref interface_audiodsp

@par Requires

- none

@par Configuration requests

- @ref PLAYER_AUDIODSP_SET_CONFIG
- @ref PLAYER_AUDIODSP_GET_CONFIG

@par Configuration file options

- device (string)
  - Default: "/dev/dsp"
  - Audio device to use.

@par Example

@verbatim
driver
(
  name "acoustics"
  provides ["audiodsp:0"]
  device "/dev/dsp""
)
@endverbatim

@author Nate Koenig

*/

/** @} */

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

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



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

class Acoustics : public Driver
{
  public:
    // Constructor
    Acoustics( ConfigFile* cf, int section);
    // Destructor
    ~Acoustics();

    int Setup();
    int Shutdown();

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

    // The main loop
    //virtual void Main();

    // Process incoming messages from clients 
    int ProcessMessage(MessageQueue * resp_queue, 
                       player_msghdr * hdr, void * data);


    // 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(float freq, float amp, 
        float duration, char* buffer, unsigned int bufferSize);

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

    unsigned int CalcBuffSize( float duration );

    // interfaces we might be using
    player_devaddr_t audiodsp_addr;

    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
    double* fft; // reused storage for samples, to be passed into the GSL
  	char * playBuffer;
};

////////////////////////////////////////////////////////////////////////////////
// Create an instance of the driver
Driver* Acoustics_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new Acoustics(cf, section)));
}

////////////////////////////////////////////////////////////////////////////////
// Register the driver
void Acoustics_Register(DriverTable* table)
{
  table->AddDriver("acoustics", Acoustics_Init );
}

////////////////////////////////////////////////////////////////////////////////
// Constructor
Acoustics::Acoustics( ConfigFile* cf, int section)
  : Driver(cf, section),
  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)
{
  memset(&this->audiodsp_addr, 0, sizeof(player_devaddr_t));

  // Create an audiodsp interface
  if (cf->ReadDeviceAddr(&(this->audiodsp_addr), section, "requires",
        PLAYER_AUDIODSP_CODE, -1, NULL) == 0)
  {
    if (this->AddInterface(this->audiodsp_addr))
    {
      this->SetError(-1);
      return;
    }
  }

  playBuffer = NULL;
  this->deviceName = cf->ReadString(section,"device",DEFAULT_DEVICE);
  assert(fft = new double[this->N]);

}

////////////////////////////////////////////////////////////////////////////////
// Destructor
Acoustics::~Acoustics()
{
  delete playBuffer;
  playBuffer = NULL;
  
  if(this->fft)
  {
    delete this->fft;
    this->fft = NULL;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int Acoustics::Setup()
{

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


  // Star the driver thread
  PLAYER_MSG0(2, "running");
  this->StartThread();
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int Acoustics::Shutdown()
{

  PLAYER_MSG0(2, "shutting down");

  this->StopThread();
  this->CloseDevice();

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

  PLAYER_MSG0(2, "shutdown done"); 
  return 0;
}



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(this->deviceName,this->openFlag)) == -1 )
    {
      PLAYER_ERROR1("failed to open audio device %s",this->deviceName);
      return -1;
    }

  }

  return 0;
}

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


////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int Acoustics::ProcessMessage(MessageQueue * resp_queue, 
                       player_msghdr * hdr, void * data)
{
  int playBufferSize=0;
 
  // Set the configuration
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_AUDIODSP_SET_CONFIG, this->audiodsp_addr))
  {
    player_audiodsp_config_t config;

    if( hdr->size != sizeof(player_audiodsp_config_t))
    {
      PLAYER_ERROR2("config request len is invalid (%d != %d)", hdr->size, 
          sizeof(player_audiodsp_config_t));
      return (PLAYER_MSGTYPE_RESP_NACK);
    }

    memcpy(&config, data, 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(config.format) == 0 &&
        this->SetChannels(config.channels) == 0 && 
        this->SetSampleRate((int)config.frequency) == 0 )
    {
      // Create the audio buffer
      this->SetBufferSize(0);

      this->Publish(this->audiodsp_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_AUDIODSP_SET_CONFIG, (void *)&config, sizeof(config), NULL);
      return 0;

    } else {
      this->Publish(this->audiodsp_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_AUDIODSP_SET_CONFIG) ;
      return -1;
    }
  }


  // Return the configuration
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                           PLAYER_AUDIODSP_GET_CONFIG, this->audiodsp_addr))
  {
    player_audiodsp_config_t config;

    if( hdr->size != sizeof(player_audiodsp_config_t) )
    {
      PLAYER_ERROR2("config request len is invalid (%d != %d)", hdr->size, 
          sizeof(player_audiodsp_config_t));
      return (PLAYER_MSGTYPE_RESP_NACK);
    }

    config.format = this->sampleFormat;
    config.frequency = (int)(this->sampleRate);
    config.channels = this->channels;

    this->Publish(this->audiodsp_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_AUDIODSP_GET_CONFIG, (void *)&config, sizeof(config), NULL);

  }


  // Command to play a tone
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_AUDIODSP_PLAY_TONE, this->audiodsp_addr))
  {
  	assert(hdr->size == sizeof(player_audiodsp_cmd_t));
  	player_audiodsp_cmd_t & audioCmd = *reinterpret_cast<player_audiodsp_cmd_t *> (data);
  	
    playBufferSize = this->CalcBuffSize(audioCmd.duration);
    delete playBuffer;
    playBuffer = new char[playBufferSize];
    assert(playBuffer);

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

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


  // Play a chirp
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_AUDIODSP_PLAY_CHIRP, this->audiodsp_addr))
  {
  	assert(hdr->size == sizeof(player_audiodsp_cmd_t));
  	player_audiodsp_cmd_t & audioCmd = *reinterpret_cast<player_audiodsp_cmd_t *> (data);
  	
    int playBufferSize = this->CalcBuffSize(audioCmd.duration);
    delete playBuffer;
    playBuffer = new char[playBufferSize];
    assert(playBuffer);
    
    // Create a chirp
    this->CreateChirp(audioCmd.bit_string, audioCmd.bit_string_count,
         audioCmd.frequency, audioCmd.amplitude, 
         audioCmd.duration, playBuffer, playBufferSize);

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

  // Replay
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_AUDIODSP_REPLAY, this->audiodsp_addr))
  {
    this->PlayBuffer(playBuffer,playBufferSize);
    return 0;
  }
  	
  if(hdr->type == PLAYER_MSGTYPE_CMD)
  {
    // Get the most significant frequencies
    if (!ListenForTones())
    {
      for (int i=0; i<this->nHighestPeaks; i++) 
      {
        this->data.frequency[i]=(this->peakFreq[i]*this->sampleRate)/this->N;
        this->data.amplitude[i]=this->peakAmp[i];
      }

      // Return the data to the user
      this->Publish(this->audiodsp_addr, NULL, PLAYER_MSGTYPE_DATA, 
                    PLAYER_AUDIODSP_DATA_TONES, 
                    (void*)&this->data, sizeof(this->data),NULL);  	
    }
    return 0;  	
  }

  return -1;
}

/*
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),NULL)) > 0)
    {

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

        case PLAYER_AUDIODSP_GET_CONFIG: