writelog.cc

机器人仿真软件

void
WriteLog::CloseFile()
{
  if(this->file)
  {
    fflush(this->file);
    fclose(this->file);
    this->file = NULL;
  }
}

int
WriteLog::ProcessMessage(MessageQueue * resp_queue,
                         player_msghdr * hdr,
                         void * data)
{
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_LOG_REQ_SET_WRITE_STATE,
                           this->device_addr))
  {
    if(hdr->size != sizeof(player_log_set_write_state_t))
    {
      PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
                    hdr->size, sizeof(player_log_set_write_state_t));
      return(-1);
    }
    player_log_set_write_state_t* sreq = (player_log_set_write_state_t*)data;

    if(sreq->state)
    {
      puts("WriteLog: start logging");
      this->enable = true;
    }
    else
    {
      puts("WriteLog: stop logging");
      this->enable = false;
    }

    // send an empty ACK
    this->Publish(this->device_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LOG_REQ_SET_WRITE_STATE);
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_LOG_REQ_GET_STATE, this->device_addr))
  {
    if(hdr->size != 0)
    {
      PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
                    hdr->size, 0);
      return(-1);
    }

    player_log_get_state_t greq;

    greq.type = PLAYER_LOG_TYPE_WRITE;
    if(this->enable)
      greq.state = 1;
    else
      greq.state = 0;

    this->Publish(this->device_addr,
                  resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_LOG_REQ_GET_STATE,
                  (void*)&greq, sizeof(greq), NULL);
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_LOG_REQ_SET_FILENAME, this->device_addr))
  {
    if(hdr->size < sizeof(uint32_t))
    {
      PLAYER_ERROR2("request is wrong length (%d < %d); ignoring",
                    hdr->size, sizeof(uint32_t));
      return(-1);
    }
    player_log_set_filename_t* freq = (player_log_set_filename_t*)data;

    if(this->enable)
    {
      PLAYER_WARN("tried to switch filenames while logging");
      return(-1);
    }

    PLAYER_MSG1(1,"Closing logfile %s", this->filename);
    this->CloseFile();
    memset(this->filename,0,sizeof(this->filename));
    strncpy(this->filename,
            (const char*)freq->filename,
            freq->filename_count);
    this->filename[sizeof(this->filename)-1] = '\0';
    PLAYER_MSG1(1,"Opening logfile %s", this->filename);
    if(this->OpenFile() < 0)
    {
      PLAYER_WARN1("Failed to open logfile %s", this->filename);
      return(-1);
    }

    this->Publish(this->device_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_LOG_REQ_SET_FILENAME);
    return(0);
  }
  else if(hdr->type == PLAYER_MSGTYPE_DATA)
  {
    // If logging is stopped, then don't log
    if(!this->enable)
      return(0);

    // Walk the device list
    for (int i = 0; i < this->device_count; i++)
    {
      WriteLogDevice * device = this->devices + i;

      if((device->addr.host != hdr->addr.host) ||
         (device->addr.robot != hdr->addr.robot) ||
         (device->addr.interf != hdr->addr.interf) ||
         (device->addr.index != hdr->addr.index))
        continue;


      // Write data to file
      this->Write(device, hdr, data);
      return(0);
    }
    return(-1);
  }
  return(-1);
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void
WriteLog::Main(void)
{
  if(this->OpenFile() < 0)
  {
    PLAYER_ERROR2("unable to open [%s]: %s\n", this->filename, strerror(errno));
    return;
  }

  while (1)
  {
    pthread_testcancel();

    // Wait on my queue
    this->Wait();

    // Process all new messages (calls ProcessMessage on each)
    this->ProcessMessages();
  }
}


