readlog.cc

机器人仿真软件

            int j;
            for(j=0;j<this->provide_count;j++)
            {
                if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr))
                    break;
            }
            if(j>=this->provide_count)
                return(-1);

            if(!this->provide_metadata[j])
                return(-1);

            this->Publish(this->provide_ids[j], resp_queue,
                          PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
                          this->provide_metadata[j],
                          sizeof(player_wsn_datatype_config_t),
                          NULL);
            return(0);
        }
        default:
            return(-1);
    }
}

int
ReadLog::ProcessMessage(MessageQueue * resp_queue,
                        player_msghdr_t * hdr,
                        void * data)
{
  // Handle log config requests
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1,
                           this->log_id))
  {
    return(this->ProcessLogConfig(resp_queue, hdr, data));
  }
  else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
          (hdr->addr.interf == PLAYER_LASER_CODE))
  {
    return(this->ProcessLaserConfig(resp_queue, hdr, data));
  }
  else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
          (hdr->addr.interf == PLAYER_SONAR_CODE))
  {
    return(this->ProcessSonarConfig(resp_queue, hdr, data));
  }
  else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
           (hdr->addr.interf == PLAYER_WSN_CODE))
  {
      return(this->ProcessWSNConfig(resp_queue, hdr, data));
  }
  else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
          (hdr->addr.interf == PLAYER_POSITION2D_CODE))
  {
    return(this->ProcessPositionConfig(resp_queue, hdr, data));
  }
  else
    return -1;
}

////////////////////////////////////////////////////////////////////////////
// Signed int conversion macros
#define NINT16(x) (htons((int16_t)(x)))
#define NUINT16(x) (htons((uint16_t)(x)))
#define NINT32(x) (htonl((int32_t)(x)))
#define NUINT32(x) (htonl((uint32_t)(x)))


////////////////////////////////////////////////////////////////////////////
// Unit conversion macros
#define M_MM(x) ((x) * 1000.0)
#define CM_MM(x) ((x) * 100.0)
#define RAD_DEG(x) ((x) * 180.0 / M_PI)


