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

/**
  * @ingroup drivers
  * @{ */

/**
  *@defgroup driver_sphere sphere
  @brief Logitech Sphere camera

  * This driver is a heavily modified version of the camerav4l one developed
  * by Andrew Howard.  The sphere driver captures images from a Logitech Sphere
  * cameras.  It differs from a normal Video4Linux device in that it has a
  * built in Pan/Tilt/Zoom unit, so this driver supports multiple interfaces.

  * @date 1.2.2005

@image html sphere.png

@par Compile-time dependencies

- <linux/videodev.h>

@par Provides

- @ref interface_camera
- @ref interface_ptz

@par Requires

- none

@par Configuration requests

- none

@par Configuration file options

- port (string)
  - Default: "/dev/video0"
  - %Device to read video data from.

- size (integer tuple)
  - Default: 320 x 240
  - Desired image size.   This may not be honoured if the driver does
    not support the requested size).

- mode (string)
  - Default: "RGB888"
  - Desired capture mode.  Can be one of:
    - GREY (8-bit monochrome)
    - RGB24P (24-bit planar RGB)
    - RGB888 (24-bit RGB)
    - YUV420P (planar YUV data; not currently implemented)

- save (integer)
  - Default: 0
  - Debugging option: set this to write each frame as an image file on disk.

- framerate (integer)
  - Default: 10
  - The framerate (0..63Hz)

- shutter_speed (integer)
  - Default: 64000
  - The shutter speed (1..65535)

- compression_preference (integer)
  - Default: 2
  - Set compression preference (0..2)

- automatic_gain (integer)
  - Default: 10000
  - Set automatic gain control (0..65535)

- automatic_wb (string)
  - Default: "auto"
  - Set automatic wb mode (auto/manual/indoor/outdoor/fl)

- sharpness (integer)
  - Default: -1
  - The image sharpness (0...65535)

- backlight (integer)
  - Default: 0
  - Set backlight compensation (0=off, other=on)

- flicker (integer)
  - Default: 0
  - Set antiflicker mode (0=off, other=on)

- noise_reduction (integer)
  - Default: 0
  - Set noise reduction mode (0=none...3=high)

- dump_settings (integer)
  - Default: 0
  - Output settings to display when driver starts (0=off, other=on)

- debug (integer)
  - Default: 0
  - Display a copy of the image on localhost (0=off, other=on)

@par Example

@verbatim
driver
(
  name "spheredriver"
  provides ["camera:0"
            "ptz:0"]
  port "/dev/video0"
  size [320 240]
  mode "RGB24"
  automatic_wb "fl"
  framerate 30
  shutter_speed   64000
  automatic_gain  10000
  dump_settings 0
)
@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 "sphere_mixed.h"

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

extern "C" {
#include "setpwc_api.h"
}

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

const timespec NSLEEP_TIME = {0, 10000000}; // (0s, 10 ms) => max 100 fps

////////////////////////////////////////////////////////////////////////////////
// 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*
SphereDriver_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return ((Driver*)(new SphereDriver(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 SphereDriver_Register(DriverTable* table)
{
  table->AddDriver("sphere", SphereDriver_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
SphereDriver::SphereDriver(ConfigFile* cf, int section)
    : Driver(cf, section),
    mPanMin(0),
    mPanMax(1),
    mTiltMin(0),
    mTiltMax(1),
    mDevice(NULL),
    mFg(NULL),
    mFrame(NULL),
    mRGBFrame(NULL),
    mFrameNumber(0),
    mWidth(0),
    mHeight(0),
    mDepth(0)
{
  // Create camera interface
  if (0 !=
    cf->ReadDeviceAddr(&(mCameraAddr),section,"provides",PLAYER_CAMERA_CODE,-1,NULL))
  {
    PLAYER_ERROR("Could not read camera ID ");
    SetError(-1);
    return;
  }
  if (0 != AddInterface(mCameraAddr))
  {
    PLAYER_ERROR("Could not add camera interface ");
    SetError(-1);
    return;
  }

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

  /// @todo is there a replacement clear command?
  //ClearCommand(mPtzAddr);

  // Read options from the configuration file
  mSleep           =
    static_cast<int32_t>(rint(1e6/cf->ReadInt(section, "frequency", 100)));

  mDevice    = cf->ReadString(section, "port", "/dev/video0");
  mWidth     = cf->ReadTupleInt(section, "size", 0, 320);
  mHeight    = cf->ReadTupleInt(section, "size", 1, 240);
  mPalette   = cf->ReadString(section, "mode", "RGB888");
  mSave      = cf->ReadInt(section, "save", 0);
  mFrameRate = cf->ReadInt(section, "framerate", 10);

  // Not sure what a good default value is here
  mShutterSpeed          = cf->ReadInt(section, "shutter_speed", 64000);
  mCompressionPreference = cf->ReadInt(section, "compression_preference", 2);
  mAutomaticGain         = cf->ReadInt(section, "automatic_gain", 10000);
  mAutomaticWb           = cf->ReadString(section, "automatic_wb", "auto");
  mSharpness             = cf->ReadInt(section, "sharpness", -1);
  mBacklight             = cf->ReadInt(section, "backlight", 0);
  mFlicker               = cf->ReadInt(section, "flicker", 0);
  mNoiseReduction        = cf->ReadInt(section, "noise_reduction", 0);
  mDumpSettings          = cf->ReadInt(section, "dump_settings", 0);
  mDebug                 = cf->ReadInt(section, "debug", 0);

  return;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int SphereDriver::Setup()
{
  puts("Setting up sphere driver");
  mFg = fg_open(mDevice);
  if (mFg == NULL)
  {
    PLAYER_ERROR("Setup():  unable to open ");
    return -1;
  }

  fg_set_format(mFg, VIDEO_PALETTE_YUV420P);
  mFrame = frame_new(mWidth, mHeight, VIDEO_PALETTE_YUV420P );

  if (0 == strcasecmp(mPalette, "GREY"))
  {
    // We don't actually have a greyscale mode, but we can just return
    // the Y component of our YUV.
    mCameraData.format = PLAYER_CAMERA_FORMAT_MONO8;
    mDepth = 8;
  }
  else if (0 == strcasecmp(mPalette, "YUV420P"))
  {
  // What should we have here?
  //    mCameraData.format = PLAYER_CAMERA_FORMAT_YUV420P;
  //    mDepth = 24;
  }
  else if (0 == strcasecmp(mPalette, "RGB24P"))
  {
    mRGBFrame = frame_new(mWidth, mHeight, VIDEO_PALETTE_RGB24 );
    mCameraData.format = PLAYER_CAMERA_FORMAT_RGB;
    mDepth = 24;
  }
  else if (0 == strcasecmp(mPalette, "RGB888"))
  {
    mRGBFrame = frame_new(mWidth, mHeight, VIDEO_PALETTE_RGB24 );
    mCameraData.format = PLAYER_CAMERA_FORMAT_RGB888;
    mDepth = 24;
  }
  else
  {
    PLAYER_ERROR("Palette is not supported ");
    return 1;
  }

  fg_set_capture_window(mFg, 0, 0, mWidth, mHeight);

  // Setup the camera based on configuration options
  set_framerate(mFg->fd, mFrameRate);
  set_shutter_speed(mFg->fd, mShutterSpeed);
  set_compression_preference(mFg->fd, mCompressionPreference);
  set_automatic_gain_control(mFg->fd, mAutomaticGain);
  set_automatic_white_balance_mode(mFg->fd, const_cast<char*>(mAutomaticWb));
  set_sharpness(mFg->fd, mSharpness);
  set_backlight_compensation(mFg->fd, mBacklight);
  set_antiflicker_mode(mFg->fd, mFlicker);
  set_noise_reduction(mFg->fd, mNoiseReduction);

  int32_t min, max;
  get_pan_or_tilt_limits(mFg->fd, GET_PAN, &min, &max);
  mPanMin = DEG2RAD(min/100.0);
  mPanMax = DEG2RAD(max/100.0);
  get_pan_or_tilt_limits(mFg->fd, GET_TILT, &min, &max);
  mTiltMin = DEG2RAD(min/100.0);
  mTiltMax = DEG2RAD(max/100.0);

  if (mDebug)
  {
    printf("min, max:\n");
    printf("pan      %.3f %.3f\n", mPanMin, mPanMax);
    printf("tilt     %.3f %.3f\n", mTiltMin, mTiltMax);
  }

  reset_pan_tilt(mFg->fd, SET_PAN);
  reset_pan_tilt(mFg->fd, SET_TILT);

  if (FALSE!=mDumpSettings)
    dump_current_settings(mFg->fd);

  puts("Sphere driver ready");

  // Start the device thread; spawns a new thread and executes
  // SphereDriver::Main(), which contains the main loop for the driver.
  StartThread();

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int SphereDriver::Shutdown()
{
  puts("Shutting Sphere driver down");

  // Stop and join the driver thread
  StopThread();

  // Set to 0,0
  set_pan_or_tilt(mFg->fd, SET_PAN, 0);
  set_pan_or_tilt(mFg->fd, SET_TILT, 0);

  // Free resources
  if (mFrame != NULL)
    frame_release(mFrame);
  if (mRGBFrame != NULL)
    frame_release(mRGBFrame);
  if (mFg != NULL)
    fg_close(mFg);

  // Here you would shut the device down by, for example, closing a
  // serial port.

  puts("Sphere driver has been shutdown");

  return(0);
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void SphereDriver::Main()
{

  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Go to sleep for a while (this is a polling loop)
    nanosleep(&NSLEEP_TIME, NULL);