readlog.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 reading log files.
 * Author: Andrew Howard
 * Date: 17 May 2003
 * CVS: $Id: readlog.cc,v 1.40.2.1 2006/09/22 23:58:35 gerkey Exp $
 *
 * The writelog driver will write data from another device to a log file.
 * The readlog driver will replay the data as if it can from the real sensors.
 */
/** @ingroup drivers */
/** @{ */
/** @defgroup driver_readlog readlog
 * @brief Playback of logged data

The readlog driver can be used to replay data stored in a log file.
This is particularly useful for debugging client programs, since users
may run their clients against the same data set over and over again.
Suitable log files can be generated using the @ref driver_writelog driver.
The format for the log file can be found in the
@ref tutorial_datalog "data logging tutorial".

See below for an example configuration file; note that the device
id's specified in the provides field must match those stored in the
log file (i.e., data logged as "position2d:0" must also be read back as
"position2d:0").

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

@par Compile-time dependencies

- none

@par Provides

The readlog driver can provide the following device interfaces.

- @ref interface_laser
- @ref interface_position2d
- @ref interface_ptz
- @ref interface_sonar
- @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

The driver also provides an interface for controlling the playback:

- @ref interface_log

@par Requires

- none

@par Configuration requests

- PLAYER_LOG_SET_READ_STATE_REQ
- PLAYER_LOG_GET_STATE_REQ
- PLAYER_LOG_SET_READ_REWIND_REQ

@par Configuration file options

- filename (filename)
  - Default: NULL
  - The log file to play back.
- speed (float)
  - Default: 1.0
  - Playback speed; 1.0 is real-time
- autoplay (integer)
  - Default: 1
  - Begin playing back log data when first client subscribes
    (as opposed to waiting for the client to tell the @ref
    interface_log device to play).
- autorewind (integer)
  - Default: 0
  - Automatically rewind and play the log file again when the end is
    reached (as opposed to not producing any more data).

@par Example

@verbatim

# Play back odometry and laser data at twice real-time from "mydata.log"
driver
(
  name "readlog"
  filename "mydata.log"
  provides ["position2d:0" "laser:0" "log:0"]
  speed 2.0
)
@endverbatim

@author Andrew Howard

*/
/** @} */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

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

#if HAVE_ZLIB_H
  #include <zlib.h>
#endif

#include <libplayercore/playercore.h>

#include "encode.h"
#include "readlog_time.h"

#if 0
// we use this pointer to reset timestamps in the client objects when the
// log gets rewound
#include "clientmanager.h"
extern ClientManager* clientmanager;
#endif

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

  // Destructor
  public: ~ReadLog();

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

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

  // Main loop
  public: virtual void Main();

  public: virtual int ProcessMessage(MessageQueue * resp_queue,
                                     player_msghdr_t * hdr,
                                     void * data);
  // Process log interface configuration requests
  private: int ProcessLogConfig(MessageQueue * resp_queue,
                                player_msghdr_t * hdr,
                                void * data);

  // Process position interface configuration requests
  private: int ProcessPositionConfig(MessageQueue * resp_queue,
                                     player_msghdr_t * hdr,
                                     void * data);

  // Process laser interface configuration requests
  private: int ProcessLaserConfig(MessageQueue * resp_queue,
                                  player_msghdr_t * hdr,
                                  void * data);

  // Process sonar interface configuration requests
  private: int ProcessSonarConfig(MessageQueue * resp_queue,
                                  player_msghdr_t * hdr,
                                  void * data);

  // Process WSN interface configuration requests
  private: int ProcessWSNConfig(MessageQueue * resp_queue,
                                player_msghdr_t * hdr,
                                void * data);

  // Parse the header info
  private: int ParseHeader(int linenum, int token_count, char **tokens,
                           player_devaddr_t *id, double *dtime,
                           unsigned short* type, unsigned short* subtype);
  // Parse some data
  private: int ParseData(player_devaddr_t id,
                         unsigned short type, unsigned short subtype,
                         int linenum, int token_count, char **tokens,
                         double time);

#if 0
  // Parse blobfinder data
  private: int ParseBlobfinder(player_devaddr_t id,
                               unsigned short type, unsigned short subtype,
                               int linenum,
                               int token_count, char **tokens, double time);

  // Parse camera data
  private: int ParseCamera(player_devaddr_t id,
                           unsigned short type, unsigned short subtype,
                           int linenum,
                          int token_count, char **tokens, double time);

  // Parse fiducial data
  private: int ParseFiducial(player_devaddr_t id,
                             unsigned short type, unsigned short subtype,
                             int linenum,
                          int token_count, char **tokens, double time);

  // Parse gps data
  private: int ParseGps(player_devaddr_t id,
                        unsigned short type, unsigned short subtype,
                        int linenum,
                        int token_count, char **tokens, double time);

  // Parse joystick data
  private: int ParseJoystick(player_devaddr_t id,
                             unsigned short type, unsigned short subtype,
                             int linenum,
                        int token_count, char **tokens, double time);
