logfile.cpp

移动机器人SLAM,参考一位外国人的硕士论文

/*
  slam3 (c) 2006 Kris Beevers

  This file is part of slam3.

  slam3 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.

  slam3 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 slam3; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include "slam.hpp"
#include "logfile.hpp"
#include <string.h>
#include <ctype.h>
#include <zlib.h>

// tokenize line by whitespace separators
int tokenize(char *line, std::vector<char *> &tokens)
{
  tokens.clear();
  int len = strlen(line);
  for(int i = 0; i < len; ++i) {
    if(isspace(line[i]))
      line[i] = 0;
    else if(i == 0 || line[i-1] == 0)
      tokens.push_back(line + i);
  }
  return tokens.size();
}

// does a string end with a specific character sequence?
bool endswith(const char *str, const char *seq)
{
  if(strlen(str) < strlen(seq))
    return false;
  const char *s = str + (strlen(str) - strlen(seq));
  return (strcmp(s, seq) == 0);
}

logfile_t * autoopen_log(const char *filename)
{
  logfile_t *logfile = 0;

  if(endswith(filename, ".dat") || endswith(filename, ".dat.gz"))
    logfile = new log_rpi;
  else if(endswith(filename, ".log") || endswith(filename, ".log.gz"))
    logfile = new log_player;
  else if(endswith(filename, ".datb") || endswith(filename, ".datb.gz"))
    logfile = new log_rpi_bin;
  else if(endswith(filename, ".clog") || endswith(filename, ".clog.gz"))
    logfile = new log_carmen;

  if(!logfile || !logfile->open(filename))
    return 0;
  return logfile;
}

logfile_t::~logfile_t()
{
  if(file) {
    cleanup();
    gzclose(file);
  }
  if(fn) delete [] fn;
}

bool logfile_t::open(const char *filename)
{
  assert(filename);
  if(file) {
    cleanup();
    gzclose(file);
  }
  file = gzopen(filename, "r");
  if(!file)
    return false;
  fn = (char *)strdup(filename);
  line_count = 0;
  if(!initialize())
    return false;

  // read up to the first odometry data to get an initial pose
  datatype_t type;
  do {
    type = read_datum();
    if(type == ERROR || eof())
      return false;
  } while(type != ODOMETRY);
  return true;
}

bool logfile_t::read_scan()
{
  assert(file);
  datatype_t type;
  do {
    if(eof())
      return false;

    type = read_datum(); // read a datum and parse it

    if(type == ERROR)
      return false;

  } while(type != SCAN); // keep reading until we get a scan

  return true;
}

bool logfile_t::eof() const
{
  assert(file);
  return gzeof(file);
}

char * logfile_t::read_line()
{
  assert(file);
  ++line_count;
  return gzgets(file, line, 4096);
}


//////////////////////////////////////////////////////////////////////
// RPI ASCII and binary logs
//////////////////////////////////////////////////////////////////////

bool log_rpi::initialize()
{
  char *line = read_line();
  if(!line)
    return false;
  if(sscanf(line, "# laser-offset %lf %lf %lf",
	    &offset_x, &offset_y, &offset_z) != 3)
    return false;

  // start with the robot at the origin
  cur_pose = pose_t(offset_x, offset_y, 0);

  uint32_t scan_size;
  double angle_min, angle_max, angle_res;
  if(!(line = read_line()))
    return false;
  if(sscanf(line, "# laser-angles %d %lf %lf %lf",
	    &scan_size, &angle_min, &angle_max, &angle_res) != 4)
    return false;

  cur_scan.resize(scan_size); // initialize scan container
  param::scan_min = deg2rad(angle_min);
  param::scan_max = deg2rad(angle_max);
  param::scan_res = deg2rad(angle_res);

  return true;
}


log_rpi::datatype_t log_rpi::read_datum()
{
  char *line = read_line();
  if(!line)
    return ERROR;

  tokens.resize(0);
  tokenize(line, tokens);

  assert(tokens.size() > 1);
  char type = tokens[0][0];

  if(type == 'P') { // position
    assert(tokens.size() >= 4);
    cur_pose.t() = deg2rad(atof(tokens[3]));
    cur_pose.x() = atof(tokens[1]) + offset_x*cos(cur_pose.t())-offset_y*sin(cur_pose.t());
    cur_pose.y() = atof(tokens[2]) + offset_x*sin(cur_pose.t())+offset_y*cos(cur_pose.t());
    return ODOMETRY;
  } else if(type == 'L') { // laser
    assert(tokens.size() >= cur_scan.size() + 1);
    for(uint32_t i = 0; i < cur_scan.size(); ++i)
      cur_scan[i] = atof(tokens[i + 1]);
    return SCAN;
  } else if(type == 'G') { // gps
    assert(tokens.size() >= 3);
    // assume gps is at same pose as laser, so no offset
    cur_gps.x() = atof(tokens[1]);
    cur_gps.y() = atof(tokens[2]);
    return GPS;
  } else if(type == 'T') { // clock
    assert(tokens.size() >= 3);
    // tokens[1] is milliseconds since robot start, which we don't
    // really need
    last_time = cur_time;
    cur_time = atof(tokens[2]); // seconds since unix epoch
    return TIME;
  }

  return ERROR; // never
}


// read in a value and check that it was properly read
#define read_item(I) if(gzread(file, &(I), sizeof(I)) != sizeof(I)) return ERROR;

bool log_rpi_bin::initialize()
{
  assert(file);

  // read in header and verify this is the right type of file
  char discard;
  const char *header = "RPILOG1.0";
  for(uint32_t i = 0; i < strlen(header); ++i) {
    read_item(discard);
    if(discard != header[i])
      return false;
  }

  // read in laser offset
  read_item(offset_x);
  read_item(offset_y);
  read_item(offset_z);

  // start with the robot at the origin
  cur_pose = pose_t(offset_x, offset_y, 0);

  // read in laser angles information
  uint32_t scan_size;
  double angle_min, angle_max, angle_res;
  read_item(scan_size);
  read_item(angle_min);
  read_item(angle_max);
  read_item(angle_res);

  cur_scan.resize(scan_size); // initialize scan container
  param::scan_min = deg2rad(angle_min);
  param::scan_max = deg2rad(angle_max);
  param::scan_res = deg2rad(angle_res);

  return true;
}

log_rpi_bin::datatype_t log_rpi_bin::read_datum()
{
  assert(file);

  // what type of datum is this?
  char type;
  read_item(type);

  ++line_count; // technically, "datum" number...
  if(type == 'P') { // position
    double x, y, t;
    read_item(x); cur_pose.x() = x + offset_x*cos(cur_pose.t())-offset_y*sin(cur_pose.t());
    read_item(y); cur_pose.y() = y + offset_x*sin(cur_pose.t())+offset_y*cos(cur_pose.t());
    read_item(t); cur_pose.t() = deg2rad(t);
    return ODOMETRY;
  } else if(type == 'L') { // laser
    for(uint32_t i = 0; i < cur_scan.size(); ++i)
      read_item(cur_scan[i]);
    return SCAN;
  } else if(type == 'G') { // gps
    double x, y;
    read_item(x); cur_gps.x() = x;
    read_item(y); cur_gps.y() = y;
    return GPS;
  } else if(type == 'T') { // clock
    uint32_t ms;
    read_item(ms); // we don't use this for anything
    last_time = cur_time;
    read_item(cur_time);
    return TIME;
  }

  return ERROR; // never
}


//////////////////////////////////////////////////////////////////////
// Player/Stage logs
//////////////////////////////////////////////////////////////////////

// based partly on Andrew Howard's pmap code
log_player::datatype_t log_player::read_datum()
{
  // read a line
  char *line = read_line();
  if(!line)
    return ERROR;

  // tokenize the line
  tokens.resize(0);
  tokenize(line, tokens);

  // skip blank lines and comments
  if(tokens.size() == 0 || tokens[0][0] == '#')
    return COMMENT;

  assert(tokens.size() >= 4);

  if(strcmp(tokens[3], "sync") == 0)
    return COMMENT; // skip sync packets

  assert(tokens.size() >= 6);
  cur_time = atof(tokens[5]);

  if(strcmp(tokens[3], "position") == 0) {

    assert(tokens.size() >= 9);
    cur_pose = pose_t(atof(tokens[6]), atof(tokens[7]), atof(tokens[8]));

    return ODOMETRY;

  } else if(strcmp(tokens[3], "laser") == 0) {

    assert(tokens.size() >= 10);
    uint32_t scan_size = atoi(tokens[9]);
    assert(tokens.size() >= 10 + scan_size * 2);
    assert(scan_size > 0);

    cur_scan.resize(scan_size); // initialize scan container
    param::scan_min = atof(tokens[6]);
    param::scan_max = atof(tokens[7]);
    param::scan_res = deg2rad(scan_size) / scan_size;

    for(uint32_t i = 0; i < scan_size; ++i)
      cur_scan[i] = atof(tokens[10 + i * 2]);

    return SCAN;
  }

  return COMMENT; // something else that we don't care about
}


//////////////////////////////////////////////////////////////////////
// CARMEN logs
//////////////////////////////////////////////////////////////////////

log_carmen::datatype_t log_carmen::read_datum()
{
  // read a line
  char *line = read_line();
  if(!line)
    return ERROR;

  // tokenize the line
  tokens.resize(0);
  tokenize(line, tokens);

  // skip blank lines and comments
  if(tokens.size() == 0 || tokens[0][0] == '#')
    return COMMENT;

  if(strcmp(tokens[0], "ODOM") == 0) {

    assert(tokens.size() >= 4);
    cur_pose.x() = atof(tokens[1]);
    cur_pose.y() = atof(tokens[2]);
    cur_pose.t() = clamp_angle(atof(tokens[3]));

    return ODOMETRY;

  } else if(strcmp(tokens[0], "FLASER") == 0) {

    assert(tokens.size() >= 2);
    uint32_t scan_size = atoi(tokens[1]);
    assert(scan_size > 0 && tokens.size() >= 2 + scan_size);

    double ang_min, ang_max;
    switch(scan_size) {
    case 181:
    case 361:
      ang_min = deg2rad(-90);
      ang_max = deg2rad(90);
    case 180:
    case 360:
    default:
      ang_min = deg2rad(-89.5);
      ang_max = deg2rad(89.5);
    }

    cur_scan.resize(scan_size); // initialize scan container
    param::scan_min = ang_min;
    param::scan_max = ang_max;
    param::scan_res = deg2rad(scan_size) / scan_size;

    for(uint32_t i = 0; i < scan_size; ++i)
      cur_scan[i] = atof(tokens[2 + i]);

    return SCAN;
  }

  return COMMENT; // something else that we don't care about
}