garcia_mixed.cc

机器人仿真软件

  // Start the device thread; spawns a new thread and executes
  // GarciaDriver::Main(), which contains the main loop for the driver.
  StartThread();

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int
GarciaDriver::Shutdown()
{
  puts("Shutting Garcia driver down");

  // Stop and join the driver thread
  StopThread();

  // Here you would shut the device down by, for example, closing a
  // serial port.

  delete mGarcia;

  puts("Garcia driver has been shutdown");

  return(0);
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void
GarciaDriver::Main()
{

  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Go to sleep for a while (this is a polling loop)
    nanosleep(&NSLEEP_TIME, NULL);

    // Process incoming messages
    ProcessMessages();

    // Write outgoing data
    RefreshData();

  }
  return;
}

// Process an incoming message
int
GarciaDriver::ProcessMessage(MessageQueue* resp_queue,
                             player_msghdr* hdr,
                             void* data)
{
  assert(resp_queue);
  assert(hdr);
  assert(data);

  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                           PLAYER_POSITION2D_CMD_POS, mPos2dAddr))
  {
    assert(hdr->size == sizeof(player_position2d_cmd_pos_t));
    ProcessPos2dPosCmd(hdr, *reinterpret_cast<player_position2d_cmd_pos_t *>(data));
    return(0);
  }
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                           PLAYER_POSITION2D_CMD_VEL, mPos2dAddr))
  {
    assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
    ProcessPos2dVelCmd(hdr, *reinterpret_cast<player_position2d_cmd_vel_t *>(data));
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                PLAYER_SPEECH_CMD_SAY, mSpeechAddr))
  {
    assert(hdr->size == sizeof(player_speech_cmd_t));
    ProcessSpeechCommand(hdr, *reinterpret_cast<player_speech_cmd_t *>(data));
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                PLAYER_DIO_CMD_VALUES, mDioAddr))
  {
    assert(hdr->size == sizeof(player_dio_cmd_t));
    ProcessDioCommand(hdr, *reinterpret_cast<player_dio_cmd_t *>(data));
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_GET_GEOM, mPos2dAddr))
  {
    ProcessPos2dGeomReq(hdr);
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_IR_POSE, mIrAddr))
  {
    ProcessIrPoseReq(hdr);
    return(0);
  }
  else
  {
    PLAYER_ERROR1("GarciaDriver received unknown message: %s", hdr->type);
  }

  return -1;
}

void
GarciaDriver::ProcessPos2dPosCmd(player_msghdr_t* hdr,
                                 player_position2d_cmd_pos_t &data)
{
  printf("Position commands currently not implemented\n");

/*
  // this code could be used to implement the position control,
  // but it will need to be modified

  double rho(0),alpha(0),beta(0);
  double delta_x(0),delta_y(0);
  double v, omega;
  // To be locally exponentially stable:
  //  k_rho  > 0
  //  k_beta < 0
  //  k_alpha-k_rho > 0

  delta_x = x-mPosX;
  delta_y = y-mPosY;

  // If we're within 1 cm, stop (for now)
  if ((fabs(delta_x) > mTranslateDeadzone)||
      (fabs(delta_y) > mTranslateDeadzone)||
      (fabs(mPosTheta-theta) > mRotateDeadzone))
  {
    rho   = sqrt(pow(delta_x,2) + pow(delta_y,2));

    if (fabs(atan2(delta_y, delta_x) - mPosTheta) < M_PI/2)
    {

      alpha = atan2(delta_y,delta_x) - mPosTheta;
      alpha = fmod(alpha, M_PI_2); // M_PI/2 == M_PI_2

      beta  = -fmod(alpha,M_PI) - mPosTheta + theta;
      beta  = fmod(beta, M_PI);

      v     = mKRho * rho;
      omega = mKAlpha * alpha + mKBeta*beta;
    }
    else
    {
      alpha = atan2(-delta_y,-delta_x) - mPosTheta;
      alpha = fmod(alpha, M_PI_2); // M_PI/2 == M_PI_2

      beta  = -alpha - mPosTheta + theta;
      beta  = fmod(beta, M_PI);

      v     = -mKRho * rho;
      omega = mKAlpha * alpha + mKBeta*beta;
    }

    SetTargetVelocity(v, omega);
  }
  else
    SetTargetVelocity(0,0);
*/

}

void
GarciaDriver::ProcessPos2dVelCmd(player_msghdr_t* hdr,
                                 player_position2d_cmd_vel_t &data)
{
  double v(data.vel.px);
  double omega(data.vel.pa);

  acpValue vl(static_cast<float>(v - mWheelBase*omega/2.0));
  acpValue vr(static_cast<float>(v + mWheelBase*omega/2.0));

  mGarcia->setNamedValue("damped-speed-left", &vl);
  mGarcia->setNamedValue("damped-speed-right", &vr);

  // do we have to do this each time, or only once?
  acpObject* behavior;
  behavior = mGarcia->createNamedBehavior("null", "vel");
  mGarcia->queueBehavior(behavior);
}

