camera1394.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
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: 1394 camera capture
// Author: Nate Koenig, Andrew Howard
// Date: 03 June 2004
// CVS: $Id: camera1394.cc,v 1.25.2.2 2006/09/23 00:11:33 gerkey Exp $
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_camera1394 camera1394
 * @brief Firewire camera capture

The camera1394 driver captures images from IEEE1394 (Firewire, iLink)
cameras.  

@par Compile-time dependencies

- libraw1394
- libdc1394_control

@par Provides

- @ref interface_camera

@par Requires

- none

@par Configuration requests

- none

@par Configuration file options

- port (integer)
  - Default: 0
  - The 1394 port the camera is attached to.

- node (integer)
  - Default: 0
  - The node within the port

- framerate (float)
  - Default: 15
  - Requested frame rate (frames/second)

- mode (string)
  - Default: "640x480_yuv422"
  - Capture mode (size and color layour).  Valid modes are:
    - "320x240_yuv422"
    - "640x480_mono"
    - "640x480_yuv422"
    - "640x480_rgb"
    - "800x600_mono"
    - "800x600_yuv422" - will be rescaled to 600x450
    - "1024x768_mono"
    - "1024x768_yuv422" - will be rescaled to 512x384
    - "1280x960_mono"
    - "1280x960_yuv422" - will be rescaled to 640x480
    - "FORMAT7_MODE0" - only available with libdc1394 >= 2.0
  - Currently, all mono modes will produce 8-bit monochrome images unless 
  a color decoding option is provided (see bayer).
  - All yuv422 modes are converted to RGB24
  
- force_raw (integer)
  - Default: 0
  - Force the driver to use (slow) memory capture instead of DMA transfer
  (for buggy 1394 drivers).
  
- save (integer)
  - Default: 0
  - Debugging option: set this to write each frame as an image file on disk.

- bayer (string)
  - Default: None.
  - Bayer color decoding options for cameras such as the Point Grey Dragonfly and Bummblebee. 
  Option activates color decoding and specifies the Bayer color pattern. Valid modes are:
    - "BGGR"
    - "GRBG"
    - "RGGB"
    - "GBRG"

- method (string)    
  - Default: None (or "DownSample" if bayer option is specified)
  - Determines the algorithm used for Bayer coloro decoding. Valid modes are:
    - "DownSample"
    - "Nearest"
    - "Edge"

- brightness (string or unsigned int)
  - Default: None
  - Sets the camera brightness setting. Valid modes are:
    - "auto"
    - any suitable unsigned integer

- exposure (string or unsigned int)
  - Default: None
  - Sets the camera exposure setting. Valid modes are:
    - "auto"
    - any suitable unsigned integer

- shutter (string or unsigned int)
  - Default: None
  - Sets the camera shutter setting. Valid modes are:
    - "auto"
    - any suitable unsigned integer

- gain (string or unsigned int)
  - Default: None
  - Sets the camera gain setting. Valid modes are:
    - "auto"
    - any suitable unsigned integer

- whitebalance (string)
  - Default: None
  - Sets the manual camera white balance setting. Only valid option:
    - a string containing two suitable blue and red value unsigned integers 

- dma_buffers
  - Default: 4
  - the number of DMA buffers to use
@par Example 

@verbatim
driver
(
  name "camera1394"
  provides ["camera:0"]
)
@endverbatim

@author Nate Koenig, Andrew Howard; major code rewrite by Paul Osmialowski, [e-mail]
*/
/** @} */

#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif

#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>       // for atoi(3)
#include <stddef.h>       // for NULL
#include <unistd.h>
#include <libraw1394/raw1394.h>
#if DC1394_DMA_SETUP_CAPTURE_ARGS == 20
#include <dc1394/dc1394_control.h>
#else
#include <libdc1394/dc1394_control.h>
#endif
#include <libplayercore/playercore.h>
#include <libplayercore/error.h>

// for color and format conversion (located in cmvision)
#include "conversions.h"

#define NUM_DMA_BUFFERS 4


