readlog.cc

机器人仿真软件

                    (player_laser_geom_t*)calloc(1,sizeof(player_laser_geom_t));
            assert(geom);

            geom->pose.px = atof(tokens[7]);
            geom->pose.py = atof(tokens[8]);
            geom->pose.pa = atof(tokens[9]);
            geom->size.sl = atof(tokens[10]);
            geom->size.sw = atof(tokens[11]);

            // Find the right place to put it
            int j;
            for(j=0;j<this->provide_count;j++)
            {
              if(Device::MatchDeviceAddress(this->provide_ids[j], id))
                break;
            }
            assert(j<this->provide_count);

            if(this->provide_metadata[j])
              free(this->provide_metadata[j]);

            this->provide_metadata[j] = (void*)geom;

            // nothing to publish
            return(0);
          }

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

    default:
      PLAYER_ERROR1("unknown laser msg type %d\n", type);
      return(-1);
  }
}

////////////////////////////////////////////////////////////////////////////
// Parse sonar data
int ReadLog::ParseSonar(player_devaddr_t id,
                        unsigned short type, unsigned short subtype,
                        int linenum,
                        int token_count, char **tokens, double time)
{
  switch(type)
  {
    case PLAYER_MSGTYPE_DATA:
      switch(subtype)
      {
        case PLAYER_SONAR_DATA_RANGES:
          {
            player_sonar_data_t data;
            if(token_count < 8)
            {
              PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
              return -1;
            }
            data.ranges_count = atoi(tokens[7]);
            int count = 0;
            for(int i=8;i<token_count;i++)
            {
              data.ranges[count++] = atof(tokens[i]);
            }
            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_SONAR_DATA_GEOM:
          {
            player_sonar_geom_t geom;
            if(token_count < 8)
            {
              PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
              return -1;
            }
            geom.poses_count = atoi(tokens[7]);
            int count = 0;
            for(int i=8;i<token_count;i+=3)
            {
              geom.poses[count].px = atof(tokens[i]);
              geom.poses[count].py = atof(tokens[i+1]);
              geom.poses[count].pa = atof(tokens[i+2]);
              count++;
            }
            if(count != (int)geom.poses_count)
            {
              PLAYER_ERROR2("range count mismatch at %s:%d",
                            this->filename, linenum);
              return -1;
            }
            this->Publish(id, NULL, type, subtype,
                          (void*)&geom, sizeof(geom), &time);
            return(0);
          }
        default:
          PLAYER_ERROR1("unknown sonar data subtype %d\n", subtype);
          return(-1);
      }
    case PLAYER_MSGTYPE_RESP_ACK:
      switch(subtype)
      {
        case PLAYER_SONAR_REQ_GET_GEOM:
          {
            if(token_count < 8)
            {
              PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
              return -1;
            }

            // cache it
            player_sonar_geom_t* geom =
                    (player_sonar_geom_t*)calloc(1,sizeof(player_sonar_geom_t));
            assert(geom);

            geom->poses_count = atoi(tokens[7]);
            int count = 0;
            for(int i=8;i<token_count;i+=3)
            {
              geom->poses[count].px = atof(tokens[i]);
              geom->poses[count].py = atof(tokens[i+1]);
              geom->poses[count].pa = atof(tokens[i+2]);
              count++;
            }
            if(count != (int)geom->poses_count)
            {
              PLAYER_ERROR2("range count mismatch at %s:%d",
                            this->filename, linenum);
              free(geom);
              return -1;
            }

            // Find the right place to put it
            int j;
            for(j=0;j<this->provide_count;j++)
            {
              if(Device::MatchDeviceAddress(this->provide_ids[j], id))
                break;
            }
            assert(j<this->provide_count);

            if(this->provide_metadata[j])
              free(this->provide_metadata[j]);

            this->provide_metadata[j] = (void*)geom;

            // nothing to publish
            return(0);
          }
        default:
          PLAYER_ERROR1("unknown sonar reply subtype %d\n", subtype);
          return(-1);
      }
    default:
      PLAYER_ERROR1("unknown sonar message type %d\n", type);
      return(-1);
  }
}


////////////////////////////////////////////////////////////////////////////
// Parse position data
int
ReadLog::ParsePosition(player_devaddr_t id,
                       unsigned short type, unsigned short subtype,
                       int linenum,
                       int token_count, char **tokens, double time)
{
  switch(type)
  {
    case PLAYER_MSGTYPE_DATA:
      switch(subtype)
      {
        case PLAYER_POSITION2D_DATA_STATE:
          {
            player_position2d_data_t data;
            if(token_count < 14)
            {
              PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
              return -1;
            }
            data.pos.px = atof(tokens[7]);
            data.pos.py = atof(tokens[8]);
            data.pos.pa = atof(tokens[9]);
            data.vel.px = atof(tokens[10]);
            data.vel.py = atof(tokens[11]);
            data.vel.pa = atof(tokens[12]);
            data.stall = atoi(tokens[13]);

            this->Publish(id, NULL, type, subtype,
                          (void*)&data, sizeof(data), &time);
            return(0);
          }
        default:
          PLAYER_ERROR1("unknown position data subtype %d\n", subtype);
          return(-1);
      }
    case PLAYER_MSGTYPE_RESP_ACK:
      switch(subtype)
      {
        case PLAYER_POSITION2D_REQ_GET_GEOM:
          {
            if(token_count < 12)
            {
              PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
              return -1;
            }

            // cache it
            player_position2d_geom_t* geom =
                    (player_position2d_geom_t*)calloc(1,sizeof(player_position2d_geom_t));
            assert(geom);

            geom->pose.px = atof(tokens[7]);
            geom->pose.py = atof(tokens[8]);
            geom->pose.pa = atof(tokens[9]);
            geom->size.sl = atof(tokens[10]);
            geom->size.sw = atof(tokens[11]);

            // Find the right place to put it
            int j;
            for(j=0;j<this->provide_count;j++)
            {
              if(Device::MatchDeviceAddress(this->provide_ids[j], id))
                break;
            }
            assert(j<this->provide_count);

            if(this->provide_metadata[j])
              free(this->provide_metadata[j]);

            this->provide_metadata[j] = (void*)geom;

            // nothing to publish
            return(0);
          }
        default:
          PLAYER_ERROR1("unknown position reply subtype %d\n", subtype);
          return(-1);
      }
    default:
      PLAYER_ERROR1("unknown position message type %d\n", type);
      return(-1);
  }
}

////////////////////////////////////////////////////////////////////////////
// Parse wifi data
int ReadLog::ParseWifi(player_devaddr_t id, 
                       unsigned short type, unsigned short subtype,
                       int linenum,
                       int token_count, char **tokens, double time)
{
  player_wifi_data_t data;
  player_wifi_link_t *link;
  int i;
  unsigned int reported_count;

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

            reported_count = atoi(tokens[7]);
            data.links_count = 0;
            for(i = 8; (i+8) < token_count; i += 9)
            {
              link = data.links + data.links_count;

              memcpy(link->mac, tokens[i + 0]+1, strlen(tokens[i+0])-2);
              link->mac_count = strlen(tokens[i+0])-2;
              memcpy(link->ip, tokens[i + 1]+1, strlen(tokens[i+1])-2);
              link->ip_count = strlen(tokens[i+1])-2;
              memcpy(link->essid, tokens[i + 2]+1, strlen(tokens[i+2])-2);
              link->essid_count = strlen(tokens[i+2])-2;
              link->mode = atoi(tokens[i + 3]);
              link->freq = atoi(tokens[i + 4]);
              link->encrypt = atoi(tokens[i + 5]);
              link->qual = atoi(tokens[i + 6]);
              link->level = atoi(tokens[i + 7]);
              link->noise = atoi(tokens[i + 8]);

              data.links_count++;
            }
            if(data.links_count != reported_count)
              PLAYER_WARN("read fewer wifi link entries than expected");

            this->Publish(id, NULL, type, subtype,
                          (void*)&data, sizeof(data), &time);
            return(0);
          }
        default:
          PLAYER_ERROR1("unknown wifi data subtype %d\n", subtype);
          return(-1);
      }
    default:
      PLAYER_ERROR1("unknown wifi message type %d\n", type);
      return(-1);
  }
}

