bool LogFile::load(const char* filename, bool verbose) {
char line[100000];
carmen_FILE *logfile = NULL;
carmen_logfile_index_p logfile_index = NULL;
logfile = carmen_fopen(filename, "r");
if(logfile == NULL) {
if (verbose)
carmen_warn("Error: could not open file %s for reading.\n", filename);
return false;
}
/* index the logfile */
logfile_index = carmen_logfile_index_messages(logfile);
for(int i = 0; i < logfile_index->num_messages; i++) {
/* read i-th line */
carmen_logfile_read_line(logfile_index, logfile, i, 100000, line);
/* create messages */
if(strncmp(line, "ODOM ", 5) == 0) {
push_back(new OdometryMessage(line));
}
else if(strncmp(line, "RAWLASER", 8) == 0) {
push_back(new LaserMessage(line));
}
else if(strncmp(line, "ROBOTLASER", 10) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "FLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "RLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "TRUEPOS ", 8) == 0) {
push_back(new TrueposMessage(line));
}
else if(strncmp(line, "IMU ", 4) == 0) {
push_back(new IMUMessage(line));
}
else if(strncmp(line, "SCANMARK ", 9) == 0) {
push_back(new ScanmarkMessage(line));
}
else if(strncmp(line, "POSITIONLASER ", 14) == 0) {
push_back(new LaserposMessage(line));
}
else if(strlen(line) > 1) {
push_back(new UnknownMessage(line));
}
}
carmen_fclose(logfile);
return true;
}
//
// This calls the procedures in the other files which do all the real work.
// If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make
// certain to set LOW_DURATION in low.h to some incredibly high number.
//
void *Slam(void * /*arg unused */)
{
TPath *path, *trashPath;
TSenseLog *obs, *trashObs;
double oldmC_D = 0;
double oldmC_T = 0;
double oldvC_D = 0;
double oldvC_T = 0;
double oldmD_D = 0;
double oldmD_T = 0;
double oldvD_D = 0;
double oldvD_T = 0;
double oldmT_D = 0;
double oldmT_T = 0;
double oldvT_D = 0;
double oldvT_T = 0;
outFile = fopen("motionModel.txt", "w");
fprintf(outFile, "This file lists the motion model parameters in the following order : \n");
fprintf(outFile, " - mean value from observed translation\n");
fprintf(outFile, " - mean value from observed rotation\n");
fprintf(outFile, " - variance contributed from observed translation\n");
fprintf(outFile, " - variance contributed from observed rotation\n");
fprintf(outFile, "\n");
fprintf(outFile, "------------Intermediate Models------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fclose(outFile);
while ((fabs(oldmC_D-meanC_D) > 0.01)||(fabs(oldmC_T-meanC_T) > 0.03)||(fabs(oldvC_D-varC_D) > 0.01)||(fabs(oldvC_T-varC_T) > 0.1) ||
(fabs(oldmD_D-meanD_D) > 0.01)||(fabs(oldmD_T-meanD_T) > 0.03)||(fabs(oldvD_D-varD_D) > 0.01)||(fabs(oldvD_T-varD_T) > 0.1) ||
(fabs(oldmT_D-meanT_D) > 0.03)||(fabs(oldmT_T-meanT_T) > 0.01)||(fabs(oldvT_D-varT_D) > 0.03)||(fabs(oldvT_T-varT_T) > 0.01)) {
readFile = carmen_fopen(PLAYBACK, "r");
if(readFile == NULL)
carmen_die("Error: could not open file %s for reading.\n", PLAYBACK);
logfile_index = carmen_logfile_index_messages(readFile);
InitLowSlam();
LowSlam(&path, &obs);
carmen_fclose(readFile);
oldmC_D = meanC_D;
oldmC_T = meanC_T;
oldvC_D = varC_D;
oldvC_T = varC_T;
oldmD_D = meanD_D;
oldmD_T = meanD_T;
oldvD_D = varD_D;
oldvD_T = varD_T;
oldmT_D = meanT_D;
oldmT_T = meanT_T;
oldvT_D = varT_D;
oldvT_T = varT_T;
outFile = fopen("motionModel.txt", "a");
Learn(path);
fclose(outFile);
// Get rid of the path and log of observations
while (path != NULL) {
trashPath = path;
path = path->next;
free(trashPath);
}
while (obs != NULL) {
trashObs = obs;
obs = obs->next;
free(trashObs);
}
}
outFile = fopen("motionModel.txt", "a");
fprintf(outFile, "\n");
fprintf(outFile, "------------Final Model------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fprintf(outFile, "#define meanC_D %.4f\n#define meanC_T %.4f\n#define varC_D %.4f\n#define varC_T %.4f\n\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "#define meanD_D %.4f\n#define meanD_T %.4f\n#define varD_D %.4f\n#define varD_T %.4f\n\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "#define meanT_D %.4f\n#define meanT_T %.4f\n#define varT_D %.4f\n#define varT_T %.4f\n\n", meanT_D, meanT_T, varT_D, varT_T);
fclose(outFile);
CloseLowSlam();
return NULL;
}
int main(int argc, char **argv)
{
FILE *index_file;
char index_file_name[2000];
memset(&odometry_ackerman, 0, sizeof(odometry_ackerman));
memset(&velocity_ackerman, 0, sizeof(velocity_ackerman));
memset(&visual_odometry, 0, sizeof(visual_odometry));
memset(&imu, 0, sizeof(imu));
memset(&truepos_ackerman, 0, sizeof(truepos_ackerman));
memset(&laser_ackerman1, 0, sizeof(laser_ackerman1));
memset(&laser_ackerman2, 0, sizeof(laser_ackerman2));
memset(&laser_ackerman3, 0, sizeof(laser_ackerman3));
memset(&laser_ackerman4, 0, sizeof(laser_ackerman4));
memset(&laser_ackerman5, 0, sizeof(laser_ackerman5));
memset(&laser_ldmrs, 0, sizeof(laser_ldmrs));
memset(&laser_ldmrs_objects, 0, sizeof(laser_ldmrs_objects));
memset(&laser_ldmrs_objects_data, 0, sizeof(laser_ldmrs_objects_data));
memset(&rawlaser1, 0, sizeof(rawlaser1));
memset(&rawlaser2, 0, sizeof(rawlaser2));
memset(&rawlaser3, 0, sizeof(rawlaser3));
memset(&rawlaser4, 0, sizeof(rawlaser4));
memset(&rawlaser5, 0, sizeof(rawlaser5));
memset(&gpsgga, 0, sizeof(gpsgga));
memset(&gpshdt, 0, sizeof(gpshdt));
memset(&gpsrmc, 0, sizeof(gpsrmc));
memset(&raw_depth_kinect_0, 0, sizeof(raw_depth_kinect_0));
memset(&raw_depth_kinect_1, 0, sizeof(raw_depth_kinect_1));
memset(&raw_video_kinect_0, 0, sizeof(raw_video_kinect_0));
memset(&raw_video_kinect_1, 0, sizeof(raw_video_kinect_1));
memset(&velodyne_partial_scan, 0, sizeof(velodyne_partial_scan));
memset(&velodyne_variable_scan, 0, sizeof(velodyne_variable_scan));
memset(&velodyne_gps, 0, sizeof(velodyne_gps));
memset(&xsens_euler, 0, sizeof(xsens_euler));
memset(&xsens_quat, 0, sizeof(xsens_quat));
memset(&xsens_matrix, 0, sizeof(xsens_matrix));
memset(&xsens_mtig, 0, sizeof(xsens_mtig));
memset(&bumblebee_basic_stereoimage1, 0, sizeof(bumblebee_basic_stereoimage1));
memset(&bumblebee_basic_stereoimage2, 0, sizeof(bumblebee_basic_stereoimage2));
memset(&bumblebee_basic_stereoimage3, 0, sizeof(bumblebee_basic_stereoimage3));
memset(&bumblebee_basic_stereoimage4, 0, sizeof(bumblebee_basic_stereoimage4));
memset(&bumblebee_basic_stereoimage5, 0, sizeof(bumblebee_basic_stereoimage5));
memset(&bumblebee_basic_stereoimage6, 0, sizeof(bumblebee_basic_stereoimage6));
memset(&bumblebee_basic_stereoimage7, 0, sizeof(bumblebee_basic_stereoimage7));
memset(&bumblebee_basic_stereoimage8, 0, sizeof(bumblebee_basic_stereoimage8));
memset(&bumblebee_basic_stereoimage9, 0, sizeof(bumblebee_basic_stereoimage9));
memset(&web_cam_message, 0, sizeof(web_cam_message));
memset(&ackerman_motion_message, 0, sizeof(ackerman_motion_message));
memset(&ultrasonic_message, 0, sizeof(ultrasonic_message));
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
register_ipc_messages ();
read_parameters (argc, argv);
carmen_playback_define_messages ();
signal(SIGINT, shutdown_playback_module);
logfile = carmen_fopen(argv[1], "r");
if (logfile == NULL)
carmen_die("Error: could not open file %s for reading.\n", argv[1]);
int fadvise_error = posix_fadvise(fileno(logfile->fp), 0, 0, POSIX_FADV_SEQUENTIAL);
if (fadvise_error)
{
printf("Could not advise POSIX_FADV_SEQUENTIAL on playback\n");
exit(1);
}
strcpy(index_file_name, argv[1]);
strcat(index_file_name, ".index");
index_file = fopen(index_file_name, "r");
if (index_file == NULL)
{
logfile_index = carmen_logfile_index_messages(logfile);
save_logindex_file(logfile_index, index_file_name);
}
else if (index_file_older_than_log_file(index_file, logfile))
{
fclose(index_file);
logfile_index = carmen_logfile_index_messages(logfile);
save_logindex_file(logfile_index, index_file_name);
}
else
{
logfile_index = load_logindex_file(index_file_name);
}
main_playback_loop();
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);
}