////////////////////////////////////////////////////////////////////////////
// Write data to file
void WriteLog::Write(WriteLogDevice *device,
                     player_msghdr_t* hdr,
                     void *data)
{
  //char host[256];
  player_interface_t iface;

  // Get interface name
  assert(device);
  ::lookup_interface_code(device->addr.interf, &iface);
  //gethostname(host, sizeof(host));

  // Write header info
  fprintf(this->file, "%014.3f %u %u %s %02u %03u %03u ",
          hdr->timestamp,
          device->addr.host,
          device->addr.robot,
          iface.name,
          device->addr.index,
          hdr->type,
          hdr->subtype);


  int retval;
  // Write the data
  switch (iface.interf)
  {
    case PLAYER_LASER_CODE:
      retval = this->WriteLaser(hdr, data);
      break;
    case PLAYER_POSITION2D_CODE:
      retval = this->WritePosition(hdr, data);
      break;
    case PLAYER_PTZ_CODE:
      retval = this->WritePTZ(hdr, data);
      break;
    case PLAYER_SONAR_CODE:
      retval = this->WriteSonar(hdr, data);
      break;
    case PLAYER_WIFI_CODE:
      retval = this->WriteWiFi(hdr, data);
      break;
    case PLAYER_WSN_CODE:
      retval = this->WriteWSN(hdr, data);
      break;
#if 0
    case PLAYER_BLOBFINDER_CODE:
      this->WriteBlobfinder((player_blobfinder_data_t*) data);
      break;
    case PLAYER_CAMERA_CODE:
      this->WriteCamera((player_camera_data_t*) data);
      if (this->cameraSaveImages)
        this->WriteCameraImage(device, (player_camera_data_t*) data, &time);
      break;
    case PLAYER_FIDUCIAL_CODE:
      this->WriteFiducial((player_fiducial_data_t*) data);
      break;
    case PLAYER_GPS_CODE:
      this->WriteGps((player_gps_data_t*) data);
      break;
    case PLAYER_JOYSTICK_CODE:
      this->WriteJoystick((player_joystick_data_t*) data);
      break;
    case PLAYER_POSITION3D_CODE:
      this->WritePosition3d((player_position3d_data_t*) data);
      break;
    case PLAYER_POWER_CODE:
      this->WritePower((player_power_data_t*) data);
      break;
    case PLAYER_PLAYER_CODE:
      break;
#endif
    default:
      PLAYER_WARN1("unsupported interface type [%s]",
                   ::lookup_interface_name(0, iface.interf));
      retval = -1;
      break;
  }

  if(retval < 0)
    PLAYER_WARN2("not logging message to interface \"%s\" with subtype %d",
                 ::lookup_interface_name(0, iface.interf), hdr->subtype);

  fprintf(this->file, "\n");

  // Flush the data (some drivers produce a lot of data; we dont want
  // it to back up and slow us down later).
  fflush(this->file);

  return;
}

/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_laser laser format

@brief laser log format

The following type:subtype laser messages can be logged:
- 1:1 (PLAYER_LASER_DATA_SCAN) - A scan.  The format is:
  - scan_id (int): unique, usually increasing index associated with the scan
  - min_angle (float): minimum scan angle, in radians
  - max_angle (float): maximum scan angle, in radians
  - resolution (float): angular resolution, in radians
  - max_range (float): maximum scan range, in meters
  - count (int): number of readings to follow
  - list of readings; for each reading:
    - range (float): in meters
    - intensity (int): intensity

- 1:2 (PLAYER_LASER_DATA_SCANPOSE) - A scan with an attached pose. The format is:
  - scan_id (int): unique, usually increasing index associated with the scan
  - px (float): X coordinate of the pose of the laser's parent object (e.g., the robot to which it is attached), in meters
  - py (float): Y coordinate of the pose of the laser's parent object (e.g., the robot to which it is attached), in meters
  - pa (float): yaw coordinate of the pose of the laser's parent object (e.g., the robot to which it is attached), in radians
  - min_angle (float): minimum scan angle, in radians
  - max_angle (float): maximum scan angle, in radians
  - resolution (float): angular resolution, in radians
  - max_range (float): maximum scan range, in meters
  - count (int): number of readings to follow
  - list of readings; for each reading:
    - range (float): in meters
    - intensity (int): intensity