////////////////////////////////////////////////////////////////////////////
// Parse WSN data
int ReadLog::ParseWSN(player_devaddr_t id, 
                      unsigned short type, unsigned short subtype,
                      int linenum,
                      int token_count, char **tokens, double time)
{
    player_wsn_data_t data;

    switch(type)
    {
        case PLAYER_MSGTYPE_DATA:
            switch(subtype)
            {
                case PLAYER_WSN_DATA:
                {
                    if(token_count < 20)
                    {
                        PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
                        return -1;
                    }
                    data.node_type      = atoi(tokens[7]);
                    data.node_id        = atoi(tokens[8]);
                    data.node_parent_id = atoi(tokens[9]);

                    data.data_packet.light       = atof(tokens[10]);
                    data.data_packet.mic         = atof(tokens[11]);
                    data.data_packet.accel_x     = atof(tokens[12]);
                    data.data_packet.accel_y     = atof(tokens[13]);
                    data.data_packet.accel_z     = atof(tokens[14]);
                    data.data_packet.magn_x      = atof(tokens[15]);
                    data.data_packet.magn_y      = atof(tokens[16]);
                    data.data_packet.magn_z      = atof(tokens[17]);
                    data.data_packet.temperature = atof(tokens[18]);
                    data.data_packet.battery     = atof(tokens[19]);

                    this->Publish(id, NULL, type, subtype,
                                  (void*)&data, sizeof(data), &time);
                    return(0);
                }
                default:
                    PLAYER_ERROR1("unknown WSN data subtype %d\n", subtype);
                    return(-1);
            }
        default:
            PLAYER_ERROR1("unknown WSN message type %d\n", type);
            return(-1);
    }
}

