b21r.cc

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

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load the sonar
int B21R::LoadSonar( WorldFile *file, WorldFileNode *node )
{
  int i;
  GzVector a, b;
  
  this->sonarMaxRange = 5.0;

  // Dimensions
  RFLEX_SONARS_CONFIG config= {RFLEX_SONARS_CONFIG_CONST};

  for (i = 0; i < 24; i++)
    {
      pos[i] = GzVectorSet( config.tab[i].x, config.tab[i].y, 0.10);
      dir[i] = config.tab[i].theta;
    }
  for (i = 24; i < 48; i++)
    {
      pos[i] = GzVectorSet( config.tab[i].x, config.tab[i].y, -0.40);
      dir[i] = config.tab[i].theta;
    }
  

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


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

  // Create power inteface
  this->power_iface = gz_power_alloc();
  if (gz_power_create(this->power_iface, this->world->gz_server, this->GetId(),
                      "B21R", 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(),
                       "B21R",  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 B21R::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 B21R::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)
  {
    this->updateTime = this->world->GetSimTime();

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

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

    //this->wheelJoints[0]->SetParam( dParamFMax, 10.0 );
    this->wheelJoints[1]->SetParam( dParamFMax, 10.0 );
    //this->wheelJoints[2]->SetParam( dParamFMax, 10.0 );
    this->wheelJoints[3]->SetParam( dParamFMax, 10.0 );
  
    // Update the sonar sensor
    this->sonarSensor->Update();

    // Update the interface
    this->PutPositionData();
    this->PutPowerData();
    this->PutSonarData();
  }
  
  return;
}


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

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

  // Average distance travelled by left and right wheels
  d1 = step * wd / 2 * wheelJoints[3]->GetAngleRate();
  d2 = step * wd / 2 * wheelJoints[1]->GetAngleRate();

  dr = (d1 + d2) / 2;
  da = (d1 - d2) / 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 B21R::GetPositionCmd()
{
  double vr, va;

  gz_position_lock(this->position_iface, 1);
  vr = this->position_iface->data->cmd_vel_pos[0];
  va = this->position_iface->data->cmd_vel_rot[2];
  gz_position_unlock(this->position_iface);
  
  this->wheelSpeed[0] = vr - va * this->wheelSep / 2;
  this->wheelSpeed[1] = vr + va * this->wheelSep / 2;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void B21R::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] = this->odomPose[2];

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

  gz_position_unlock(this->position_iface);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the power interface
void B21R::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 B21R::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 = 48;
  
  // Sonar's data
  for (i = 0; i < 48; 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];

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

  gz_sonar_unlock(this->sonar_iface);
    
  return;
}