// lots of defines are renames in v2 API, mask this so we dont ahve to modify all our code
#if LIBDC1394_VERSION == 0200
// Frame rate enum
#define FRAMERATE_1_875 DC1394_FRAMERATE_1_875
#define FRAMERATE_3_75 DC1394_FRAMERATE_3_75
#define FRAMERATE_7_5 DC1394_FRAMERATE_7_5
#define FRAMERATE_15 DC1394_FRAMERATE_15
#define FRAMERATE_30 DC1394_FRAMERATE_30
#define FRAMERATE_60 DC1394_FRAMERATE_60
#define FRAMERATE_120 DC1394_FRAMERATE_120
#define FRAMERATE_240 DC1394_FRAMERATE_240

// Format - not used in new API
#define FORMAT_VGA_NONCOMPRESSED 0
#define FORMAT_SVGA_NONCOMPRESSED_1 0
#define FORMAT_SVGA_NONCOMPRESSED_2 0
#define FORMAT_6 0
#define FORMAT_7 0

// mode enumneration
#define 	  MODE_160x120_YUV444	  DC1394_VIDEO_MODE_160x120_YUV444
#define 	  MODE_320x240_YUV422	  DC1394_VIDEO_MODE_320x240_YUV422
#define 	  MODE_640x480_YUV411	  DC1394_VIDEO_MODE_640x480_YUV411
#define 	  MODE_640x480_YUV422	  DC1394_VIDEO_MODE_640x480_YUV422
#define 	  MODE_640x480_RGB	  DC1394_VIDEO_MODE_640x480_RGB8
#define 	  MODE_640x480_MONO	  DC1394_VIDEO_MODE_640x480_MONO8
#define 	  MODE_640x480_MONO16	  DC1394_VIDEO_MODE_640x480_MONO16
#define 	  MODE_800x600_YUV422	  DC1394_VIDEO_MODE_800x600_YUV422
#define 	  MODE_800x600_RGB	  DC1394_VIDEO_MODE_800x600_RGB8
#define 	  MODE_800x600_MONO	  DC1394_VIDEO_MODE_800x600_MONO8
#define 	  MODE_1024x768_YUV422	  DC1394_VIDEO_MODE_1024x768_YUV422
#define 	  MODE_1024x768_RGB	  DC1394_VIDEO_MODE_1024x768_RGB8
#define 	  MODE_1024x768_MONO	  DC1394_VIDEO_MODE_1024x768_MONO8
#define 	  MODE_800x600_MONO16	  DC1394_VIDEO_MODE_800x600_MONO16
#define 	  MODE_1024x768_MONO16	  DC1394_VIDEO_MODE_1024x768_MONO16
#define 	  MODE_1280x960_YUV422	  DC1394_VIDEO_MODE_1280x960_YUV422
#define 	  MODE_1280x960_RGB	  DC1394_VIDEO_MODE_1280x960_RGB8
#define 	  MODE_1280x960_MONO	  DC1394_VIDEO_MODE_1280x960_MONO8
#define 	  MODE_1600x1200_YUV422	  DC1394_VIDEO_MODE_1600x1200_YUV422
#define 	  MODE_1600x1200_RGB	  DC1394_VIDEO_MODE_1600x1200_RGB8
#define 	  MODE_1600x1200_MONO	  DC1394_VIDEO_MODE_1600x1200_MONO8
#define 	  MODE_1280x960_MONO16	  DC1394_VIDEO_MODE_1280x960_MONO16
#define 	  MODE_1600x1200_MONO16	  DC1394_VIDEO_MODE_1600x1200_MONO16
#define 	  MODE_EXIF	          DC1394_VIDEO_MODE_EXIF
#define 	  MODE_FORMAT7_0	  DC1394_VIDEO_MODE_FORMAT7_0
#define 	  MODE_FORMAT7_1	  DC1394_VIDEO_MODE_FORMAT7_1
#define 	  MODE_FORMAT7_2	  DC1394_VIDEO_MODE_FORMAT7_2
#define 	  MODE_FORMAT7_3	  DC1394_VIDEO_MODE_FORMAT7_3
#define 	  MODE_FORMAT7_4	  DC1394_VIDEO_MODE_FORMAT7_4
#define 	  MODE_FORMAT7_5	  DC1394_VIDEO_MODE_FORMAT7_5
#define 	  MODE_FORMAT7_6	  DC1394_VIDEO_MODE_FORMAT7_6
#define 	  MODE_FORMAT7_7	  DC1394_VIDEO_MODE_FORMAT7_7