////////////////////////////////////////////////////////////////////////////
// Parse PTZ data
int ReadLog::ParsePTZ (player_devaddr_t id, 
                      unsigned short type, unsigned short subtype,
                      int linenum,
                      int token_count, char **tokens, double time)
{
    switch(type)
    {
        case PLAYER_MSGTYPE_DATA:
            switch(subtype)
            {
                case PLAYER_PTZ_DATA_STATE:
                {
                    if (token_count < 12)
                    {
                        PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
                        return -1;
                    }
		    player_ptz_data_t data;
		    
		    data.pan  = atof (tokens[7]);
		    data.tilt = atof (tokens[8]);
		    data.zoom = atof (tokens[9]);
		    data.panspeed  = atof (tokens[10]);
		    data.tiltspeed = atof (tokens[11]);
		    
                    this->Publish (id, NULL, type, subtype,
                                  (void*)&data, sizeof(data), &time);
                    return (0);
                }
		
                default:
                    PLAYER_ERROR1 ("unknown PTZ data subtype %d\n", subtype);
                    return (-1);
            }
        default:
            PLAYER_ERROR1 ("unknown PTZ message type %d\n", type);
            return (-1);
    }
}

#if 0
////////////////////////////////////////////////////////////////////////////
// Parse position3d data
int ReadLog::ParsePosition3d(player_devaddr_t id, int linenum,
                             int token_count, char **tokens, struct timeval time)
{
 player_position3d_data_t data;

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

  data.xpos = NINT32(M_MM(atof(tokens[6])));
  data.ypos = NINT32(M_MM(atof(tokens[7])));
  data.zpos = NINT32(M_MM(atof(tokens[8])));

  data.roll = NINT32(1000 * atof(tokens[9]));
  data.pitch = NINT32(1000 * atof(tokens[10]));
  data.yaw = NINT32(1000 * atof(tokens[11]));

  data.xspeed = NINT32(M_MM(atof(tokens[12])));
  data.yspeed = NINT32(M_MM(atof(tokens[13])));
  data.zspeed = NINT32(M_MM(atof(tokens[14])));

  data.rollspeed = NINT32(1000 * atof(tokens[15]));
  data.pitchspeed = NINT32(1000 * atof(tokens[16]));
  data.yawspeed = NINT32(1000 * atof(tokens[17]));

  data.stall = atoi(tokens[18]);

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

  return 0;
}

////////////////////////////////////////////////////////////////////////////
// Parse truth data
int ReadLog::ParseTruth(player_devaddr_t id, int linenum,
                             int token_count, char **tokens, struct timeval time)
{
 player_truth_data_t data;

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

  data.pos[0] = NINT32(M_MM(atof(tokens[6])));
  data.pos[1] = NINT32(M_MM(atof(tokens[7])));
  data.pos[2] = NINT32(M_MM(atof(tokens[8])));

  data.rot[0] = NINT32(M_MM(atof(tokens[9])));
  data.rot[1] = NINT32(M_MM(atof(tokens[10])));
  data.rot[2] = NINT32(M_MM(atof(tokens[11])));

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

  return 0;
}


#endif