pioneer2dx.cc

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

//////////////////////////////////////////////////////////////////////////////
// Load the sonar
int Pioneer2DX::LoadSonar( WorldFile *file, WorldFileNode *node )
{
  int i;
  GzVector a, b;
  double posZ = -0.04;
  
  this->sonarMaxRange = 5.0;
  
  pos[0] = GzVectorSet( 0.14103, 0.135, posZ );
  pos[1] = GzVectorSet( 0.14103, 0.135, posZ );
  pos[2] = GzVectorSet( 0.14103, 0.135 - 2 * 0.03429, posZ );
  pos[3] = GzVectorSet( 0.14103, 0.135 - 4 * 0.03429, posZ );
  pos[4] = GzVectorSet( 0.14103, -0.135 + 4 * 0.03429, posZ );
  pos[5] = GzVectorSet( 0.14103, -0.135 + 2 * 0.03429, posZ );
  pos[6] = GzVectorSet( 0.14103, -0.135, posZ );
  pos[7] = GzVectorSet( 0.14103, -0.135, posZ );
  pos[8] = GzVectorSet( -0.24103, -0.135, posZ );
  pos[9] = GzVectorSet( -0.24103, -0.135, posZ );
  pos[10] = GzVectorSet( -0.24103, -0.135 + 2 * 0.03429, posZ );
  pos[11] = GzVectorSet( -0.24103, -0.135 + 4 * 0.03429, posZ );
  pos[12] = GzVectorSet( -0.24103, +0.135 - 4 * 0.03429, posZ );
  pos[13] = GzVectorSet( -0.24103, +0.135 - 2 * 0.03429, posZ );
  pos[14] = GzVectorSet( -0.24103, 0.135, posZ );
  pos[15] = GzVectorSet( -0.24103, 0.135, posZ );
  
  dir[0] = 90;
  dir[1] = 50;
  dir[2] = 30;
  dir[3] = 10;
  dir[4] = -10;
  dir[5] = -30;
  dir[6] = -50;
  dir[7] = -90;
  dir[8] = -90;
  dir[9] = -130;
  dir[10] = -150;
  dir[11] = -170;
  dir[12] = 170;
  dir[13] = 150;
  dir[14] = 130;
  dir[15] = 90;

  this->sonarSensor = new RayProximity(this->world, this->chassis, 16);
  for (i = 0; i < 16; i++)
  {
    a = pos[i];
    b = GzVectorSet(this->sonarMaxRange * cos(dir[i] * M_PI / 180),
                    this->sonarMaxRange * sin(dir[i] * M_PI / 180), 0.0);
    b = GzVectorAdd(a, b); 
    this->sonarSensor->SetRay(i, a, b);
  }
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Pioneer2DX::Init( WorldFile *file, WorldFileNode *node )
{
  // Create position interface
  this->position_iface = gz_position_alloc();
  if (gz_position_create(this->position_iface, this->world->gz_server, this->GetId(),
                         "Pioneer2DX", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  // Set initial interface state
  this->position_iface->data->cmd_enable_motors = 1;

  // Create power inteface
  this->power_iface = gz_power_alloc();
  if (gz_power_create(this->power_iface, this->world->gz_server, this->GetId(),
                      "Pioneer2DX", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  // Create sonar inteface
  this->sonar_iface = gz_sonar_alloc();
  if (gz_sonar_create(this->sonar_iface, this->world->gz_server, this->GetId(),
                       "Pioneer2DX",  this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  // Reset odometric pose
  this->odomPose[0] = 0.0;
  this->odomPose[1] = 0.0;
  this->odomPose[2] = 0.0;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Pioneer2DX::Fini()
{
  // Close sonar interface
  gz_sonar_destroy( this->sonar_iface );
  gz_sonar_free( this->sonar_iface );
  this->sonar_iface = NULL;

  // Close power interface
  gz_power_destroy( this->power_iface );
  gz_power_free( this->power_iface );
  this->power_iface = NULL;

  // Close position interface
  gz_position_destroy( this->position_iface );
  gz_position_free( this->position_iface );
  this->position_iface = NULL;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void Pioneer2DX::Update( double step )
{
  // Do nothing if paused
  if (step == 0)
    return;

  // Update the odometry (do this always for better accuracy)
  this->UpdateOdometry( step );

  // Otherwise, update periodically
  if (this->world->GetSimTime() - this->updateTime < this->updatePeriod)
    return;
  this->updateTime = this->world->GetSimTime();

  // Get commands from the external interface
  this->GetPositionCmd();

  if (this->enableMotors)
  {
    this->wheelJoints[0]->SetParam( dParamVel, 
                                    this->wheelSpeed[1] / this->wheelDiam * 2 );
    this->wheelJoints[1]->SetParam( dParamVel, 
                                    this->wheelSpeed[0] / this->wheelDiam * 2 );
    

    /*this->wheelJoints[0]->SetTorque(this->wheelSpeed[0]);
    this->wheelJoints[1]->SetTorque(this->wheelSpeed[1]);
    */

    this->wheelJoints[0]->SetParam( dParamFMax, 1.1 );
    this->wheelJoints[1]->SetParam( dParamFMax, 1.1 );
  }
  else
  {
    this->wheelJoints[0]->SetParam( dParamVel, 0.0 ); 
    this->wheelJoints[1]->SetParam( dParamVel, 0.0 );
  }

  // Update the sonar sensor
  this->sonarSensor->Update();
  
  // Update the interfaces
  this->PutPositionData();
  this->PutPowerData();
  this->PutSonarData();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry
void Pioneer2DX::UpdateOdometry( double step )
{
  double wd, ws;
  double d1, d2;
  double dr, da;

  wd = this->wheelDiam;
  ws = this->wheelSep;

  // Distance travelled by front wheels
  d1 = step * wd / 2 * wheelJoints[0]->GetAngleRate();
  d2 = step * wd / 2 * wheelJoints[1]->GetAngleRate();

  dr = (d1 + d2) / 2;
  da = (d2 - d1) / ws;
  
  // Compute odometric pose
  this->odomPose[0] += dr * cos( this->odomPose[2] );
  this->odomPose[1] += dr * sin( this->odomPose[2] );
  this->odomPose[2] += da;

  // Compute odometric instantaneous velocity
  this->odomVel[0] = dr / step;
  this->odomVel[1] = 0.0;
  this->odomVel[2] = da / step;

  // Update the power discharge; this is probably completely bogus
  this->batteryLevel -= this->batteryCurve[0] * step;
  this->batteryLevel -= this->batteryCurve[1] * d1 * d1;
  this->batteryLevel -= this->batteryCurve[1] * d2 * d2;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Pioneer2DX::GetPositionCmd()
{
  double vr, va;

  vr = this->position_iface->data->cmd_vel_pos[0];
  va = this->position_iface->data->cmd_vel_rot[2];

  this->enableMotors = this->position_iface->data->cmd_enable_motors;
  this->wheelSpeed[0] = vr + va * this->wheelSep / 2;
  this->wheelSpeed[1] = vr - va * this->wheelSep / 2;
  // TESTING
  //this->wheelSpeed[0] = vr;
  //this->wheelSpeed[1] = va;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void Pioneer2DX::PutPositionData()
{
  gz_position_lock(this->position_iface, 1);
  
  // Data timestamp
  this->position_iface->data->time = this->world->GetSimTime();

  this->position_iface->data->pos[0] = this->odomPose[0];
  this->position_iface->data->pos[1] = this->odomPose[1];
  this->position_iface->data->rot[2] = NORMALIZE(this->odomPose[2]);

  this->position_iface->data->vel_pos[0] = this->odomVel[0];
  this->position_iface->data->vel_rot[2] = this->odomVel[2];

  // TODO
  this->position_iface->data->stall = 0;

  gz_position_unlock(this->position_iface);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the power interface
void Pioneer2DX::PutPowerData()
{
  gz_power_lock(this->power_iface, 1);
    
  this->power_iface->data->time = this->world->GetSimTime();
  this->power_iface->data->levels[0] = this->batteryLevel;

  gz_power_unlock(this->power_iface);
    
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the sonar interface
void Pioneer2DX::PutSonarData()
{
  int i;
  double r;

  gz_sonar_lock(this->sonar_iface, 1);
    
  // Data timestamp
  this->sonar_iface->data->time = this->world->GetSimTime();

  // Sonar count valid
  this->sonar_iface->data->sonar_count = 16;
  
  // Sonar's data
  for (i = 0; i < 16; i++)
  {
    r = Min(this->sonarSensor->GetRange(i), this->sonarMaxRange);

    this->sonar_iface->data->sonar_pos[i][0] = pos[i].x;
    this->sonar_iface->data->sonar_pos[i][1] = pos[i].y;
    this->sonar_iface->data->sonar_pos[i][2] = pos[i].z;

    this->sonar_iface->data->sonar_rot[i][0] = 0;
    this->sonar_iface->data->sonar_rot[i][1] = 0;
    this->sonar_iface->data->sonar_rot[i][2] = dir[i] * M_PI / 180;

    this->sonar_iface->data->sonar_ranges[i] = r;
  }

  gz_sonar_unlock(this->sonar_iface);
    
  return;
}