// Feature enumeration
#define	  FEATURE_BRIGHTNESS	  DC1394_FEATURE_BRIGHTNESS
#define	  FEATURE_EXPOSURE	  DC1394_FEATURE_EXPOSURE
#define	  FEATURE_SHARPNESS	  DC1394_FEATURE_SHARPNESS
#define	  FEATURE_WHITE_BALANCE	  DC1394_FEATURE_WHITE_BALANCE
#define	  FEATURE_HUE	  DC1394_FEATURE_HUE
#define	  FEATURE_SATURATION	  DC1394_FEATURE_SATURATION
#define	  FEATURE_GAMMA	  DC1394_FEATURE_GAMMA
#define	  FEATURE_SHUTTER	  DC1394_FEATURE_SHUTTER
#define	  FEATURE_GAIN	  DC1394_FEATURE_GAIN
#define	  FEATURE_IRIS	  DC1394_FEATURE_IRIS
#define	  FEATURE_FOCUS	  DC1394_FEATURE_FOCUS
#define	  FEATURE_TEMPERATURE	  DC1394_FEATURE_TEMPERATURE
#define	  FEATURE_TRIGGER	  DC1394_FEATURE_TRIGGER
#define	  FEATURE_TRIGGER_DELAY	  DC1394_FEATURE_TRIGGER_DELAY
#define	  FEATURE_WHITE_SHADING	  DC1394_FEATURE_WHITE_SHADING
#define	  FEATURE_FRAME_RATE	  DC1394_FEATURE_FRAME_RATE
#define	  FEATURE_ZOOM	  DC1394_FEATURE_ZOOM
#define	  FEATURE_PAN	  DC1394_FEATURE_PAN
#define	  FEATURE_TILT	  DC1394_FEATURE_TILT
#define	  FEATURE_OPTICAL_FILTER	  DC1394_FEATURE_OPTICAL_FILTER
#define	  FEATURE_CAPTURE_SIZE	  DC1394_FEATURE_CAPTURE_SIZE
#define	  FEATURE_CAPTURE_QUALITY	  DC1394_FEATURE_CAPTURE_QUALITY

// speed enumerations
#define DC1394_SPEED_100 DC1394_ISO_SPEED_100
#define DC1394_SPEED_200 DC1394_ISO_SPEED_200,
#define DC1394_SPEED_400 DC1394_ISO_SPEED_400,
#define DC1394_SPEED_800 DC1394_ISO_SPEED_800,
#define DC1394_SPEED_1600 DC1394_ISO_SPEED_1600,
#define DC1394_SPEED_3200 DC1394_ISO_SPEED_3200

#endif

// Driver for detecting laser retro-reflectors.
class Camera1394 : public Driver
{
  // Constructor
  public: Camera1394( ConfigFile* cf, int section);

  // Setup/shutdown routines.
  public: virtual int Setup();
  public: virtual int Shutdown();
  private: void SafeCleanup();

  // Main function for device thread.
  private: virtual void Main();

  // This method will be invoked on each incoming message
  public: virtual int ProcessMessage(MessageQueue* resp_queue,
                                     player_msghdr * hdr,
                                     void * data);

  // Save a frame to memory
  private: int GrabFrame();

  private: unsigned char * resized;//[1280 * 960 * 3];

  // Save a frame to disk
  private: int SaveFrame( const char *filename );

  // Update the device data (the data going back to the client).
  private: void RefreshData();

  // Video device
  private: unsigned int port;
  private: unsigned int node;
  private: raw1394handle_t handle;
#if LIBDC1394_VERSION == 0200
  private: dc1394camera_t * camera;
  // Camera features
  private: dc1394featureset_t features;
  private: dc1394format7modeset_t modeset;
#else  
  private: dc1394_cameracapture camera;
  // Camera features
  private: dc1394_feature_set features;
#endif


  // Capture method: RAW or VIDEO (DMA)
  private: enum {methodRaw, methodVideo, methodNone};
  private: int method;
  private: bool forceRaw;

#if LIBDC1394_VERSION == 0200
  private: dc1394framerate_t frameRate;
  private: unsigned int format;
  private: dc1394video_mode_t mode;
#else
  private: unsigned int frameRate;
  private: unsigned int format;
  private: unsigned int mode;
#endif

