writelog.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 * Desc: Driver for writing log files.
 * Author: Andrew Howard
 * Date: 14 Jun 2003
 * CVS: $Id: writelog.cc,v 1.71.2.1 2006/09/22 23:58:35 gerkey Exp $
 *
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_writelog writelog
 * @brief Logging data

The writelog driver will write data from another device to a log file.
Each data message is written to a separate line.  The format for the 
file is given in the 
@ref tutorial_datalog "data logging tutorial".

The @ref driver_readlog driver can be used to replay the data
(to client programs, the replayed data will appear to come from the
real sensors).

The writelog driver logs data independently of any client connections to
the devices that it is logging.  As long as it's enabled and recording,
the writelog driver records data from the specified list of devices
at the rate that new data is produced by the first device in that list
(so put the fastest one first).  Commands are not logged.

For help in remote-controlling logging, try @ref util_playervcr.
Note that you must declare a @ref interface_log device to allow
logging control.

Note that unless you plan to remote-control this driver via the @ref
interface_log interface (e.g., using @ref util_playervcr),
you should specify the @p alwayson option in the configuration file so
that logging start when Player starts.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_log : can be used to turn logging on/off

@par Requires

The writelog driver takes as input a list of devices to log data from.
The driver with the <b>highest data rate</b> should be placed first in the list.
The writelog driver can will log data from the following interfaces:

- @ref interface_laser
- @ref interface_sonar
- @ref interface_position2d
- @ref interface_ptz
- @ref interface_wifi
- @ref interface_wsn

The following interfaces are supported in principle but are currently
disabled because they need to be updated:

- @ref interface_blobfinder
- @ref interface_camera
- @ref interface_fiducial
- @ref interface_gps
- @ref interface_joystick
- @ref interface_position3d
- @ref interface_power
- @ref interface_truth

@par Configuration requests

- PLAYER_LOG_SET_WRITE_STATE
- PLAYER_LOG_GET_STATE
- PLAYER_LOG_SET_FILENAME

@par Configuration file options

- filename (string)
  - Default: "writelog_YYYY_MM_DD_HH_MM.log", where YYYY is the year,
    MM is the month, etc.
  - Name of logfile.
- autorecord (integer)
  - Default: 0
  - Default log state; set to 1 for continous logging.
- camera_save_images (integer)
  - Default: 0
  - Save camera data to image files as well as to the log file.  The image
    files are named "writelog_YYYY_MM_DD_HH_MM_camera_II_NNNNNNN.pnm",
    where II is the device index and NNNNNNN is the frame number.

@par Example

@verbatim
# Log data from laser:0 position2d:0 to "mydata.log"
driver
(
  name "writelog"
  filename "mydata.log"
  requires ["laser:0" "position2d:0"]
  provides ["log:0"]
  alwayson 1
  autorecord 1
)
@endverbatim

@author Andrew Howard

*/
/** @} */

#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif

#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <sys/types.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <libplayercore/playercore.h>

#include "encode.h"

// Utility class for storing per-device info
struct WriteLogDevice
{
  public: player_devaddr_t addr;
  public: Device *device;
  public: int cameraFrame;
};


// The logfile driver
class WriteLog: public Driver
{
  /// Constructor
  public: WriteLog(ConfigFile* cf, int section);

  /// Destructor
  public: ~WriteLog();

  // MessageHandler
  public: virtual int ProcessMessage(MessageQueue * resp_queue,
                                     player_msghdr * hdr,
                                     void * data);

  /// Initialize the driver
  public: virtual int Setup();

  /// Finalize the driver
  public: virtual int Shutdown();

  // Device thread
  private: virtual void Main(void);

  // Open this->filename, save the resulting FILE* to this->file, and
  // write in the logfile header.
  private: int OpenFile();

  // Request and write geometries
  private: void WriteGeometries();

  // Flush and close this->file
  private: void CloseFile();

  // Write data to file
  private: void Write(WriteLogDevice *device,
                      player_msghdr_t* hdr, void *data);

  // Write laser data to file
  private: int WriteLaser(player_msghdr_t* hdr, void *data);

  // Write position data to file
  private: int WritePosition(player_msghdr_t* hdr, void *data);

