rawdata.c

一个外国人网站上的资料

/*
  ratbot-slam (c) 2006 Kris Beevers

  This file is part of ratbot-slam.

  ratbot-slam 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.

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

// use the odometry model to estimate a trajectory and compute the raw
// ir data points; outputs to traj.dat and points.dat

#include "ratbot-slam.h"
#include <stdio.h>
#include <math.h>

FILE *traj, *points;

void print_scan_points(int16_t *scan, pose_t p)
{
  fixed_t range, x, y, tx, ty;

  // right back
  range = fp_mul(int2fp(scan[0]), CONVERT_IR_RB);
  if(range >= IR_MIN && range <= IR_MAX) {
    x = -IR_OFF_X_BACK;
    y = -IR_OFF_Y - range;
    transform_points(&x, &y, 1, &p, &tx, &ty);
    fprintf(points, "%g %g\n", fp2float(tx), fp2float(ty));
  }

  // right front
  range = fp_mul(int2fp(scan[1]), CONVERT_IR_RF);
  if(range >= IR_MIN && range <= IR_MAX) {
    x = IR_OFF_X_CORNER;
    y = -IR_OFF_Y - range;
    transform_points(&x, &y, 1, &p, &tx, &ty);
    fprintf(points, "%g %g\n", fp2float(tx), fp2float(ty));
  }

  // front
  range = fp_mul(int2fp(scan[2]), CONVERT_IR_F);
  if(range >= IR_MIN && range <= IR_MAX) {
    x = IR_OFF_X_FRONT + range;
    y = 0;
    transform_points(&x, &y, 1, &p, &tx, &ty);
    fprintf(points, "%g %g\n", fp2float(tx), fp2float(ty));
  }

  // left front
  range = fp_mul(int2fp(scan[3]), CONVERT_IR_LF);
  if(range >= IR_MIN && range <= IR_MAX) {
    x = IR_OFF_X_CORNER;
    y = IR_OFF_Y + range;
    transform_points(&x, &y, 1, &p, &tx, &ty);
    fprintf(points, "%g %g\n", fp2float(tx), fp2float(ty));
  }

  // left back
  range = fp_mul(int2fp(scan[4]), CONVERT_IR_LB);
  if(range >= IR_MIN && range <= IR_MAX) {
    x = -IR_OFF_X_BACK;
    y = IR_OFF_Y + range;
    transform_points(&x, &y, 1, &p, &tx, &ty);
    fprintf(points, "%g %g\n", fp2float(tx), fp2float(ty));
  }
}

int main(int argc, char **argv)
{
  if(argc < 2) {
    printf("Usage: %s logfile\n", argv[0]);
    return 1;
  }

  FILE *logfile = fopen(argv[1], "rb");
  if(!logfile) {
    printf("Unable to open %s\n", argv[1]);
    return 1;
  }

  traj = fopen("traj.dat", "w");
  points = fopen("points.dat", "w");

  pose_t p, pold;
  //  p.x = p.y = p.t = 0;
  p.x=float2fp(0.5); p.y=0; p.t=float2fp(1.57);
  cov3_t cov;
  pold = p;

  double d = 0, a = 0;

  uint32_t count = 0;
  int16_t frame[7];
  while(!feof(logfile)) {
    if(fread(frame, sizeof(int16_t), 7, logfile) != 7)
      break;
    fprintf(stderr, "%d          \r", ++count);

    motion_model(frame[0], frame[1], &p, &cov);
    //    printf("%d %d\n", frame[0], frame[1]);
    fprintf(traj, "%g %g %g\n", fp2float(p.x), fp2float(p.y), fp2float(p.t));
    //        printf("%d %d %d %d %d\n", frame[2], frame[3], frame[4], frame[5], frame[6]);
    print_scan_points(frame+2, p);

    d += sqrt(fp2float(p.x-pold.x)*fp2float(p.x-pold.x) + fp2float(p.y-pold.y)*fp2float(p.y-pold.y));
    a += fp2float(fp_abs(p.t-pold.t));
    pold = p;
  }

  printf("trajectory length: %g\ntotal rotation: %g\n", d, a);

  return 0;
}