writelog.cc

机器人仿真软件

  player_sonar_geom_t* geom;
  player_sonar_data_t* range_data;

  // Check the type
  switch(hdr->type)
  {
    case PLAYER_MSGTYPE_DATA:

      // Check the subtype
      switch(hdr->subtype)
      {
        case PLAYER_SONAR_DATA_GEOM:
          // Format:
          //   pose_count x0 y0 a0 x1 y1 a1 ...
          geom = (player_sonar_geom_t*)data;
          fprintf(this->file, "%u ", geom->poses_count);
          for(i=0;i<geom->poses_count;i++)
            fprintf(this->file, "%+07.3f %+07.3f %+07.4f ",
                    geom->poses[i].px,
                    geom->poses[i].py,
                    geom->poses[i].pa);
          return(0);

        case PLAYER_SONAR_DATA_RANGES:
          // Format:
          //   range_count r0 r1 ...
          range_data = (player_sonar_data_t*)data;
          fprintf(this->file, "%u ", range_data->ranges_count);
          for(i=0;i<range_data->ranges_count;i++)
            fprintf(this->file, "%.3f ", range_data->ranges[i]);

          return(0);
        default:
          return(-1);
      }

    case PLAYER_MSGTYPE_RESP_ACK:
      switch(hdr->subtype)
      {
        case PLAYER_SONAR_REQ_GET_GEOM:
          // Format:
          //   pose_count x0 y0 a0 x1 y1 a1 ...
          geom = (player_sonar_geom_t*)data;
          fprintf(this->file, "%u ", geom->poses_count);
          for(i=0;i<geom->poses_count;i++)
            fprintf(this->file, "%+07.3f %+07.3f %+07.4f ",
                    geom->poses[i].px,
                    geom->poses[i].py,
                    geom->poses[i].pa);

          return(0);
        default:
          return(-1);
      }
    default:
      return(-1);
  }
}

/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_wifi WiFi format

@brief wifi log format

The format for each @ref interface_wifi message is:
  - links_count (int): number of nodes to follow
  - list of nodes; for each node:
    - mac (string): MAC address
    - ip (string): IP address
    - essid (string): ESSID
    - mode (int): operating mode (master, adhoc, etc.)
    - freq (int): in MHz
    - encrypt (int): encrypted?
    - qual (int): link quality
    - level (int): link level
    - noise (int): noise level
*/
int
WriteLog::WriteWiFi(player_msghdr_t* hdr, void *data)
{
  unsigned int i;
  char mac[32], ip[32], essid[32];
  player_wifi_data_t* wdata;

  // Check the type
  switch(hdr->type)
  {
    case PLAYER_MSGTYPE_DATA:
      // Check the subtype
      switch(hdr->subtype)
      {
	case PLAYER_WIFI_DATA_STATE:
	  wdata = (player_wifi_data_t*)data;
          fprintf(this->file, "%04d ", wdata->links_count);

          for (i = 0; i < wdata->links_count; i++)
          {
	    memset(mac,0,sizeof(mac));
	    memset(ip,0,sizeof(ip));
	    memset(essid,0,sizeof(essid));

	    assert(wdata->links[i].mac_count < sizeof(mac));
	    assert(wdata->links[i].ip_count < sizeof(ip));
	    assert(wdata->links[i].essid_count < sizeof(essid));

	    memcpy(mac, wdata->links[i].mac, wdata->links[i].mac_count);
	    memcpy(ip, wdata->links[i].ip, wdata->links[i].ip_count);
            memcpy(essid, wdata->links[i].essid, wdata->links[i].essid_count);
        
            fprintf(this->file, "'%s' '%s' '%s' %d %d %d %d %d %d ",
                    mac, ip, essid,
                    wdata->links[i].mode, 
                    wdata->links[i].freq,
                    wdata->links[i].encrypt,
                    wdata->links[i].qual,
                    wdata->links[i].level,
                    wdata->links[i].noise);
          }
          return(0);

        default:
          return(-1);
      }

   default:
     return(-1);
  }
}

