sonyvid30.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  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: A Sony VID30 PTZ camera
 * Author: Nate Koenig
 * Date: 16 July 2003
 * CVS: $Id: SonyVID30.cc,v 1.59 2006/02/22 15:29:24 natepak Exp $
 */

/// @addtogroup models 
/// @{ 
/** @defgroup SonyVID30 Sony VID 30

@htmlinclude SonyVID30_view.html

The SonyVID30 model simulates the Sony VID 30 pan-tilt-zoom camera.


@par libgazebo interfaces

This model supports the @ref ptz and @ref camera interfaces.

@par Player drivers

Pan/tilt/zoom information is available through the %gz_ptz driver;
image data is available through the %gz_camera driver.


@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- updateRate (float, Hz)
  - Frames per second.
  - Default: 10
  
- imageSize (integer tuple, pixels)
  - Image dimensions (width and height).
  - Default: 320 240

- hfov (float, degrees)
  - Horizontal field of view for a perspective lens.
  - Default: 60

- nearClip, farClip (float, meters)
  - Near and far clipping planes; see @ref gazebo_opengl.
  - Default: 0.5 100.0

- renderMethod (string)
  - OpenGL rendering method: SGIX, GLX, XLIB, or AUTO.  See @ref gazebo_opengl.
  - Default: AUTO

- zBufferDepth (integer)
  - Z buffer depth in bits (8, 16, 24 or 32)
	- Default: 16
	
- savePath (string, path)
  - Directory for saving image frames.
  - Default: .

- saveFrames (bool)
  - Set to true to save image frames.
  - Default: false

- useFrustrum (bool)
  - Create a frustrum to provide user feedback in the ObserveCam.
  - Default: true
  
- zoomLimits (float tuple)
  - Minimum and maximum zoom factor.
  - Default: 10 60

- motionGain (float)
  - Servo gain term (P controller on pan, tilt axes).
  - Default: 2

- zoomGain (float)
  - Servo gain term (P controller on zoom axis).
  - Default: 2

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:SonyVID30>
  <id>ptz1</id>
  <xyz>0 0 0</xyz>  
</model:SonyVID30>
@endverbatim

@par Views

@htmlinclude SonyVID30_more_views.html

@par Authors

Nate Koenig, Andrew Howard

*/
/// @}

#include <assert.h>
#include <float.h>
#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "CylinderGeom.hh"
#include "BoxGeom.hh"
#include "FrustrumGeom.hh"
#include "HingeJoint.hh"
#include "Camera/Camera.hh"

#include "SonyVID30.hh"



//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("SonyVID30", SonyVID30);


//////////////////////////////////////////////////////////////////////////////
// Constructor
SonyVID30::SonyVID30( World *world )
    : Model( world )
{
  int i;

  this->camera = new Camera(this->world);
  this->cameraIface = gz_camera_alloc();
  this->ptzIface = gz_ptz_alloc();

  for (i=0; i<8; i++)
    this->bodyGeoms[i] = NULL;

  this->frustrum = NULL;
  this->body = NULL;
  this->middle = NULL;
  this->head = NULL;
  this->lensGeom = NULL;
  this->panJoint = NULL;
  this->tiltJoint = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
SonyVID30::~SonyVID30()
{
  int i;

  delete this->ptzIface;
  delete this->cameraIface;
  delete this->camera;

  if (this->frustrum)
    delete this->frustrum;
  if (this->body)
    delete this->body;
  if (this->middle)
    delete this->middle;
  if (this->head)
    delete this->head;
  if (this->lensGeom)
    delete this->lensGeom;
  if (this->panJoint)
    delete this->panJoint;
  if (this->tiltJoint)
    delete this->tiltJoint;

  for (i=0; i<8; i++)
  {
    if (this->bodyGeoms[i])
      delete this->bodyGeoms;
    this->bodyGeoms[i] = NULL;
  }

  this->ptzIface = NULL;
  this->cameraIface = NULL;
  this->camera = NULL;
  this->frustrum = NULL;
  this->body = NULL;
  this->middle = NULL;
  this->head = NULL;
  this->lensGeom = NULL;
  this->panJoint = NULL;
  this->tiltJoint = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int SonyVID30::Load( WorldFile *file, WorldFileNode *node )
{
  int imgWidth, imgHeight, zDepth;
  double nearClip, farClip;
  const char *method;

  // Get the time between ptz updates
  this->ptzUpdatePeriod = 0.100;
  this->ptzUpdateTime = -this->ptzUpdatePeriod;

  // Get the time between camera updates
  this->cameraUpdatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6);
  this->cameraUpdateTime = -this->cameraUpdatePeriod;

  // Lens field of view
  this->hfov = node->GetAngle("hfov", DTOR(60));

  // Zoom limits
  this->zoomMin = node->GetTupleAngle("zoomLimits", 0, 1);
  this->zoomMax = node->GetTupleAngle("zoomLimits", 1, 10);
  
  // Gain term on PTZ controller
  this->motionGain = node->GetDouble( "motionGain", 2.0 );
  this->zoomGain = node->GetDouble( "zoomGain", 2.0 );

  // Initial camera settings
  this->cmdZoom = this->zoomMin;
  this->zoom = this->cmdZoom;

  // Get image dimensions
  imgWidth = node->GetTupleInt("imageSize", 0, 320);
  imgHeight = node->GetTupleInt("imageSize", 1, 240);

  // Get camera parameters
  nearClip = node->GetLength("nearClip", 0.50, 0);
  farClip = node->GetLength("farClip", 50.0, 0);
  method = node->GetString("renderMethod", "auto");
  zDepth = node->GetInt("zBufferDepth", 16);
  if (((zDepth % 8) != 0) || (zDepth < 8) || (zDepth  > 32))
    zDepth = 16;
  
  // Initialize the sensor
  if (this->camera->Init(imgWidth, imgHeight, this->hfov * this->cmdZoom,
                         nearClip, farClip, method, zDepth) != 0)
    return -1;

  // Path for saving frames
  this->camera->SetSavePath(node->GetString("savePath", "."));
  this->camera->EnableSaveFrame(node->GetString("saveFrames", false));

  // Create the ODE objects
  if (this->OdeLoad( file, node ) != 0)
    return -1;
  
  // Create a frustrum for user feedback
  if (node->GetBool("useFrustrum", true))
  {
    this->frustrum = new FrustrumGeom(this->head, this->modelSpaceId,
                                      (double) imgWidth / imgHeight, nearClip, farClip);
  }


  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int SonyVID30::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  dReal mass;
  dReal baseRadius, baseHeight;
  dReal uprightLength, uprightWidth, uprightHeight;
  dReal headLength, headWidth, headHeight;
  dReal lensRadius, lensHeight;
  dReal zOffset;