void
GarciaDriver::ProcessSpeechCommand(player_msghdr_t* hdr,
                                   player_speech_cmd_t &data)
{
  // todo, there is currently a problem if we receive messages too quickly
  cout << data.string << endl;

  acpValue phrase(data.string);
  acpObject* behavior;

  behavior = mGarcia->createNamedBehavior("say", data.string);
  behavior->setNamedValue("phrase", &phrase);
  behavior->setNamedValue("speed", &mSpeed);
  behavior->setNamedValue("pitch", &mPitch);
  behavior->setNamedValue("volume", &mVolume);
  mGarcia->queueBehavior(behavior);
}

void
GarciaDriver::ProcessDioCommand(player_msghdr_t* hdr,
                                player_dio_cmd_t &data)
{
  PLAYER_WARN("Garcia driver currently doesn't support DIO commands");
}

void
GarciaDriver::ProcessPos2dGeomReq(player_msghdr_t* hdr)
{
  player_position2d_geom_t geom;

  geom.pose.px = 0.03; // [m]
  geom.pose.py = 0.00; // [m]
  geom.pose.pa = 0;    // [rad]
  geom.size.sl = mLength;  // [m]
  geom.size.sw = mWidth;  // [m]

  Publish(mPos2dAddr, NULL,
          PLAYER_MSGTYPE_RESP_ACK,
          PLAYER_POSITION2D_REQ_GET_GEOM,
          &geom, sizeof(geom), NULL);
}

void
GarciaDriver::ProcessIrPoseReq(player_msghdr_t* hdr)
{
  player_pose_t poses[6] = {{ 0.105, 0.045, M_PI/6}, //   front-left
                            { 0.105,-0.045,-M_PI/6}, //   front-right
                            { 0.080, 0.020, M_PI_2}, //   side-left
                            { 0.080,-0.020,-M_PI_2}, //   side-right
                            {-0.050, 0.070, M_PI},   //   rear-left
                            {-0.050,-0.070,-M_PI}};  //   rear-right

  player_ir_pose_t pose;
  pose.poses_count = 6;
  memcpy(pose.poses, poses, 6*sizeof(player_pose_t));

  Publish(mIrAddr, NULL,
          PLAYER_MSGTYPE_RESP_ACK,
          PLAYER_IR_POSE,
          &pose, sizeof(pose), NULL);

}

void
GarciaDriver::RefreshData()
{
  // how do we update these?
  mPos2dData.pos.px  = 0.0;
  mPos2dData.pos.py  = 0.0;
  mPos2dData.pos.pa  = 0.0;

  mPos2dData.vel.px  = 0.0;
  mPos2dData.vel.py  = 0.0;
  mPos2dData.vel.pa  = 0.0;

  Publish(mPos2dAddr, NULL,
          PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
          reinterpret_cast<void*>(&mPos2dData), sizeof(mPos2dData), NULL);

  // update the IR data
  mIrData.voltages_count = 0;
  mIrData.ranges_count = 6;

  mIrData.ranges[0] = mGarcia->getNamedValue("front-ranger-left")->getFloatVal();
  mIrData.ranges[1] = mGarcia->getNamedValue("front-ranger-right")->getFloatVal();
  mIrData.ranges[2] = mGarcia->getNamedValue("side-ranger-left")->getFloatVal();
  mIrData.ranges[3] = mGarcia->getNamedValue("side-ranger-right")->getFloatVal();
  mIrData.ranges[4] = mGarcia->getNamedValue("rear-ranger-left")->getFloatVal();
  mIrData.ranges[5] = mGarcia->getNamedValue("rear-ranger-right")->getFloatVal();

  Publish(mIrAddr, NULL,
          PLAYER_MSGTYPE_DATA, PLAYER_IR_DATA_RANGES,
          reinterpret_cast<void*>(&mIrData), sizeof(mIrData), NULL);

  // do we currently have a dio device?
  static int dio_test = 0;
  mDioData.count = 16;
  mDioData.digin = ++dio_test;

  Publish(mDioAddr, NULL,
          PLAYER_MSGTYPE_DATA, PLAYER_DIO_DATA_VALUES,
          reinterpret_cast<void*>(&mDioData), sizeof(mDioData), NULL);

  mPowerData.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
  mPowerData.volts = mGarcia->getNamedValue("battery-voltage")->getFloatVal();
  mPowerData.percent = mGarcia->getNamedValue("battery-level")->getFloatVal();

  Publish(mPowerAddr, NULL,
          PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE,
          reinterpret_cast<void*>(&mPowerData), sizeof(mPowerData), NULL);
}