/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_wsn WSN format

@brief WSN log format

The format for each @ref interface_wsn message is:
  - node_type      (int): The type of WSN node
  - node_id        (int): The ID of the WSN node
  - node_parent_id (int): The ID of the WSN node's parent (if existing)
  - data_packet         : The WSN node's data packet
    - light       (float): light measurement from a light sensor
    - mic         (float): accoustic measurement from a microphone
    - accel_x     (float): acceleration on X-axis from an acceleration sensor
    - accel_y     (float): acceleration on Y-axis from an acceleration sensor
    - accel_z     (float): acceleration on Z-axis from an acceleration sensor
    - magn_x      (float): magnetic measurement on X-axis from a magnetometer
    - magn_y      (float): magnetic measurement on Y-axis from a magnetometer
    - magn_z      (float): magnetic measurement on Z-axis from a magnetometer
    - temperature (float): temperature measurement from a temperature sensor
    - battery     (float): remaining battery voltage
 */
int
WriteLog::WriteWSN(player_msghdr_t* hdr, void *data)
{
    //unsigned int i;
    player_wsn_data_t* wdata;

  // Check the type
    switch(hdr->type)
    {
        case PLAYER_MSGTYPE_DATA:
      // Check the subtype
            switch(hdr->subtype)
            {
                case PLAYER_WSN_DATA:
                    wdata = (player_wsn_data_t*)data;
                    fprintf(this->file,"%d %d %d %f %f %f %f %f %f %f %f %f %f",
                            wdata->node_type, 
                            wdata->node_id, 
                            wdata->node_parent_id,
                            wdata->data_packet.light,
                            wdata->data_packet.mic,
                            wdata->data_packet.accel_x,
                            wdata->data_packet.accel_y,
                            wdata->data_packet.accel_z,
                            wdata->data_packet.magn_x,
                            wdata->data_packet.magn_y,
                            wdata->data_packet.magn_z,
                            wdata->data_packet.temperature,
                            wdata->data_packet.battery);
                    return(0);

                default:
                    return(-1);
            }

        default:
            return(-1);
    }
}

#if 0
/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_blobfinder Blobfinder format

@brief blobfinder log format

The format for each @ref interface_blobfinder message is:
  - width (int): in pixels, of image
  - height (int): in pixels, of image
  - count (int): number of blobs to follow
  - list of blobs; for each blob:
    - id (int): id of blob (if supported)
    - color (int): packed 24-bit RGB color of blob
    - area (int): in pixels, of blob
    - x y (ints): in pixels, of blob center
    - left right top bottom (ints): in pixels, of bounding box
    - range (int): in mm, of range to blob (if supported)

*/
void WriteLog::WriteBlobfinder(player_blobfinder_data_t *data)
{

  fprintf(this->file, " %d %d %d",
          HUINT16(data->width),
          HUINT16(data->height),
          HUINT16(data->blob_count));

  for(int i=0; i < HUINT16(data->blob_count); i++)
  {
    fprintf(this->file, " %d %d %d %d %d %d %d %d %d %f",
            HINT16(data->blobs[i].id),
            HUINT32(data->blobs[i].color),
            HUINT32(data->blobs[i].area),
            HUINT16(data->blobs[i].x),
            HUINT16(data->blobs[i].y),
            HUINT16(data->blobs[i].left),
            HUINT16(data->blobs[i].right),
            HUINT16(data->blobs[i].top),
            HUINT16(data->blobs[i].bottom),
            MM_M(HUINT16(data->blobs[i].range)));
  }

  return;
}


/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_camera Camera format

@brief camera data format

The format for each @ref interface_camera message is:
  - width (int): in pixels
  - height (int): in pixels
  - depth (int): in bits per pixel
  - format (int): image format
  - compression (int): image compression
  - image data, encoded as a string of ASCII hex values
