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
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