  // number of DMA buffers to use
  private: unsigned int num_dma_buffers;

  // Write frames to disk?
  private: int save;

  // Frame capture size
  private: size_t frameSize;

  // Capture timestamp
  private: struct timeval frameTime;
  
  // Data to send to server
  private: player_camera_data_t data;

  // Bayer Colour Conversion
  private: bool DoBayerConversion;
  private: int BayerPattern;
  private: int BayerMethod;

  // Camera settings
  private: bool setBrightness, setExposure, setWhiteBalance, setShutter, setGain;
  private: bool autoBrightness, autoExposure, autoShutter, autoGain;
  private: unsigned int brightness, exposure, redBalance, blueBalance, shutter, gain;

};


// Initialization function
Driver* Camera1394_Init( ConfigFile* cf, int section)
{
  return ((Driver*) (new Camera1394( cf, section)));
}


// a driver registration function
void Camera1394_Register(DriverTable* table)
{
  table->AddDriver("camera1394", Camera1394_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
Camera1394::Camera1394(ConfigFile* cf, int section)
  : Driver(cf,
           section,
           true,
           PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
           PLAYER_CAMERA_CODE)
{
  float fps;

  resized=NULL;

  this->handle = NULL;
  this->method = methodNone;

  // The port the camera is attached to
  this->port = cf->ReadInt(section, "port", 0);

  // The node inside the port
  this->node = cf->ReadInt(section, "node", 0);

  // Video frame rate
  fps = cf->ReadFloat(section, "framerate", 15);
  if (fps < 3.75)
    this->frameRate = FRAMERATE_1_875;
  else if (fps < 7.5)
    this->frameRate = FRAMERATE_3_75;
  else if (fps < 15)
    this->frameRate = FRAMERATE_7_5;
  else if (fps < 30)
    this->frameRate = FRAMERATE_15;
  else if (fps < 60)
    this->frameRate = FRAMERATE_30;
  else
    this->frameRate = FRAMERATE_60;

  // Get uncompressed video
  this->format = FORMAT_VGA_NONCOMPRESSED;
    
  // Image size. This determines the capture resolution. There are a limited
  // number of options available. At 640x480, a camera can capture at
  // _RGB or _MONO or _MONO16.  
  const char* str;
  str =  cf->ReadString(section, "mode", "640x480_yuv422");
  /*
  if (0==strcmp(str,"160x120_yuv444"))
  {
    this->mode = MODE_160x120_YUV444;
    this->frameSize = 160 * 120 * 2; // Is this correct?
  }
  */
  if (0==strcmp(str,"320x240_yuv422"))
  {
    this->mode = MODE_320x240_YUV422;
    this->frameSize = 320 * 240 * 3;
  }
  /*
  else if (0==strcmp(str,"640x480_mono16"))
  {
    this->mode = MODE_640x480_MONO16;
    assert(false);
  }
  else if (0==strcmp(str,"640x480_yuv411"))
  {
    this->mode = MODE_640x480_YUV411;
  }
  */
  else if (0==strcmp(str,"640x480_mono"))
  {
    this->mode = MODE_640x480_MONO;
    this->frameSize = 640 * 480 * 1;
  }
  else if (0==strcmp(str,"640x480_yuv422"))
  {
    this->mode = MODE_640x480_YUV422;
    this->frameSize = 640 * 480 * 3;
  }
  else if (0==strcmp(str,"640x480_rgb"))
  {
    this->mode = MODE_640x480_RGB;
    this->frameSize = 640 * 480 * 3;
  }
  else if (0==strcmp(str,"800x600_mono"))
  {
    this->mode = MODE_800x600_MONO;
    this->format = FORMAT_SVGA_NONCOMPRESSED_1;
    this->frameSize = 800 * 600 * 1;
  }
  else if (0==strcmp(str,"800x600_yuv422"))
  {
    this->mode = MODE_800x600_YUV422;
    this->format = FORMAT_SVGA_NONCOMPRESSED_1;
    this->frameSize = 600 * 450 * 3;