garcia_mixed.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey et al.
 *
 *
 *  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
 *
 */

/** Mixed mode driver for Garcia robot by Acroname
  * @author Brad Kratochvil
  * @date 20050915
  * @ingroup drivers
  * @{ */

/**
  *@defgroup driver_garcia garcia
  @brief The Garcia mobile robot

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

The garcia driver captures

@par Compile-time dependencies

- <acpGarcia.h>

@par Provides

- @ref interface_position2d
- @ref interface_ir
- @ref interface_speech
- @ref interface_dio
- @ref interface_power
- @ref interface_ptz (not yet implemented)
- @ref interface_position1d (not yet implemented)


@par Requires

- none

@par Configuration requests

- @ref interface_position2d
  - PLAYER_POSITION_GET_GEOM_REQ
  - PLAYER_POSITION_SET_ODOM_REQ :
  - PLAYER_POSITION_RESET_ODOM_REQ :
  - PLAYER_POSITION_POWER_REQ :
  - PLAYER_POSITION_SPEED_PID_REQ :
  - PLAYER_POSITION_POSITION_PID_REQ :
  - PLAYER_POSITION_SPEED_PROF_REQ :
  - PLAYER_IR_GET_GEOM_REQ :

@par Configuration file options

- port (filename)
  - Default: "ttyS0"
  - Path to the serial port
- baud (int)
  - Default: 38400
  - Baudrate of the serial port
- speed (float)
  - Default: 0.7f
  - The speed for speaking the phrase.  Values have a range of 0.0 to 1.0 and
    will be clamped to this range. 0.0 is the slowest and 1.0 is the fastest
    speed for saying the phrase.
- pitch (float)
  - Default: 0.6f
  - The pitch for speaking the phrase.  Values have a range of 0.0 to 1.0 and
    will be clamped to this range. 0.0 is the lowest and 1.0 is the highest
    pitch for saying the phrase.
- volume (float)
  - Default: 1.0f
  - The volume for speaking the phrase.  Values have a range of 0.0 to 1.0 and
    will be clamped to this range. 0.0 is the quietest and 1.0 is the loudest
    volume for saying the phrase.

@par Example

@verbatim
driver
(
  name "garcia"
  provides ["position2d:0"
            "ir:0"
            "dio:0"
            "speech:0"]
  port "ttyS0"
  baud "38400"
)
@endverbatim

@author Brad Kratochvil

*/
/** @} */

// ONLY if you need something that was #define'd as a result of configure
// (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so:
/*
#if HAVE_CONFIG_H
  #include <config.h>
#endif
*/
#include "garcia_mixed.h"

#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <sys/time.h>
#include <time.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>
using namespace std;

#include <iostream> // only used for debugging, so remove when done

const timespec NSLEEP_TIME = {0, 20000000}; // (0s, 20 ms) => max 50 hz

////////////////////////////////////////////////////////////////////////////////
// Now the driver

// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class).  In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver*
GarciaDriver_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return ((Driver*)(new GarciaDriver(cf, section)));

}

// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void
GarciaDriver_Register(DriverTable* table)
{
  table->AddDriver("garcia", GarciaDriver_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
GarciaDriver::GarciaDriver(ConfigFile* cf, int section)
    : Driver(cf, section),
      mLength(0.28),
      mWidth(0.20),
      mWheelBase(0.182),
      mWheelRadius(0.1)
{
  // Create position2d interface
  if (0 != cf->ReadDeviceAddr(&mPos2dAddr, section, "provides",
                              PLAYER_POSITION2D_CODE, -1, NULL))
  {
    PLAYER_ERROR("Could not read position2d ID ");
    SetError(-1);
    return;
  }
  if (0 != AddInterface(mPos2dAddr))
  {
    PLAYER_ERROR("Could not add position2d interface ");
    SetError(-1);
    return;
  }

  // Create ir interface
  if (0 != cf->ReadDeviceAddr(&mIrAddr, section, "provides",
                              PLAYER_IR_CODE, -1, NULL))
  {
    PLAYER_ERROR("Could not read ir ID ");
    SetError(-1);
    return;
  }
  if (0 != AddInterface(mIrAddr))
  {
    PLAYER_ERROR("Could not add ir interface ");
    SetError(-1);
    return;
  }

  // Create speech interface
  if (0 != cf->ReadDeviceAddr(&mSpeechAddr, section, "provides",
                              PLAYER_SPEECH_CODE, -1, NULL))
  {
    PLAYER_ERROR("Could not read speech ID ");
    SetError(-1);
    return;
  }
  if (0 != AddInterface(mSpeechAddr))
  {
    PLAYER_ERROR("Could not add speech interface ");
    SetError(-1);
    return;
  }

  // Create dio interface
  if (0 != cf->ReadDeviceAddr(&(mDioAddr),section,"provides",
                              PLAYER_DIO_CODE,-1,NULL))
  {
    PLAYER_ERROR("Could not read dio ID ");
    SetError(-1);
    return;
  }
  if (0 != AddInterface(mDioAddr))
  {
    PLAYER_ERROR("Could not add dio interface ");
    SetError(-1);
    return;
  }

  // Create power interface
  if (0 != cf->ReadDeviceAddr(&mPowerAddr,
                            section,
                            "provides",
                            PLAYER_POWER_CODE,
                            -1,
                            NULL))
  {
    PLAYER_ERROR("could not read power address");
    SetError(-1);
    return;
  }

  if (0 != AddInterface(mPowerAddr))
  {
    PLAYER_ERROR("could not add power interface");
    SetError(-1);
    return;
  }

  // Read options from the configuration file
  const char* portname = cf->ReadFilename(section, "portname", "ttyS0");
  int baudrate = cf->ReadInt(section, "baudrate", 38400);

  // let's just create the config file wherever we are:
  static FILE* config_file;
  config_file = fopen("garcia_api.config", "a+");

  fprintf(config_file, "portname=%s\n", portname);
  fprintf(config_file, "baudrate=%i\n", baudrate);

  mSpeed = static_cast<float>(cf->ReadFloat(section, "speed", 0.7f));
  mPitch = static_cast<float>(cf->ReadFloat(section, "pitch", 0.6f));
  mVolume = static_cast<float>(cf->ReadFloat(section, "volume", 1.0f));

  fclose(config_file);

  return;
}

GarciaDriver::~GarciaDriver()
{
  // get rid of the Acroname config file
  //remove("garcia_api.config");
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int
GarciaDriver::Setup()
{

  cout << "Setting up Garcia driver" << flush;
  mGarcia = new acpGarcia;

  while (!mGarcia->getNamedValue("active")->getBoolVal())
  {
    cout << "." << flush;
    nanosleep(&NSLEEP_TIME, NULL);
  }

  // enable the IR sensors
  acpValue enable(1);
  mGarcia->setNamedValue("front-ranger-enable", &enable);
  mGarcia->setNamedValue("side-ranger-enable", &enable);
  mGarcia->setNamedValue("rear-ranger-enable", &enable);


  puts("finished!");