*/
void WriteLog::WriteCamera(player_camera_data_t *data)
{
  char *str;
  size_t src_size, dst_size;

  // Image format
  fprintf(this->file, "%d %d %d %d %d %d " ,
          HUINT16(data->width), HUINT16(data->height),
          data->bpp, data->format, data->compression,
          HUINT32(data->image_size));

  // Check image size
  src_size = HUINT32(data->image_size);
  dst_size = ::EncodeHexSize(src_size);
  str = (char*) malloc(dst_size + 1);

  // Encode image into string
  ::EncodeHex(str, dst_size, data->image, src_size);

  // Write image bytes
  fprintf(this->file, str);
  free(str);

  return;
}


////////////////////////////////////////////////////////////////////////////
// Write camera data to image file as well
void WriteLog::WriteCameraImage(WriteLogDevice *device, player_camera_data_t *data, struct timeval *time)
{
  FILE *file;
  char filename[1024];

  if (data->compression != PLAYER_CAMERA_COMPRESS_RAW)
  {
    PLAYER_WARN("unsupported compression method");
    return;
  }

  snprintf(filename, sizeof(filename), "%s_camera_%02d_%06d.pnm",
           this->default_basename, device->id.index, device->cameraFrame++);

  file = fopen(filename, "w+");
  if (file == NULL)
    return;

  if (data->format == PLAYER_CAMERA_FORMAT_RGB888)
  {
    // Write ppm header
    fprintf(file, "P6\n%d %d\n%d\n", HUINT16(data->width), HUINT16(data->height), 255);
    fwrite(data->image, 1, HUINT32(data->image_size), file);
  }
  else if (data->format == PLAYER_CAMERA_FORMAT_MONO8)
  {
    // Write pgm header
    fprintf(file, "P5\n%d %d\n%d\n", HUINT16(data->width), HUINT16(data->height), 255);
    fwrite(data->image, 1, HUINT32(data->image_size), file);
  }
  else
  {
    PLAYER_WARN("unsupported image format");
  }

  fclose(file);

  return;
}


/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_fiducial Fiducial format

@brief fiducial log format

The format for each @ref interface_fiducial message is:
  - count (int): number of fiducials to follow
  - list of fiducials; for each fiducial:
    - id (int): fiducial ID
    - x (float): relative X position, in meters
    - y (float): relative Y position, in meters
    - z (float): relative Z position, in meters
    - roll (float): relative roll orientation, in radians
    - pitch (float): relative pitch orientation, in radians
    - yaw (float): relative yaw orientation, in radians
    - ux (float): uncertainty in relative X position, in meters
    - uy (float): uncertainty in relative Y position, in meters
    - uz (float): uncertainty in relative Z position, in meters
    - uroll (float): uncertainty in relative roll orientation, in radians
    - upitch (float): uncertainty in relative pitch orientation, in radians
    - uyaw (float): uncertainty in relative yaw orientation, in radians

*/
void WriteLog::WriteFiducial(player_fiducial_data_t *data)
{
  // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw> <ux> <uy> <uz> etc] ...
  fprintf(this->file, "%d", HUINT16(data->count));
  for(int i=0;i<HUINT16(data->count);i++)
  {
    fprintf(this->file, " %d"
            " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f"
            " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f",
            HINT16(data->fiducials[i].id),
            MM_M(HINT32(data->fiducials[i].pos[0])),
            MM_M(HINT32(data->fiducials[i].pos[1])),
            MM_M(HINT32(data->fiducials[i].pos[2])),
            MM_M(HINT32(data->fiducials[i].rot[0])),
            MM_M(HINT32(data->fiducials[i].rot[1])),
            MM_M(HINT32(data->fiducials[i].rot[2])),
            MM_M(HINT32(data->fiducials[i].upos[0])),
            MM_M(HINT32(data->fiducials[i].upos[1])),
            MM_M(HINT32(data->fiducials[i].upos[2])),
            MM_M(HINT32(data->fiducials[i].urot[0])),
            MM_M(HINT32(data->fiducials[i].urot[1])),
            MM_M(HINT32(data->fiducials[i].urot[2])));
  }

  return;
}


