void robot_rearlaser_handler(carmen_robot_laser_message *laser) { fprintf(stderr, "R"); carmen_logwrite_write_robot_laser(laser, 2, outfile, carmen_get_time() - logger_starttime); }
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); }
int main(int argc, char **argv) { FILE *fp; carmen_FILE *outfile; int m, d, y, h, min, temp1, temp2, i; float s; // int type_id; double timestamp, first_timestamp = 0; double last_front_laser_timestamp, last_odometry_timestamp; int first = 1; char key; carmen_ipc_initialize(argc, argv); carmen_param_check_version(argv[0]); if(argc < 3) { fprintf(stderr, "Error: wrong number of arguments.\n"); fprintf(stderr, "Usage: %s laserint-logname logname\n", argv[0]); exit(1); } fp = fopen(argv[1], "r"); if(fp == NULL) { fprintf(stderr, "Error: could not open file %s for reading.\n", argv[1]); exit(1); } outfile = carmen_fopen(argv[2], "r"); if (outfile != NULL) { fprintf(stderr, "Overwrite %s? ", argv[2]); scanf("%c", &key); if (toupper(key) != 'Y') exit(-1); carmen_fclose(outfile); } outfile = carmen_fopen(argv[2], "w"); if(outfile == NULL) { fprintf(stderr, "Error: could not open file %s for writing.\n", argv[2]); exit(1); } front_laser.num_readings = 180; front_laser.range = (float *)calloc(180, sizeof(float)); carmen_test_alloc(front_laser.range); front_laser.host = carmen_get_host(); odometry.host = carmen_get_host(); carmen_logwrite_write_header(outfile); carmen_logwrite_write_robot_name("beesoft", outfile); get_all_params(outfile); while(!feof(fp)) { fgets(line, LINE_SIZE, fp); if(strncmp(line, "@SENS", 5) == 0) { fgets(line2, 7, fp); if(strncmp(line2, "#LASER", 6) == 0) { last_front_laser_timestamp = front_laser_timestamp; sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s); timestamp = h * 3600.0 + min * 60.0 + s; fscanf(fp, " %d %d: ", &temp1, &temp2); for(i = 0; i < 180; i++) { fscanf(fp, "%d", &temp1); front_laser.range[i] = temp1/100.0; } front_laser.laser_pose.x = odometry.x; front_laser.laser_pose.y = odometry.y; front_laser.laser_pose.theta = odometry.theta; front_laser.robot_pose.x = odometry.x; front_laser.robot_pose.y = odometry.y; front_laser.robot_pose.theta = odometry.theta; if(first) { first_timestamp = timestamp; first = 0; } front_laser_timestamp = timestamp - first_timestamp; if(current_front_laser > 0 && front_laser_timestamp < last_front_laser_timestamp) front_laser_timestamp = last_front_laser_timestamp; // type_id = ROBOT_FRONTLASER_ID; front_laser.timestamp = front_laser_timestamp; carmen_logwrite_write_robot_laser(&front_laser, 1, outfile, front_laser_timestamp); current_front_laser++; } else if(strncmp(line2, "#ROBOT", 6) == 0) { last_odometry_timestamp = odometry_timestamp; sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s); timestamp = h * 3600.0 + min * 60.0 + s; fscanf(fp, " %lf %lf %lf", &odometry.x, &odometry.y, &odometry.theta); odometry.x /= 100.0; odometry.y /= 100.0; odometry.theta = carmen_normalize_theta(carmen_degrees_to_radians(odometry.theta)); if(first) { first_timestamp = timestamp; first = 0; } odometry_timestamp = timestamp - first_timestamp; if(current_odometry > 0 && odometry_timestamp < last_odometry_timestamp) odometry_timestamp = last_odometry_timestamp; // type_id = ODOM_ID; carmen_logwrite_write_odometry(&odometry, outfile, odometry_timestamp); current_odometry++; } fgets(line2, LINE_SIZE, fp); } } fclose(fp); carmen_fclose(outfile); return 0; }
void RobotLaserMessage::save(carmen_FILE *logfile, double logger_timestamp) { carmen_logwrite_write_robot_laser(m_msg, m_msg->id , logfile, logger_timestamp); }