#endif

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

  // Parse sonar data
  private: int ParseSonar(player_devaddr_t id,
                          unsigned short type, unsigned short subtype,
                          int linenum,
                          int token_count, char **tokens, double time);
  // Parse position data
  private: int ParsePosition(player_devaddr_t id,
                             unsigned short type, unsigned short subtype,
                             int linenum,
                             int token_count, char **tokens, double time);

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

  // Parse PTZ data
  private: int ParsePTZ (player_devaddr_t id,
                         unsigned short type, unsigned short subtype,
                         int linenum,
                         int token_count, char **tokens, double time);
#if 0

  // Parse position3d data
  private: int ParsePosition3d(player_devaddr_t id,
                               unsigned short type, unsigned short subtype,
                               int linenum,
                               int token_count, char **tokens, double time);

  // Parse truth data
  private: int ParseTruth(player_devaddr_t id,
                          unsigned short type, unsigned short subtype,
                          int linenum,
                          int token_count, char **tokens, double time);

#endif

  // List of provided devices
  private: int provide_count;
  private: player_devaddr_t provide_ids[1024];
  // spots to cache metadata for a device (e.g., sonar geometry)
  private: void* provide_metadata[1024];

  // The log interface (at most one of these)
  private: player_devaddr_t log_id;

  // File to read data from
  private: const char *filename;
  private: FILE *file;
#if HAVE_ZLIB_H
  private: gzFile gzfile;
#endif

  // Input buffer
  private: size_t line_size;
  private: char *line;

  // File format
  private: char *format;

  // Playback speed (1 = real time, 2 = twice real time)
  private: double speed;

  // Playback enabled?
  public: bool enable;

  // Has a client requested that we rewind?
  public: bool rewind_requested;

  // Should we auto-rewind?  This is set in the log devie in the .cfg
  // file, and defaults to false
  public: bool autorewind;
};


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


////////////////////////////////////////////////////////////////////////////
// Device factory registration
void ReadLog_Register(DriverTable* table)
{
  table->AddDriver("readlog", ReadReadLog_Init);
  return;
}


////////////////////////////////////////////////////////////////////////////
// Constructor
ReadLog::ReadLog(ConfigFile* cf, int section)
    : Driver(cf, section)
{
  int i,j;
  player_devaddr_t id;

  this->filename = cf->ReadFilename(section, "filename", NULL);
  if(!this->filename)
  {
    PLAYER_ERROR("must specify a log file to read from");
    this->SetError(-1);
    return;
  }
  this->speed = cf->ReadFloat(section, "speed", 1.0);

  this->provide_count = 0;
  memset(&this->log_id, 0, sizeof(this->log_id));
  memset(this->provide_metadata,0,sizeof(this->provide_metadata));

  // Get a list of devices to provide
  for (i = 0; i < 1024; i++)
  {
    // TODO: fix the indexing here
    if (cf->ReadDeviceAddr(&id, section, "provides", -1, i, NULL) != 0)
      break;
    if (id.interf == PLAYER_LOG_CODE)
      this->log_id = id;
    else
      this->provide_ids[this->provide_count++] = id;
  }

  // Register the log device
  if (this->log_id.interf == PLAYER_LOG_CODE)
  {
    if (this->AddInterface(this->log_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }

  // Register all the provides devices
  for (i = 0; i < this->provide_count; i++)
  {
    if (this->AddInterface(this->provide_ids[i]) != 0)
    {
      for(j=0;j<this->provide_count;j++)
      {
        // free any allocated metadata slots
        if(this->provide_metadata[j])
        {
          free(provide_metadata[j]);
          provide_metadata[j] = NULL;
        }
      }
      this->SetError(-1);
      return;
    }

    // if it's sonar, then make a spot to cache geometry info
    if(this->provide_ids[i].interf == PLAYER_SONAR_CODE)
      assert((this->provide_metadata[i] =
              calloc(sizeof(player_sonar_geom_t),1)));
  }

  // Get replay options
  this->enable = cf->ReadInt(section, "autoplay", 1);
  this->autorewind = cf->ReadInt(section, "autorewind", 0);

  // Initialize other stuff
  this->format = strdup("unknown");
  this->file = NULL;
#if HAVE_ZLIB_H
  this->gzfile = NULL;
#endif

  // Set up the global time object.  We're just shoving our own in over the
  // pre-existing WallclockTime object.  Not pretty but it works.
  if(GlobalTime)
    delete GlobalTime;
  GlobalTime = new ReadLogTime();

  return;
}


////////////////////////////////////////////////////////////////////////////
// Destructor
ReadLog::~ReadLog()
{
  // Free allocated metadata slots
  for(int i=0;i<this->provide_count;i++)
  {
    if(this->provide_metadata[i])
    {
      free(this->provide_metadata[i]);
      this->provide_metadata[i] = NULL;
    }
  }

  return;
}


////////////////////////////////////////////////////////////////////////////
// Initialize driver
int ReadLog::Setup()
{
  // Reset the time
  ReadLogTime_time.tv_sec = 0;
  ReadLogTime_time.tv_usec = 0;
  ReadLogTime_timeDouble = 0.0;

  // Open the file (possibly compressed)
  if (strlen(this->filename) >= 3 && \
      strcasecmp(this->filename + strlen(this->filename) - 3, ".gz") == 0)
  {
#if HAVE_ZLIB_H
    this->gzfile = gzopen(this->filename, "r");
#else
    PLAYER_ERROR("no support for reading compressed log files");
    return -1;
#endif
  }
  else
    this->file = fopen(this->filename, "r");