/*  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;}