char* RobotLaserMessage::fromString(char* s) {
if (m_msg == NULL) {
m_msg = new carmen_robot_laser_message;
carmen_erase_structure(m_msg, sizeof(carmen_robot_laser_message));
}
if (!strncmp(s,"FLASER ", 7))
return carmen_string_to_robot_laser_message_orig(s, m_msg);
else if (!strncmp(s,"RLASER ", 7))
return carmen_string_to_robot_laser_message_orig(s, m_msg);
else
return carmen_string_to_robot_laser_message(s, m_msg);
}
//
// 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);
}