- 4:1 (PLAYER_LASER_REQ_GET_GEOM) - Laser pose information. The format is:
  - lx (float): X coordinate of the laser's pose wrt its parent (e.g., the robot to which it is attached), in meters.
  - ly (float): Y coordinate of the laser's pose wrt its parent (e.g., the robot to which it is attached), in meters.
  - la (float): yaw coordinate of the laser's pose wrt its parent (e.g., the robot to which it is attached), in radians.
  - sx (float): length of the laser
  - sy (float): width of the laser
*/
int
WriteLog::WriteLaser(player_msghdr_t* hdr, void *data)
{
  size_t i;
  player_laser_data_t* scan;
  player_laser_data_scanpose_t* scanpose;
  player_laser_geom_t* geom;

  // Check the type
  switch(hdr->type)
  {
    case PLAYER_MSGTYPE_DATA:
      // Check the subtype
      switch(hdr->subtype)
      {
        case PLAYER_LASER_DATA_SCAN:
          scan = (player_laser_data_t*)data;
          // Note that, in this format, we need a lot of precision in the
          // resolution field.

          fprintf(this->file, "%04d %+07.4f %+07.4f %+.8f %+07.4f %04d ",
                  scan->id, scan->min_angle, scan->max_angle,
                  scan->resolution, scan->max_range, scan->ranges_count);

          for (i = 0; i < scan->ranges_count; i++)
            fprintf(this->file, "%.3f %2d ",
                    scan->ranges[i], scan->intensity[i]);
          return(0);

        case PLAYER_LASER_DATA_SCANPOSE:
          scanpose = (player_laser_data_scanpose_t*)data;
          // Note that, in this format, we need a lot of precision in the
          // resolution field.

          fprintf(this->file, "%04d %+07.3f %+07.3f %+07.3f %+07.4f %+07.4f %+.8f %+07.4f %04d ",
                  scanpose->scan.id,
                  scanpose->pose.px, scanpose->pose.py, scanpose->pose.pa,
                  scanpose->scan.min_angle, scanpose->scan.max_angle,
                  scanpose->scan.resolution, scanpose->scan.max_range,
                  scanpose->scan.ranges_count);

          for (i = 0; i < scanpose->scan.ranges_count; i++)
            fprintf(this->file, "%.3f %2d ",
                    scanpose->scan.ranges[i], scanpose->scan.intensity[i]);
          return(0);

        default:
          return(-1);
      }
    case PLAYER_MSGTYPE_RESP_ACK:
      switch(hdr->subtype)
      {
        case PLAYER_LASER_REQ_GET_GEOM:
          geom = (player_laser_geom_t*)data;
          fprintf(this->file, "%+7.3f %+7.3f %7.3f %7.3f %7.3f",
                  geom->pose.px,
                  geom->pose.py,
                  geom->pose.pa,
                  geom->size.sl,
                  geom->size.sw);
          return(0);
        default:
          return(-1);
      }
    default:
      return(-1);
  }
}