////////////////////////////////////////////////////////////////////////////
// Parse the header info
int
ReadLog::ParseHeader(int linenum, int token_count, char **tokens,
                     player_devaddr_t *id, double *dtime,
                     unsigned short* type, unsigned short* subtype)
{
  char *name;
  player_interface_t interface;

  if (token_count < 7)
  {
    PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
    return -1;
  }

  name = tokens[3];

  if (lookup_interface(name, &interface) == 0)
  {
    *dtime = atof(tokens[0]);
    id->host = atoi(tokens[1]);
    id->robot = atoi(tokens[2]);
    id->interf = interface.interf;
    id->index = atoi(tokens[4]);
    *type = atoi(tokens[5]);
    *subtype = atoi(tokens[6]);
  }
  else
  {
    PLAYER_WARN1("unknown interface name [%s]", name);
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Parse data
int ReadLog::ParseData(player_devaddr_t id,
                       unsigned short type, unsigned short subtype,
                       int linenum, int token_count, char **tokens,
                       double time)
{
#if 0
  if (id.interf == PLAYER_BLOBFINDER_CODE)
    return this->ParseBlobfinder(id, type, subtype, linenum,
                                 token_count, tokens, time);
  else if (id.interf == PLAYER_CAMERA_CODE)
    return this->ParseCamera(id, type, subtype, linenum,
                             token_count, tokens, time);
  else if (id.interf == PLAYER_FIDUCIAL_CODE)
    return this->ParseFiducial(id, type, subtype, linenum,
                               token_count, tokens, time);
  else if (id.interf == PLAYER_GPS_CODE)
    return this->ParseGps(id, type, subtype, linenum,
                          token_count, tokens, time);
  else if (id.interf == PLAYER_JOYSTICK_CODE)
    return this->ParseJoystick(id, type, subtype, linenum,
                               token_count, tokens, time);
#endif
  if (id.interf == PLAYER_LASER_CODE)
    return this->ParseLaser(id, type, subtype, linenum,
                            token_count, tokens, time);
  else if (id.interf == PLAYER_SONAR_CODE)
    return this->ParseSonar(id, type, subtype, linenum,
                            token_count, tokens, time);
  else if (id.interf == PLAYER_POSITION2D_CODE)
    return this->ParsePosition(id, type, subtype, linenum,
                               token_count, tokens, time);
  else if (id.interf == PLAYER_WIFI_CODE)
    return this->ParseWifi(id, type, subtype, linenum,
                           token_count, tokens, time);
  else if (id.interf == PLAYER_WSN_CODE)
      return this->ParseWSN(id, type, subtype, linenum,
                            token_count, tokens, time);
  else if (id.interf == PLAYER_PTZ_CODE)
      return this->ParsePTZ (id, type, subtype, linenum,
                            token_count, tokens, time);

#if 0
  else if (id.interf == PLAYER_POSITION3D_CODE)
    return this->ParsePosition3d(id, type, subtype, linenum,
                                 token_count, tokens, time);
  else if (id.interf == PLAYER_TRUTH_CODE)
    return this->ParseTruth(id, type, subtype, linenum,
                            token_count, tokens, time);
#endif

  PLAYER_WARN1("unknown interface code [%s]",
               ::lookup_interface_name(0, id.interf));
  return -1;
}

#if 0
////////////////////////////////////////////////////////////////////////////
// Parse blobfinder data
int ReadLog::ParseBlobfinder(player_devaddr_t id, int linenum,
                             int token_count, char **tokens, struct timeval time)
{
  player_blobfinder_data_t data;
  player_blobfinder_blob_t *blob;
  size_t size;
  int i, blob_count;

  if (token_count < 9)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  data.width = NUINT16(atoi(tokens[6]));
  data.height = NUINT16(atoi(tokens[7]));
  blob_count = atoi(tokens[8]);
  data.blob_count = NUINT16(blob_count);

  if (token_count < 9 + blob_count * 10)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  for (i = 0; i < blob_count; i++)
  {
    blob = data.blobs + i;
    blob->id =  NINT16(atoi(tokens[9 + i]));
    blob->color = NUINT32(atoi(tokens[10 + i]));
    blob->area = NUINT32(atoi(tokens[11 + i]));
    blob->x = NUINT16(atoi(tokens[12 + i]));
    blob->y = NUINT16(atoi(tokens[13 + i]));
    blob->left = NUINT16(atoi(tokens[14 + i]));
    blob->right = NUINT16(atoi(tokens[15 + i]));
    blob->top = NUINT16(atoi(tokens[16 + i]));
    blob->bottom = NUINT16(atoi(tokens[17 + i]));
    blob->range = NUINT16(M_MM(atof(tokens[18 + i])));
  }

  size = sizeof(data) - sizeof(data.blobs) + blob_count * sizeof(data.blobs[0]);
  this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, size, &time);

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Parse camera data
int ReadLog::ParseCamera(player_devaddr_t id, int linenum,
                               int token_count, char **tokens, struct timeval time)
{
  player_camera_data_t *data;
  size_t src_size, dst_size;

  if (token_count < 13)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  data = (player_camera_data_t*) malloc(sizeof(player_camera_data_t));
  assert(data);

  data->width = NUINT16(atoi(tokens[6]));
  data->height = NUINT16(atoi(tokens[7]));
  data->bpp = atoi(tokens[8]);
  data->format = atoi(tokens[9]);
  data->compression = atoi(tokens[10]);
  data->image_size = NUINT32(atoi(tokens[11]));

  // Check sizes
  src_size = strlen(tokens[12]);
  dst_size = ::DecodeHexSize(src_size);
  assert(dst_size = NUINT32(data->image_size));
  assert(dst_size < sizeof(data->image));

  // Decode string
  ::DecodeHex(data->image, dst_size, tokens[12], src_size);

  this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, data, sizeof(*data) - sizeof(data->image) + dst_size, &time);

  free(data);

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Parse fiducial data
int ReadLog::ParseFiducial(player_devaddr_t id, int linenum,
                               int token_count, char **tokens, struct timeval time)
{
  player_fiducial_data_t data;
  int fiducial_count;

  if (token_count < 7)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  fiducial_count = atoi( tokens[6] );
  data.count = NUINT16( fiducial_count );

  for( int i = 0; i < fiducial_count; i++ )
  {
    data.fiducials[i].id = NINT16( atof(tokens[13*i + 7]) );
  data.fiducials[i].pos[0] = NINT32(M_MM(atof(tokens[13*i+ 8])));
  data.fiducials[i].pos[1] = NINT32(M_MM(atof(tokens[13*i+ 9])));
  data.fiducials[i].pos[2] = NINT32(M_MM(atof(tokens[13*i+10])));
  data.fiducials[i].rot[0] = NINT32(M_MM(atof(tokens[13*i+11])));
  data.fiducials[i].rot[1] = NINT32(M_MM(atof(tokens[13*i+12])));
  data.fiducials[i].rot[2] = NINT32(M_MM(atof(tokens[13*i+13])));
  data.fiducials[i].upos[0] = NINT32(M_MM(atof(tokens[13*i+14])));
  data.fiducials[i].upos[1] = NINT32(M_MM(atof(tokens[13*i+15])));
  data.fiducials[i].upos[2] = NINT32(M_MM(atof(tokens[13*i+16])));
  data.fiducials[i].urot[0] = NINT32(M_MM(atof(tokens[13*i+17])));
  data.fiducials[i].urot[1] = NINT32(M_MM(atof(tokens[13*i+18])));
  data.fiducials[i].urot[2] = NINT32(M_MM(atof(tokens[13*i+19])));
  }

  this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time);

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Parse GPS data
int ReadLog::ParseGps(player_devaddr_t id, int linenum,
                      int token_count, char **tokens, struct timeval time)
{
  player_gps_data_t data;

  if (token_count < 17)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  data.time_sec = NUINT32((int) atof(tokens[6]));
  data.time_usec = NUINT32((int) fmod(atof(tokens[6]), 1.0));

  data.latitude = NINT32((int) (60 * 60 * 60 * atof(tokens[7])));
  data.longitude = NINT32((int) (60 * 60 * 60 * atof(tokens[8])));
  data.altitude = NINT32(M_MM(atof(tokens[9])));

  data.utm_e = NINT32(CM_MM(atof(tokens[10])));
  data.utm_n = NINT32(CM_MM(atof(tokens[11])));

  data.hdop = NINT16((int) (10 * atof(tokens[12])));
  data.hdop = NINT16((int) (10 * atof(tokens[13])));
  data.err_horz = NUINT32(M_MM(atof(tokens[14])));
  data.err_vert = NUINT32(M_MM(atof(tokens[15])));

  data.quality = atoi(tokens[16]);
  data.num_sats = atoi(tokens[17]);

  this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0,&data, sizeof(data), &time);

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Parse joystick data
int ReadLog::ParseJoystick(player_devaddr_t id, int linenum,
                      int token_count, char **tokens, struct timeval time)
{
  player_joystick_data_t data;

  if (token_count < 11)
  {
    PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
    return -1;
  }

  data.xpos = NINT16((short) atoi(tokens[6]));
  data.ypos = NINT16((short) atoi(tokens[7]));
  data.xscale = NINT16((short) atoi(tokens[8]));
  data.yscale = NINT16((short) atoi(tokens[9]));
  data.buttons = NUINT16((unsigned short) (unsigned int) atoi(tokens[10]));

  this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time);

  return 0;
}
#endif

////////////////////////////////////////////////////////////////////////////
// Parse laser data
int ReadLog::ParseLaser(player_devaddr_t id,
                        unsigned short type, unsigned short subtype,
                        int linenum,
                        int token_count, char **tokens, double time)
{
  int i, count;

  switch(type)
  {
    case PLAYER_MSGTYPE_DATA:
      switch(subtype)
      {
        case PLAYER_LASER_DATA_SCAN:
          {
            player_laser_data_t data;

            if (token_count < 13)
            {
              PLAYER_ERROR2("incomplete line at %s:%d",
                            this->filename, linenum);
              return -1;
            }

            data.id = atoi(tokens[7]);
            data.min_angle = atof(tokens[8]);
            data.max_angle = atof(tokens[9]);
            data.resolution = atof(tokens[10]);
            data.max_range = atof(tokens[11]);
            data.ranges_count = atoi(tokens[12]);
            data.intensity_count = data.ranges_count;

            count = 0;
            for (i = 13; i < token_count; i += 2)
            {
              data.ranges[count] = atof(tokens[i + 0]);
              data.intensity[count] = atoi(tokens[i + 1]);
              count += 1;
            }

            if (count != (int)data.ranges_count)
            {
              PLAYER_ERROR2("range count mismatch at %s:%d",
                            this->filename, linenum);
              return -1;
            }
            this->Publish(id, NULL, type, subtype,
                          (void*)&data, sizeof(data), &time);
            return(0);
          }

        case PLAYER_LASER_DATA_SCANPOSE:
          {
            player_laser_data_scanpose_t data;

            if (token_count < 16)
            {
              PLAYER_ERROR2("incomplete line at %s:%d",
                            this->filename, linenum);
              return -1;
            }

            data.scan.id = atoi(tokens[7]);
            data.pose.px = atof(tokens[8]);
            data.pose.py = atof(tokens[9]);
            data.pose.pa = atof(tokens[10]);
            data.scan.min_angle = atof(tokens[11]);
            data.scan.max_angle = atof(tokens[12]);
            data.scan.resolution = atof(tokens[13]);
            data.scan.max_range = atof(tokens[14]);
            data.scan.ranges_count = atoi(tokens[15]);
            data.scan.intensity_count = data.scan.ranges_count;

            count = 0;
            for (i = 16; i < token_count; i += 2)
            {
              data.scan.ranges[count] = atof(tokens[i + 0]);
              data.scan.intensity[count] = atoi(tokens[i + 1]);
              count += 1;
            }

            if (count != (int)data.scan.ranges_count)
            {
              PLAYER_ERROR2("range count mismatch at %s:%d",
                            this->filename, linenum);
              return -1;
            }

            this->Publish(id, NULL, type, subtype,
                          (void*)&data, sizeof(data), &time);
            return(0);
          }

        default:
          PLAYER_ERROR1("unknown laser data subtype %d\n", subtype);
          return(-1);
      }
      break;

    case PLAYER_MSGTYPE_RESP_ACK:
      switch(subtype)
      {
        case PLAYER_LASER_REQ_GET_GEOM:
          {
            if(token_count < 12)
            {
              PLAYER_ERROR2("incomplete line at %s:%d",
                            this->filename, linenum);
              return -1;
            }

            // cache it
            player_laser_geom_t* geom =