From: Toby C. <th...@us...> - 2008-02-15 01:06:17
|
Update of /cvsroot/playerstage/code/player/utils/pmap In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30928/utils/pmap Modified Files: logfile.cpp logfile.h pmap_test.cpp Log Message: aplpied patch [ 1850791 ] generalizing pmap Index: pmap_test.cpp =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/pmap/pmap_test.cpp,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** pmap_test.cpp 5 May 2007 11:53:08 -0000 1.3 --- pmap_test.cpp 15 Feb 2008 09:06:18 -0000 1.4 *************** *** 178,182 **** static int opt_laser_index = 0; static double opt_scale = 0.025; ! static double opt_range_max = 8.0; static double opt_range_res = 0.10; static pose2_t opt_robot_pose; --- 178,182 ---- static int opt_laser_index = 0; static double opt_scale = 0.025; ! static double opt_range_max = -1.0; static double opt_range_res = 0.10; static pose2_t opt_robot_pose; *************** *** 190,193 **** --- 190,201 ---- static int opt_fine_max = 100; + // configuration data, read from logfile + static int laser_range_count; + static double laser_range_max; + static double laser_angle_min; + static double laser_angle_max; + static double laser_angle_step; + + // GUI settings static int win; *************** *** 219,222 **** --- 227,231 ---- static void process(); static void save(); + static void process_init(); static int process_coarse(); static void save_coarse(); *************** *** 429,432 **** --- 438,476 ---- } + // recover configuration data from logfile + void process_init() + { + while (1) + { + int logresult = logfile_read(logfile); + if (logresult < 0) + { + fprintf(stderr, "\n"); + return; + } + else if(logresult > 0) + continue; + + // wait for first laser scan + if (strcmp(logfile->interface, "laser") == 0 && + logfile->index == opt_laser_index) + { + // save data + laser_range_count = logfile->laser_range_count; + laser_range_max = logfile->laser_range_max; + laser_angle_min = logfile->laser_angle_min; + laser_angle_max = logfile->laser_angle_max; + laser_angle_step = logfile->laser_angle_step; + + fprintf(stderr, "range_count = %d\n", laser_range_count); + fprintf(stderr, "range_max = %f\n", laser_range_max); + fprintf(stderr, "angle_min = %f\n", laser_angle_min); + fprintf(stderr, "angle_max = %f\n", laser_angle_max); + fprintf(stderr, "angle_step = %f\n", laser_angle_step); + + return; + } + } + } // Coarse map generation (process log file) *************** *** 470,480 **** logfile->index == opt_laser_index) { - if (logfile->laser_range_count != 181) - { - fprintf(stderr, "incorrect range count; expecting %d, got %d", - lodo->num_ranges, logfile->laser_range_count); - exit(-1); - } - // Update the local pose estimate (corrected odometry) lodo_pose = lodo_add_scan(lodo, odom_pose, --- 514,517 ---- *************** *** 799,805 **** if (!logfile) return -1; // Create lodo handle ! lodo = lodo_alloc(181, opt_range_max, opt_range_res, -M_PI / 2, M_PI / 180); if (!lodo) return -1; --- 836,852 ---- if (!logfile) return -1; + + //aquire laser configuration data + process_init(); + + if (opt_range_max > 0) + laser_range_max = opt_range_max; // override maximum laser range if specified in command line + + // restart logreader + logfile_free(logfile); + logfile = logfile_alloc(opt_filename); // Create lodo handle ! lodo = lodo_alloc(laser_range_count, laser_range_max, opt_range_res, laser_angle_min, laser_angle_step); if (!lodo) return -1; *************** *** 809,814 **** // Create pmap handle ! pmap = pmap_alloc(181, opt_range_max, -M_PI / 2, M_PI / 180, ! opt_num_samples, opt_grid_width, opt_grid_height, opt_grid_scale); if (!pmap) return -1; --- 856,861 ---- // Create pmap handle ! pmap = pmap_alloc(laser_range_count, laser_range_max, laser_angle_min, laser_angle_step, ! opt_num_samples, opt_grid_width, opt_grid_height, opt_grid_scale); if (!pmap) return -1; *************** *** 822,833 **** // Create rmap handle ! rmap = rmap_alloc(181, opt_range_max, -M_PI / 2, M_PI / 180, ! opt_grid_width, opt_grid_height); if (!rmap) return -1; // Create omap handle ! omap = omap_alloc(181, opt_range_max, -M_PI / 2, M_PI / 180, ! opt_grid_width, opt_grid_height, opt_grid_scale); if (!rmap) return -1; --- 869,880 ---- // Create rmap handle ! rmap = rmap_alloc(laser_range_count, laser_range_max, laser_angle_min, laser_angle_step, ! opt_grid_width, opt_grid_height); if (!rmap) return -1; // Create omap handle ! omap = omap_alloc(laser_range_count, laser_range_max, laser_angle_min, laser_angle_step, ! opt_grid_width, opt_grid_height, opt_grid_scale); if (!rmap) return -1; Index: logfile.cpp =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/pmap/logfile.cpp,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** logfile.cpp 24 Jan 2007 18:22:55 -0000 1.3 --- logfile.cpp 15 Feb 2008 09:06:18 -0000 1.4 *************** *** 39,42 **** --- 39,49 ---- } + void logfile_free(logfile_t *self) + { + fclose(self->file); + delete self->line; + delete self; + } + // Read a line from the log file *************** *** 126,129 **** --- 133,140 ---- assert(self->token_count >= 13); self->laser_range_count = atoi(self->tokens[12]); + self->laser_range_max = atof(self->tokens[11]); + self->laser_angle_min = atof(self->tokens[8]); + self->laser_angle_max = atof(self->tokens[9]); + self->laser_angle_step = atof(self->tokens[10]); assert(self->token_count >= 13 + self->laser_range_count * 2); Index: logfile.h =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/pmap/logfile.h,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** logfile.h 25 Apr 2006 19:24:41 -0000 1.1 --- logfile.h 15 Feb 2008 09:06:18 -0000 1.2 *************** *** 42,46 **** /// Laser data int laser_range_count; ! double laser_ranges[181]; } logfile_t; --- 42,50 ---- /// Laser data int laser_range_count; ! double laser_angle_min; ! double laser_angle_max; ! double laser_angle_step; ! double laser_range_max; ! double laser_ranges[2048]; } logfile_t; *************** *** 54,57 **** --- 58,63 ---- int logfile_read(logfile_t *self); + /// @brief Close Logfile and Delete logfile reader + void logfile_free(logfile_t *self); #endif |