pioneer2dx.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: Model for a Pioneer2DX
 * Author: Andrew Howard
 * Date: 8 May 2003
 * CVS: $Id: Pioneer2DX.cc,v 1.40 2006/03/03 03:54:19 natepak Exp $
 */

/// @addtogroup models 
/// @{ 
/** @defgroup Pioneer2DX Pioneer 2 DX

@htmlinclude Pioneer2DX_view.html

The Pioneer2DX model simulates an ActivMedia Pioneer2DX mobile robot
base with 16 sonars.


@par libgazebo interfaces

- Position information is available through the @ref position interface.
- Power information is available through the @ref power interface.
- Sonar information is available through the @ref sonar interface.


@par Player drivers

- Position information is available through the %gz_position driver.
- Power information is available through the %gz_power driver.
- Sonar information is available through the %gz_sonar driver.


@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- updateRate (float, Hz)
  - Updates per second
  - Default: 10

- batteryLevel (float, volts)
  - Initial battery level
  - Default: 12.4

- batteryCurve (float)
  - Discharge curve: about one hour quiescent
  - Default: 2/3600  2/1e4


@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:Pioneer2DX>
  <xyz>0 0 0</xyz>
</model:Pioneer2DX>
@endverbatim

@par Views

@htmlinclude Pioneer2DX_more_views.html

@par Authors

Andrew Howard

*/
/// @} 

#include <assert.h>

#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "SphereGeom.hh"
#include "WheelGeom.hh"
#include "HingeJoint.hh"
#include "BallJoint.hh"
#include "RayProximity/RayProximity.hh"

#include "Pioneer2DX.hh"


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


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

  this->chassis = NULL;
  this->castorWheel = NULL;
  this->castorJoint = NULL;
  this->sonarSensor = NULL;

  for (i=0; i<2; i++)
    this->wheels[i] = NULL;

  for (i=0; i<3; i++)
    this->wheelJoints[i] = NULL;

  return;
}


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

  if (this->chassis)
    delete this->chassis;
  if (this->castorWheel)
    delete this->castorWheel;
  if (this->castorJoint)
    delete this->castorJoint;
  if (this->sonarSensor)
    delete this->sonarSensor;

  this->chassis = NULL;
  this->castorWheel = NULL;
  this->castorJoint = NULL;
  this->sonarSensor = NULL;

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

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


  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Pioneer2DX::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.35;
  this->wheelDiam = 0.19;

  this->updatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6);
  this->updateTime = -updatePeriod;

  // Create the ODE objects
  if (this->LoadODE(file, node) != 0)
    return -1;

  // Create the sonar sensor
  if (this->LoadSonar(file, node) != 0)
    return -1;

  // Initial battery level
  this->batteryLevel = node->GetDouble("batteryLevel", 12.4);

  // Discharge curve: about one hour quiescent
  this->batteryCurve[0] = node->GetTupleDouble("batteryCurve", 0, 2 / 3600.0);
  this->batteryCurve[1] = node->GetTupleDouble("batteryCurve", 1, 2 / 1e4);  

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int Pioneer2DX::LoadODE( WorldFile *file, WorldFileNode *node )
{
  int i;
  Geom *geom;
  GzVector a;

  // Note: model origin is the top of the robot, above the center of
  // the axle.
  
  // TODO: get correct dimensions
  // We give the caster a large mass to stop the robot falling
  // on it's face.  It's either that, or simulate the batteries
  // *inside* the robot.
  double mass = 5.0;
  double casterMass = 5.0;

  GzVector bodySize = GzVectorSet(0.380, 0.270, 0.170);
  GzVector bodyPos = GzVectorSet(-0.050, 0, -0.170/2);

  GzVector topSize = GzVectorSet(0.450, 0.380, 0.005);
  GzVector topPos = GzVectorSet(-0.050, 0, 0);

  GzVector wheelSize = GzVectorSet(this->wheelDiam, this->wheelDiam, 0.05);
  GzVector wheelPos = GzVectorSet(0, this->wheelSep/2, -0.145);

  GzVector casterSize = GzVectorSet(0.08, 0.08, 0.08);
  GzVector casterPos = GzVectorSet(-0.220, 0, wheelPos.z - wheelSize.x/2 + casterSize.x/2);
  
  // Create the main chassis of the robot
  this->chassis = new Body( this->world );

  geom = new BoxGeom(this->chassis, this->modelSpaceId, bodySize.x, bodySize.y, bodySize.z);
  geom->SetRelativePosition( GzVectorSet(bodyPos.x, bodyPos.y, bodyPos.z) );
  geom->SetMass( mass );
  geom->SetColor( GzColor(1, 0, 0) );  

  geom = new BoxGeom(this->chassis, this->modelSpaceId, topSize.x, topSize.y, topSize.z);
  geom->SetRelativePosition( GzVectorSet(topPos.x, topPos.y, topPos.z) );
  geom->SetMass( 0 );
  geom->SetColor( GzColor(0, 0, 0) );  

  this->AddBody( this->chassis, true );

  // Create the wheels
  for (i = 0; i < 2; i++)
  {
    this->wheels[i] = new Body( this->world );

    geom = new WheelGeom(this->wheels[i], this->modelSpaceId, 0.5 * wheelSize.x, wheelSize.z);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );
    this->AddBody( this->wheels[i] );
  }

  // Set the wheel positions
  this->wheels[0]->SetPosition(GzVectorSet(wheelPos.x, +wheelPos.y, wheelPos.z));
  this->wheels[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->wheels[1]->SetPosition(GzVectorSet(wheelPos.x, -wheelPos.y, wheelPos.z));
  this->wheels[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Attach the wheels to the chassis at their current positions
  for (i = 0; i < 2; i++)
  {
    this->wheelJoints[i] = new HingeJoint( this->world );
    this->wheelJoints[i]->Attach( this->wheels[i], this->chassis );
    a = wheels[i]->GetPosition();
    this->wheelJoints[i]->SetAnchor( a );
    this->wheelJoints[i]->SetAxis( GzVectorSet(0, 1, 0) );
    this->wheelJoints[i]->SetParam( dParamSuspensionERP, 0.4 );
    this->wheelJoints[i]->SetParam( dParamSuspensionCFM, 0.8 );
  }

  // Create the castor wheel
  this->castorWheel = new Body(this->world);
  geom = new SphereGeom(this->castorWheel, this->modelSpaceId, casterSize.x/2);
  geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
  geom->SetMass(casterMass);
  geom->SetColor(GzColor(0, 0, 0));

  this->castorWheel->SetPosition(casterPos);
  this->castorWheel->SetRotation(GzQuaternFromEuler(M_PI / 2, 0, 0));
  this->AddBody(this->castorWheel);

  // Attach the castor to the chassis
  this->castorJoint = new BallJoint(this->world);
  this->castorJoint->Attach(this->castorWheel, this->chassis);
  a = this->castorWheel->GetPosition();
  this->castorJoint->SetAnchor( a );

  return 0;
}