inertiacube2.cc

机器人仿真平台,和stage配合运行

    PLAYER_ERROR("failed to reset heading");
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Finalize the imu.
int InertiaCube2::ShutdownImu()
{
  ISD_CloseTracker(this->imu);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Get the tracker type.
const char *InertiaCube2::ImuType(int type) 
{
  switch (type) 
  {
    case ISD_NONE:
      return "Unknown";
    case ISD_PRECISION_SERIES:
      return "IS Precision Series";
    case ISD_INTERTRAX_SERIES:
      return "InterTrax Series";
  }
  return "Unknown";
}


////////////////////////////////////////////////////////////////////////////////
// Get the tracker model.
const char *InertiaCube2::ImuModel(int model) 
{
  switch (model) 
  {
    case ISD_IS300:
      return "IS-300 Series";
    case ISD_IS600:
      return "IS-600 Series";
    case ISD_IS900:
      return "IS-900 Series";
    case ISD_INTERTRAX:
      return "InterTrax 30";
    case ISD_INTERTRAX_2:
      return "InterTrax2";
    case ISD_INTERTRAX_LS:
      return "InterTraxLS";
    case ISD_INTERTRAX_LC:
      return "InterTraxLC";
    case ISD_ICUBE2:
      return "InertiaCube2";
    case ISD_ICUBE2_PRO:
      return "InertiaCube2 Pro";
    case ISD_IS1200:
      return "IS-1200 Series";
  }
  return "Unknown";
}

 
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void InertiaCube2::Main() 
{
  struct timespec sleeptime;

  // Sleep for 1ms (will actually take longer than this).
  sleeptime.tv_sec = 0;
  sleeptime.tv_nsec = 1000000L;
  
  while (true)
  {
    // Go to sleep for a while (this is a polling loop).
    nanosleep(&sleeptime, NULL);

    // Test if we are supposed to cancel this thread.
    pthread_testcancel();

    // Process any pending requests.
    HandleRequests();

    // Update the InertiaCube
    UpdateImu();

    // See if there is any new position data.  If there is, generate a
    // new pose estimate.
    if (UpdatePosition())
    {
      // Generate a new pose estimate.
      UpdatePose();

      // TESTING
      printf("%.3f %.3f %.0f  :  ",
             this->position_new_pose[0],
             this->position_new_pose[1],
             this->position_new_pose[2] * 180 / M_PI);
      printf("%.3f %.3f %.0f            \r",
             this->pose[0],
             this->pose[1],
             this->pose[2] * 180 / M_PI);
      
      // Expose the new estimate to the server.
      UpdateData();
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Process requests.  Returns 1 if the configuration has changed.
int InertiaCube2::HandleRequests()
{
  int len;
  void *client;
  char request[PLAYER_MAX_REQREP_SIZE];
  
  while ((len = GetConfig(&client, &request, sizeof(request))) > 0)
  {
    switch (request[0])
    {
      /* TODO
      case PLAYER_POSITION_GET_GEOM_REQ:
        HandleGetGeom(client, request, len);
        break;
      */

      default:
        if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
          PLAYER_ERROR("PutReply() failed");
        break;
    }
  }
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Handle geometry requests.
void InertiaCube2::HandleGetGeom(void *client, void *request, int len)
{
  /* TODO
  player_device_id_t id;
  uint8_t req;
  player_position_geom_t geom;
  struct timeval ts;
  uint16_t reptype;

  if (len != 1)
  {
    PLAYER_ERROR2("geometry request len is invalid (%d != %d)", len, 1);
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  id.code = PLAYER_POSITION_CODE;
  id.index = this->position_index;
  id.port = this->device_id.port;
  
  // Get underlying device geometry.
  req = PLAYER_POSITION_GET_GEOM_REQ;
  if (this->Request(&id, this, &req, 1, &reptype, &ts, &geom, sizeof(geom)) != 0)
  {
    PLAYER_ERROR("unable to get position device geometry");
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }
  if (reptype != PLAYER_MSGTYPE_RESP_ACK)
  {
    if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return;
  }

  if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom)) != 0)
    PLAYER_ERROR("PutReply() failed");
  */
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the InertiaCube.
void InertiaCube2::UpdateImu()
{
  ISD_TRACKER_DATA_TYPE data;
  
  // Update the tracker data.
  if (ISD_GetData(this->imu, &data) == 0)
  {
    PLAYER_ERROR("error getting data");
    return;
  }

  // Pick out the yaw value.
  this->imu_new_orient = -data.Station[0].Orientation[0] * M_PI / 180;

  /*
  printf("orientation %f %f %f\r",
         data.Station[0].Orientation[0],
         data.Station[0].Orientation[1],
         data.Station[0].Orientation[2]);
  */

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the position device (returns non-zero if changed).
int InertiaCube2::UpdatePosition()
{
  int i;
  size_t size;
  player_position_data_t data;
  uint32_t timesec, timeusec;
  double time;
  
  // Get the position device data.
  size = this->position->GetData(this,(unsigned char*) &data, sizeof(data), &timesec, &timeusec);
  time = (double) timesec + ((double) timeusec) * 1e-6;

  // Dont do anything if this is old data.
  if (time - this->position_time < 0.001)
    return 0;
  this->position_time = time;

  // Byte swap
  data.xpos = ntohl(data.xpos);
  data.ypos = ntohl(data.ypos);
  data.yaw = ntohl(data.yaw);

  this->position_new_pose[0] = (double) data.xpos / 1000.0;
  this->position_new_pose[1] = (double) data.ypos / 1000.0;
  this->position_new_pose[2] = (double) data.yaw * M_PI / 180;
  
  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Generate a new pose estimate.
// This algorithm assumes straight line segments.  We can probably to better
// than this.
void InertiaCube2::UpdatePose()
{
  double dx, dy, da;
  double tx, ty;

  // Compute change in pose relative to previous pose.
  dx = this->position_new_pose[0] - this->position_old_pose[0];
  dy = this->position_new_pose[1] - this->position_old_pose[1];
  da = this->position_old_pose[2];
  tx =  dx * cos(da) + dy * sin(da);
  ty = -dx * sin(da) + dy * cos(da);

  // Add this to the previous pose esimate.
  this->pose[0] += tx * cos(this->imu_old_orient) - ty * sin(this->imu_old_orient);
  this->pose[1] += tx * sin(this->imu_old_orient) + ty * cos(this->imu_old_orient);
  this->pose[2] = this->imu_new_orient;

  this->position_old_pose[0] = this->position_new_pose[0];
  this->position_old_pose[1] = this->position_new_pose[1];
  this->position_old_pose[2] = this->position_new_pose[2];
  this->imu_old_orient = this->imu_new_orient;
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void InertiaCube2::UpdateData()
{
  uint32_t timesec, timeusec;
  player_position_data_t data;

  data.xpos = (int32_t) (this->pose[0] * 1000);
  data.ypos = (int32_t) (this->pose[1] * 1000);
  data.yaw = (int32_t) (this->pose[2] * 180 / M_PI);

  // Byte swap
  data.xpos = htonl(data.xpos);
  data.ypos = htonl(data.ypos);
  data.yaw = htonl(data.yaw);

  // Compute time.  Use the position device's time.
  timesec = (uint32_t) this->position_time;
  timeusec = (uint32_t) (fmod(this->position_time, 1.0) * 1e6);

  // Copy data to server.
  PutData((unsigned char*) &data, sizeof(data), timesec, timeusec);

  return;
}