  // Write PTZ data to file
  private: int WritePTZ(player_msghdr_t* hdr, void *data);

  // Write sonar data to file
  private: int WriteSonar(player_msghdr_t* hdr, void *data);

  // Write wifi data to file
  private: int WriteWiFi(player_msghdr_t* hdr, void *data);

  // Write WSN data to file
  private: int WriteWSN(player_msghdr_t* hdr, void *data);
#if 0
  // Write blobfinder data to file
  private: void WriteBlobfinder(player_blobfinder_data_t *data);

  // Write camera data to file
  private: void WriteCamera(player_camera_data_t *data);

  // Write camera data to image file as well
  private: void WriteCameraImage(WriteLogDevice *device, player_camera_data_t *data, struct timeval *ts);

  // Write fiducial data to file
  private: void WriteFiducial(player_fiducial_data_t *data);

  // Write GPS data to file
  private: void WriteGps(player_gps_data_t *data);

  // Write joystick data to file
  private: void WriteJoystick(player_joystick_data_t *data);

  // Write position3d data to file
  private: void WritePosition3d(player_position3d_data_t *data);

  // Write power data to file
  private: void WritePower(player_power_data_t *data);

#endif

  // File to write data to
  private: char default_basename[1024];
  private: char default_filename[1024];
  private: char filename[1024];
  private: FILE *file;

  // Subscribed device list
  private: int device_count;
  private: WriteLogDevice devices[1024];

  // Is writing enabled? (client can start/stop)
  private: bool enable;
  private: bool enable_default;

  // Save camera frames to image files as well?
  private: bool cameraSaveImages;
};


////////////////////////////////////////////////////////////////////////////
// Pointer to the one-and-only time
extern PlayerTime* GlobalTime;
extern int global_playerport;


////////////////////////////////////////////////////////////////////////////
// Create a driver for reading log files
Driver* WriteLog_Init(ConfigFile* cf, int section)
{
  return ((Driver*) (new WriteLog(cf, section)));
}


////////////////////////////////////////////////////////////////////////////
// Device factory registration
void WriteLog_Register(DriverTable* table)
{
  table->AddDriver("writelog", WriteLog_Init);
  return;
}


////////////////////////////////////////////////////////////////////////////
// Constructor
WriteLog::WriteLog(ConfigFile* cf, int section)
    : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LOG_CODE)
{
  int i;
  player_devaddr_t addr;
  WriteLogDevice *device;
  time_t t;
  struct tm *ts;

  this->file = NULL;

  // Construct default filename from date and time.  Note that we use
  // the system time, *not* the Player time.  I think that this is the
  // correct semantics for working with simulators.
  time(&t);
  ts = localtime(&t);
  strftime(this->default_basename, sizeof(this->default_filename),
           "writelog_%Y_%m_%d_%H_%M", ts);
  snprintf(this->default_filename, sizeof(this->default_filename), "%s.log",
           this->default_basename);

  // Let user override default filename
  strcpy(this->filename,
         cf->ReadString(section, "filename", this->default_filename));

  // Default enabled?
  if(cf->ReadInt(section, "autorecord", 1) > 0)
    this->enable_default = true;
  else
    this->enable_default = false;

  this->device_count = 0;

  // Get a list of input devices
  for (i = 0; i < cf->GetTupleCount(section, "requires"); i++)
  {
    if (cf->ReadDeviceAddr(&addr, section, "requires", -1, i, NULL) != 0)
    {
      this->SetError(-1);
      return;
    }

    // Add to our device table
    assert(this->device_count < (int) (sizeof(this->devices) / sizeof(this->devices[0])));
    device = this->devices + this->device_count++;
    device->addr = addr;
    device->device = NULL;
    device->cameraFrame = 0;
  }

  // Camera specific settings
  this->cameraSaveImages = cf->ReadInt(section, "camera_save_images", 0);

  return;
}


////////////////////////////////////////////////////////////////////////////
// Destructor
WriteLog::~WriteLog()
{
  return;
}


