// // ReadLog // // Reads back into the sensor data structures the raw readings that were stored to file by WriteLog (above) // Reads a single line from the file, and interprets it by its delineator (either Laser or Odometry). If the line // is not delineated for some reason, the function prints out an error message, and advances to the next line. // While there is still information in the file, it will return 0. When it reaches the end of the file, it will return 1. // int ReadLog(carmen_FILE *logFile, carmen_logfile_index_p logfile_index, TSense &sense) { int i, max; char line[4096]; int laser; carmen_robot_laser_message laser_msg; if (carmen_logfile_eof(logfile_index)) { fprintf(stderr, "End of Log File.\n"); return 1; } do { laser = carmen_logfile_read_next_line(logfile_index, logFile, 4095, line); } while (strncmp(line, "ROBOTLASER1 ", 12) != 0); memset(&laser_msg, 0, sizeof(laser_msg)); carmen_string_to_robot_laser_message(line, &laser_msg); max = laser_msg.num_readings; if (max > SENSE_NUMBER) max = SENSE_NUMBER; // Now read in the whole list of laser readings. for (i = 0; i < max; i++) { sense[i].theta = (i*M_PI/max) - M_PI/2; sense[i].distance = laser_msg.range[i] * MAP_SCALE; if (sense[i].distance > MAX_SENSE_RANGE) sense[i].distance = MAX_SENSE_RANGE; } // Read x and y coordinates. odometry.x = laser_msg.laser_pose.x; odometry.y = laser_msg.laser_pose.y; // Read the facing angle of the robot. odometry.theta = laser_msg.laser_pose.theta; odometry.theta = carmen_normalize_theta(odometry.theta); return 0; }
int main( int argc, char *argv[] ) { carmen_point_t pos, corrpos, odo; double time; int i; int scancnt; carmen_move_t pos_move = {0.0, 0.0, 0.0}; carmen_move_t corr_move = {0.0, 0.0, 0.0}; carmen_point_t old_pos = {0.0, 0.0, 0.0}; carmen_point_t old_corrpos = {0.0, 0.0, 0.0}; int which_laser = 0; carmen_FILE stdout_carmen; stdout_carmen.compressed = 0; stdout_carmen.fp = stdout; if (argc < 2) { print_usage(); exit(1); } scancnt=0; for (i=1; i < argc-1; i++) { if (!strcmp(argv[i], "-f") || !strcmp(argv[i], "--flaser")) which_laser = 1; if (!strcmp(argv[i], "-r") || !strcmp(argv[i], "--robotlaser")) which_laser = 2; } carmen_ipc_initialize(argc, argv); vascocore_init(argc, argv); /* file handler */ carmen_FILE *logfile = NULL; /* log file index structure */ carmen_logfile_index_p logfile_index = NULL; /* open logfile for reading */ logfile = carmen_fopen(argv[argc-1], "r"); if(logfile == NULL) carmen_die("Error: could not open file %s for reading.\n", argv[1]); /* create index structure */ logfile_index = carmen_logfile_index_messages(logfile); /* buffer for reading the lines */ const int max_line_length=9999; char line[max_line_length+1]; /* used to read the messages. erase stucture before! */ carmen_robot_laser_message l; carmen_erase_structure(&l, sizeof(carmen_robot_laser_message) ); /* iterate over all entries in the logfile */ while (!carmen_logfile_eof(logfile_index)) { int correct_msg = 1; /* read the line from the file */ carmen_logfile_read_next_line(logfile_index, logfile, max_line_length, line); /* what kind of message it this? */ if (which_laser != 1 && strncmp(line, "ROBOTLASER1 ", 12) == 0) { /* convert the string using the corresponding read function */ char* next = carmen_string_to_robot_laser_message(line, &l); time = atof( strtok( next, " ") ); } else if (which_laser != 2 && strncmp(line, "FLASER ", 7) == 0) { /* convert the string using the corresponding read function */ char* next = carmen_string_to_robot_laser_message_orig(line, &l); time = atof( strtok( next, " ") ); } else { correct_msg = 0; fputs(line, stdout); } if (correct_msg) { odo = l.robot_pose; pos = l.laser_pose; /* using the new scan match function that takes robot laser messages... carmen_laser_laser_message scan; carmen_erase_structure(&scan, sizeof(carmen_laser_laser_message) ); scan.timestamp = l.timestamp; scan.config = l.config; scan.num_readings = l.num_readings; scan.num_remissions = l.num_remissions; scan.range = l.range; scan.remission = l.remission; scan.host = l.host; corrpos = vascocore_scan_match( scan, pos );*/ corrpos = vascocore_scan_match_robot( l ); scancnt++; corr_move = carmen_move_between_points( old_corrpos, corrpos ); pos_move = carmen_move_between_points( old_pos, pos ); fprintf( stderr, "expected movement %.6f m %.6f m %.6f degree\n", corr_move.forward, corr_move.sideward, rad2deg(corr_move.rotation) ); fprintf( stderr, "corrected movement %.6f m %.6f m %.6f degree\n\n", pos_move.forward, pos_move.sideward, rad2deg(pos_move.rotation) ); l.laser_pose = corrpos; carmen_logwrite_write_robot_laser(&l,1,&stdout_carmen, time); fflush(stdout); old_pos = pos; old_corrpos = corrpos; } } /* close the file */ carmen_fclose(logfile); fprintf(stderr, "\ndone, corrected %d messages!\n", scancnt); /* free index structure */ carmen_logfile_free_index(&logfile_index); return(0); }