/** @ingroup tutorial_datalog
 * @defgroup player_driver_writelog_position position2d format

@brief position2d log format

The following type:subtype position2d messages can be logged:
- 1:1 (PLAYER_POSITION2D_DATA_STATE) Odometry information.  The format is:
  - px (float): X coordinate of the device's pose, in meters
  - py (float): Y coordinate of the device's pose, in meters
  - pa (float): yaw coordinate of the device's pose, in radians
  - vx (float): X coordinate of the device's velocity, in meters/sec
  - vy (float): Y coordinate of the device's velocity, in meters/sec
  - va (float): yaw coordinate of the device's velocity, in radians/sec
  - stall (int): Motor stall flag

- 4:0 (PLAYER_POSITION2D_REQ_GET_GEOM) Geometry info.  The format is:
  - px (float): X coordinate of the offset of the device's center of rotation, in meters
  - py (float): Y coordinate of the offset of the device's center of rotation, in meters
  - pa (float): yaw coordinate of the offset of the device's center of rotation, in radians
  - sx (float): The device's length, in meters
  - sy (float): The device's width, in meters
*/
int
WriteLog::WritePosition(player_msghdr_t* hdr, void *data)
{
  // Check the type
  switch(hdr->type)
  {
    case PLAYER_MSGTYPE_DATA:
      // Check the subtype
      switch(hdr->subtype)
      {
        case PLAYER_POSITION2D_DATA_STATE:
          {
            player_position2d_data_t* pdata =
                    (player_position2d_data_t*)data;
            fprintf(this->file,
                    "%+07.3f %+07.3f %+04.3f %+07.3f %+07.3f %+07.3f %d",
                    pdata->pos.px,
                    pdata->pos.py,
                    pdata->pos.pa,
                    pdata->vel.px,
                    pdata->vel.py,
                    pdata->vel.pa,
                    pdata->stall);
            return(0);
          }
        default:
          return(-1);
      }

    case PLAYER_MSGTYPE_RESP_ACK:
      // Check the subtype
      switch(hdr->subtype)
      {
        case PLAYER_POSITION2D_REQ_GET_GEOM:
          {
            player_position2d_geom_t* gdata =
                    (player_position2d_geom_t*)data;
            fprintf(this->file,
                    "%+07.3f %+07.3f %+04.3f %+07.3f %+07.3f",
                    gdata->pose.px,
                    gdata->pose.py,
                    gdata->pose.pa,
                    gdata->size.sl,
                    gdata->size.sw);

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

    default:
      return(-1);
  }
}

/** @ingroup tutorial_datalog
 @defgroup player_driver_writelog_ptz ptz format
 
@brief PTZ log format
The format for each @ref interface_wsn message is:
  - pan       (float): The pan angle/value
  - tilt      (float): The tilt angle/value
  - zoom      (float): The zoom factor
  - panspeed  (float): The current panning speed
  - tiltspeed (float): The current tilting speed
 */
int
WriteLog::WritePTZ (player_msghdr_t* hdr, void *data)
{
  // Check the type
  switch(hdr->type)
  {
    case PLAYER_MSGTYPE_DATA:
      // Check the subtype
      switch(hdr->subtype)
      {
        case PLAYER_PTZ_DATA_STATE:
          {
            player_ptz_data_t* pdata =
                    (player_ptz_data_t*)data;
            fprintf(this->file,
                    "%+07.3f %+07.3f %+04.3f %+07.3f %+07.3f",
                    pdata->pan,
                    pdata->tilt,
                    pdata->zoom,
                    pdata->panspeed,
                    pdata->tiltspeed);
            return(0);
          }
        default:
          return(-1);
      }
    default:
      return(-1);
  }
}

/** @ingroup tutorial_datalog
 @defgroup player_driver_writelog_sonar sonar format

@brief sonar log format

The following type:subtype sonar messages can be logged:
- 1:1 (PLAYER_SONAR_DATA_RANGES) Range data.  The format is:
  - range_count (int): number range values to follow
  - list of readings; for each reading:
    - range (float): in meters

- 1:2 (PLAYER_SONAR_DATA_GEOM) Geometry data. The format is:
  - pose_count (int): number of sonar poses to follow
  - list of tranducer poses; for each pose:
    - x (float): relative X position of transducer, in meters
    - y (float): relative Y position of transducer, in meters
    - a (float): relative yaw orientation of transducer, in radians

- 4:1 (PLAYER_SONAR_REQ_GET_GEOM) Geometry info. The format is:
  - pose_count (int): number of sonar poses to follow
  - list of tranducer poses; for each pose:
    - x (float): relative X position of transducer, in meters
    - y (float): relative Y position of transducer, in meters
    - a (float): relative yaw orientation of transducer, in radians
*/
int
WriteLog::WriteSonar(player_msghdr_t* hdr, void *data)
{
  unsigned int i;