/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_gps GPS format

@brief gps log format

The format for each @ref interface_gps message is:
  - time (float): current GPS time, in seconds
  - latitude (float): in degrees
  - longitude (float): in degrees
  - altitude (float): in meters
  - utm_e (float): UTM WGS84 easting, in meters
  - utm_n (float): UTM WGS84 northing, in meters
  - hdop (float): horizontal dilution of position
  - vdop (float): vertical dilution of position
  - err_horz (float): horizontal error, in meters
  - err_vert (float): vertical error, in meters
  - quality (int): quality of fix (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
  - num_sats (int): number of satellites in view
*/
void WriteLog::WriteGps(player_gps_data_t *data)
{
  fprintf(this->file,
          "%.3f "
          "%.6f %.6f %.6f "
          "%.3f %.3f "
          "%.3f %.3f %.3f %.3f "
          "%d %d",
          (double) (uint32_t) HINT32(data->time_sec) +
          (double) (uint32_t) HINT32(data->time_sec) * 1e-6,
          (double) HINT32(data->latitude) / (1e7),
          (double) HINT32(data->longitude) / (1e7),
          MM_M(HINT32(data->altitude)),
          CM_M(HINT32(data->utm_e)),
          CM_M(HINT32(data->utm_n)),
          (double) HINT16(data->hdop) / 10,
          (double) HINT16(data->vdop) / 10,
          MM_M(HINT32(data->err_horz)),
          MM_M(HINT32(data->err_vert)),
          (int) data->quality,
          (int) data->num_sats);

  return;
}


/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_joystick Joystick format

@brief joystick log format

The format for each @ref interface_joystick message is:
  - xpos (int): unscaled X position of joystick
  - ypos (int): unscaled Y position of joystick
  - xscale (int): maximum X position
  - yscale (int): maximum Y position
  - buttons (hex string): bitmask of button states
*/
void WriteLog::WriteJoystick(player_joystick_data_t *data)
{
  fprintf(this->file, "%+d %+d %d %d %X",
          HINT16(data->xpos),
          HINT16(data->ypos),
          HINT16(data->xscale),
          HINT16(data->yscale),
          HUINT16(data->buttons));

  return;
}



/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_position3d Position3d format

@brief position3d format

The format for each @ref interface_position3d message is:
  - xpos (float): in meters
  - ypos (float): in meters
  - zpos (float): in meters
  - roll (float): in radians
  - pitch (float): in radians
  - yaw (float): in radians
  - xspeed (float): in meters / second
  - yspeed (float): in meters / second
  - zspeed (float): in meters / second
  - rollspeed(float): in radians / second
  - pitchspeed(float): in radians / second
  - yawspeed(float): in radians / second
  - stall (int): motor stall sensor
*/
void WriteLog::WritePosition3d(player_position3d_data_t *data)
{
  fprintf(this->file,
          "%+.4f %+.4f %+.4f "
          "%+.4f %+.4f %+.4f "
          "%+.4f %+.4f %+.4f "
          "%+.4f %+.4f %+.4f "
          "%d",
          MM_M(HINT32(data->xpos)),
          MM_M(HINT32(data->ypos)),
          MM_M(HINT32(data->zpos)),
          HINT32(data->roll) / 1000.0,
          HINT32(data->pitch) / 1000.0,
          HINT32(data->yaw) / 1000.0,
          MM_M(HINT32(data->xspeed)),
          MM_M(HINT32(data->yspeed)),
          MM_M(HINT32(data->zspeed)),
          HINT32(data->rollspeed) / 1000.0,
          HINT32(data->pitchspeed) / 1000.0,
          HINT32(data->yawspeed) / 1000.0,
          data->stall);

  return;
}


/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_power Power format

@brief power log format

The format for each @ref interface_power message is:
  - charge (float): in volts
*/
void WriteLog::WritePower(player_power_data_t *data)
{
  fprintf(this->file, "%.1f ", HUINT16(data->charge) / 10.0);
  return;
}


#endif