////////////////////////////////////////////////////////////////////////////
// Initialize driver
int WriteLog::Setup()
{
  int i;
  WriteLogDevice *device;

  // Subscribe to the underlying devices
  for (i = 0; i < this->device_count; i++)
  {
    device = this->devices + i;

    device->device = deviceTable->GetDevice(device->addr);
    if (!device->device)
    {
      PLAYER_ERROR3("unable to locate device [%d:%s:%d] for logging",
                    device->addr.robot,
                    ::lookup_interface_name(0, device->addr.interf),
                    device->addr.index);
      return -1;
    }
    if(device->device->Subscribe(this->InQueue) != 0)
    {
      PLAYER_ERROR("unable to subscribe to device for logging");
      return -1;
    }
  }

  // Enable/disable logging, according to default set in config file
  this->enable = this->enable_default;

  // Start device thread
  this->StartThread();

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Finalize the driver
int WriteLog::Shutdown()
{
  int i;
  WriteLogDevice *device;

  // Stop the device thread
  this->StopThread();

  // Close the file
  this->CloseFile();

  // Unsubscribe to the underlying devices
  for (i = 0; i < this->device_count; i++)
  {
    device = this->devices + i;

    // Unsbscribe from the underlying device
    device->device->Unsubscribe(this->InQueue);
    device->device = NULL;
  }

  return 0;
}

int
WriteLog::OpenFile()
{
  // Open the file
  this->file = fopen(this->filename, "w+");
  if(this->file == NULL)
  {
    PLAYER_ERROR2("unable to open [%s]: %s\n", this->filename, strerror(errno));
    return(-1);
  }

  // Write the file header
  fprintf(this->file, "## Player version %s \n", VERSION);
  fprintf(this->file, "## File version %s \n", "0.3.0");

  fprintf(this->file, "## Format: \n");
  fprintf(this->file, "## - Messages are newline-separated\n");
  fprintf(this->file, "## - Common header to each message is:\n");
  fprintf(this->file, "##   time     host   robot  interface index  type   subtype\n");
  fprintf(this->file, "##   (double) (uint) (uint) (string)  (uint) (uint) (uint)\n");
  fprintf(this->file, "## - Following the common header is the message payload \n");

  this->WriteGeometries();

  return(0);
}

////////////////////////////////////////////////////////////////////////////
// Request and write geometries
void WriteLog::WriteGeometries()
{
  // Subscribe to the underlying devices
  for (int i = 0; i < this->device_count; i++)
  {
    WriteLogDevice* device = this->devices + i;

    if (device->addr.interf == PLAYER_SONAR_CODE)
    {
      // Get the sonar geometry
      Message* msg;
      if(!(msg = device->device->Request(this->InQueue,
                                         PLAYER_MSGTYPE_REQ,
                                         PLAYER_SONAR_REQ_GET_GEOM,
                                         NULL, 0, NULL, true)))
      {
        // oh well.
        PLAYER_WARN("unable to get sonar geometry");
      }
      else
      {
        // log it
        this->Write(device, msg->GetHeader(), msg->GetPayload());
        delete msg;
      }
    }
    else if (device->addr.interf == PLAYER_LASER_CODE)
    {
      // Get the laser geometry
      Message* msg;
      if(!(msg = device->device->Request(this->InQueue,
                                         PLAYER_MSGTYPE_REQ,
                                         PLAYER_LASER_REQ_GET_GEOM,
                                         NULL, 0, NULL, true)))
      {
        // oh well.
        PLAYER_WARN("unable to get laser geometry");
      }
      else
      {
        // log it
        this->Write(device, msg->GetHeader(), msg->GetPayload());
        delete msg;
      }
    }
    else if (device->addr.interf == PLAYER_POSITION2D_CODE)
    {
      // Get the laser geometry
      Message* msg;
      if(!(msg = device->device->Request(this->InQueue,
                                         PLAYER_MSGTYPE_REQ,
                                         PLAYER_POSITION2D_REQ_GET_GEOM,
                                         NULL, 0, NULL, true)))
      {
        // oh well.
        PLAYER_WARN("unable to get position geometry");
      }
      else
      {
        // log it
        this->Write(device, msg->GetHeader(), msg->GetPayload());
        delete msg;
      }
    }
  }
}