Update of /cvsroot/playerstage/code/player/utils/playerv In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv9713/utils/playerv Modified Files: Tag: b_thjc_dynamic_arrays dev_bumper.c dev_ir.c dev_laser.c dev_ranger.c dev_sonar.c playerv.h Log Message: static array sizes removed from player client library updated to use dynamic arrays Index: dev_bumper.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_bumper.c,v retrieving revision 1.6 retrieving revision 1.6.2.1 diff -C2 -d -r1.6 -r1.6.2.1 *** dev_bumper.c 9 Jul 2007 17:18:02 -0000 1.6 --- dev_bumper.c 10 Sep 2007 04:11:01 -0000 1.6.2.1 *************** *** 55,58 **** --- 55,59 ---- bumper->drivername = strdup(drivername); bumper->datatime = 0; + bumper->mainwnd = mainwnd; snprintf(section, sizeof(section), "bumper:%d", index); *************** *** 67,77 **** rtk_menuitem_check(bumper->subscribe_item, subscribe); ! // Construct figures ! for (i = 0; i < PLAYERC_BUMPER_MAX_SAMPLES; i++) ! bumper->scan_fig[i] = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 1); ! return bumper; } // Destroy a bumper device --- 68,87 ---- rtk_menuitem_check(bumper->subscribe_item, subscribe); ! bumper->fig_count = 0; return bumper; } + void bumper_allocate_figures(bumper_t * bumper, int fig_count) + { + int i; + if (bumper->fig_count <= fig_count) + return; + bumper->scan_fig = realloc(bumper->scan_fig,fig_count*sizeof(bumper->scan_fig[0])); + + // Construct figures + for (i = bumper->fig_count; i < fig_count; i++) + bumper->scan_fig[i] = rtk_fig_create(bumper->mainwnd->canvas, bumper->mainwnd->robot_fig, 1); + bumper->fig_count = fig_count; + } // Destroy a bumper device *************** *** 84,89 **** playerc_bumper_destroy(bumper->proxy); ! for (i = 0; i < PLAYERC_BUMPER_MAX_SAMPLES; i++) rtk_fig_destroy(bumper->scan_fig[i]); rtk_menuitem_destroy(bumper->subscribe_item); --- 94,100 ---- playerc_bumper_destroy(bumper->proxy); ! for (i = 0; i < bumper->fig_count; i++) rtk_fig_destroy(bumper->scan_fig[i]); + free(bumper->scan_fig); rtk_menuitem_destroy(bumper->subscribe_item); *************** *** 111,115 **** if (playerc_bumper_get_geom(bumper->proxy) != 0) PRINT_ERR1("get_geom failed : %s", playerc_error_str()); ! for (i = 0; i < bumper->proxy->pose_count; i++){ //fprintf(stderr, "bumper poses %02d: %f %f %f %f %f\n",i,bumper->proxy->poses[i][0],bumper->proxy->poses[i][1],bumper->proxy->poses[i][2],bumper->proxy->poses[i][3],bumper->proxy->poses[i][4]); --- 122,126 ---- if (playerc_bumper_get_geom(bumper->proxy) != 0) PRINT_ERR1("get_geom failed : %s", playerc_error_str()); ! bumper_allocate_figures(bumper, bumper->proxy->pose_count); for (i = 0; i < bumper->proxy->pose_count; i++){ //fprintf(stderr, "bumper poses %02d: %f %f %f %f %f\n",i,bumper->proxy->poses[i][0],bumper->proxy->poses[i][1],bumper->proxy->poses[i][2],bumper->proxy->poses[i][3],bumper->proxy->poses[i][4]); Index: playerv.h =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/playerv.h,v retrieving revision 1.50 retrieving revision 1.50.2.1 diff -C2 -d -r1.50 -r1.50.2.1 *** playerv.h 20 Aug 2007 19:42:49 -0000 1.50 --- playerv.h 10 Sep 2007 04:11:01 -0000 1.50.2.1 *************** *** 186,190 **** // Figures for drawing the bumper ! rtk_fig_t *scan_fig[PLAYERC_BUMPER_MAX_SAMPLES]; // Bumper device proxy --- 186,192 ---- // Figures for drawing the bumper ! rtk_fig_t **scan_fig; ! int fig_count; ! mainwnd_t *mainwnd; // Bumper device proxy *************** *** 473,477 **** // Figures for drawing the sonar scan ! rtk_fig_t *scan_fig[PLAYERC_SONAR_MAX_SAMPLES]; // Sonar device proxy --- 475,481 ---- // Figures for drawing the sonar scan ! rtk_fig_t **scan_fig; ! int fig_count; ! mainwnd_t *mainwnd; // Sonar device proxy *************** *** 512,516 **** // Figures for drawing the sonar scan ! rtk_fig_t *scan_fig[PLAYERC_IR_MAX_SAMPLES]; // Sonar device proxy --- 516,522 ---- // Figures for drawing the sonar scan ! rtk_fig_t **scan_fig; ! int fig_count; ! mainwnd_t *mainwnd; // Sonar device proxy Index: dev_laser.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_laser.c,v retrieving revision 1.27 retrieving revision 1.27.2.1 diff -C2 -d -r1.27 -r1.27.2.1 *** dev_laser.c 20 May 2007 00:30:15 -0000 1.27 --- dev_laser.c 10 Sep 2007 04:11:01 -0000 1.27.2.1 *************** *** 225,229 **** double r, b, res; int point_count; ! double points[PLAYERC_LASER_MAX_SAMPLES + 1][2]; rtk_fig_show(laser->scan_fig, 1); rtk_fig_clear(laser->scan_fig); --- 225,229 ---- double r, b, res; int point_count; ! double * points; rtk_fig_show(laser->scan_fig, 1); rtk_fig_clear(laser->scan_fig); *************** *** 253,271 **** // Draw in the range scan (empty space) ! point_count = 0; ! points[point_count][0] = 0; ! points[point_count][1] = 0; ! point_count++; for (i = 0; i < laser->proxy->scan_count; i++) { r = laser->proxy->scan[i][0]; b = laser->proxy->scan[i][1]; ! points[point_count][0] = r * cos(b - res); ! points[point_count][1] = r * sin(b - res); ! point_count++; } rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_EMP); ! rtk_fig_polygon(laser->scan_fig, 0, 0, 0, point_count, points, 1); ! // Draw in the range scan (occupied space) rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC); --- 253,269 ---- // Draw in the range scan (empty space) ! points = calloc(laser->proxy->scan_count,sizeof(double)*2); for (i = 0; i < laser->proxy->scan_count; i++) { r = laser->proxy->scan[i][0]; b = laser->proxy->scan[i][1]; ! points[i*2] = r * cos(b - res); ! points[i*2+1] = r * sin(b - res); } rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_EMP); ! rtk_fig_polygon(laser->scan_fig, 0, 0, 0, laser->proxy->scan_count, (double *)points, 1); ! free(points); ! points = NULL; ! // Draw in the range scan (occupied space) rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC); Index: dev_sonar.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_sonar.c,v retrieving revision 1.19 retrieving revision 1.19.2.1 diff -C2 -d -r1.19 -r1.19.2.1 *** dev_sonar.c 9 Jul 2007 17:18:02 -0000 1.19 --- dev_sonar.c 10 Sep 2007 04:11:01 -0000 1.19.2.1 *************** *** 57,61 **** sonar->drivername = strdup(drivername); sonar->datatime = 0; ! snprintf(section, sizeof(section), "sonar:%d", index); --- 57,62 ---- sonar->drivername = strdup(drivername); sonar->datatime = 0; ! sonar->mainwnd = mainwnd; ! snprintf(section, sizeof(section), "sonar:%d", index); *************** *** 69,79 **** rtk_menuitem_check(sonar->subscribe_item, subscribe); ! // Construct figures ! for (i = 0; i < PLAYERC_SONAR_MAX_SAMPLES; i++) ! sonar->scan_fig[i] = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 1); return sonar; } // Destroy a sonar device --- 70,90 ---- rtk_menuitem_check(sonar->subscribe_item, subscribe); ! sonar->fig_count = 0; return sonar; } + void sonar_allocate_figures(sonar_t * sonar, int fig_count) + { + int i; + if (sonar->fig_count <= fig_count) + return; + sonar->scan_fig = realloc(sonar->scan_fig,fig_count*sizeof(sonar->scan_fig[0])); + + // Construct figures + for (i = sonar->fig_count; i < fig_count; i++) + sonar->scan_fig[i] = rtk_fig_create(sonar->mainwnd->canvas, sonar->mainwnd->robot_fig, 1); + sonar->fig_count = fig_count; + } // Destroy a sonar device *************** *** 86,90 **** playerc_sonar_destroy(sonar->proxy); ! for (i = 0; i < PLAYERC_SONAR_MAX_SAMPLES; i++) rtk_fig_destroy(sonar->scan_fig[i]); --- 97,101 ---- playerc_sonar_destroy(sonar->proxy); ! for (i = 0; i < sonar->fig_count; i++) rtk_fig_destroy(sonar->scan_fig[i]); *************** *** 113,117 **** if (playerc_sonar_get_geom(sonar->proxy) != 0) PRINT_ERR1("get_geom failed : %s", playerc_error_str()); - sonar_update_geom(sonar); } --- 124,127 ---- *************** *** 149,157 **** { int i; ! for (i = 0; i < sonar->proxy->pose_count; i++) ! rtk_fig_origin(sonar->scan_fig[i], ! sonar->proxy->poses[i].px, ! sonar->proxy->poses[i].py, ! sonar->proxy->poses[i].pyaw); } --- 159,168 ---- { int i; ! sonar_allocate_figures(sonar, sonar->proxy->pose_count); ! for (i = 0; i < sonar->proxy->pose_count; i++) ! rtk_fig_origin(sonar->scan_fig[i], ! sonar->proxy->poses[i].px, ! sonar->proxy->poses[i].py, ! sonar->proxy->poses[i].pyaw); } Index: dev_ranger.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_ranger.c,v retrieving revision 1.2 retrieving revision 1.2.2.1 diff -C2 -d -r1.2 -r1.2.2.1 *** dev_ranger.c 17 Jun 2007 00:28:02 -0000 1.2 --- dev_ranger.c 10 Sep 2007 04:11:01 -0000 1.2.2.1 *************** *** 188,192 **** int ii = 0, jj = 0; int point_count; ! double point1[2], point2[2], points[PLAYER_LASER_MAX_SAMPLES + 1][2]; double current_angle = 0.0f, temp = 0.0f; unsigned int ranges_per_sensor = 0; --- 188,193 ---- int ii = 0, jj = 0; int point_count; ! double point1[2], point2[2]; ! double *points; double current_angle = 0.0f, temp = 0.0f; unsigned int ranges_per_sensor = 0; *************** *** 207,210 **** --- 208,212 ---- { // Draw sonar-like + points = calloc(3, sizeof(double)*2); temp = 20.0f * M_PI / 180.0f / 2.0f; for (ii = 0; ii < ranger->proxy->sensor_count; ii++) *************** *** 216,231 **** // Draw a cone for the first range for each sensor // Assume the range is straight ahead (ignore min_angle and resolution properties) ! points[0][0] = 0.0f; ! points[0][1] = 0.0f; ! points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp); ! points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp); ! points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp); ! points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp); rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1); ! // Draw the sensor itself rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER); rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0); } } else --- 218,235 ---- // Draw a cone for the first range for each sensor // Assume the range is straight ahead (ignore min_angle and resolution properties) ! points[0] = 0.0f; ! points[1] = 0.0f; ! points[2] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp); ! points[3] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp); ! points[4] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp); ! points[5] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp); rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1); ! // Draw the sensor itself rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER); rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0); } + free(points); + points=NULL; } else *************** *** 235,238 **** --- 239,243 ---- { // Draw each sensor in turn + points = calloc(ranger->proxy->sensor_count, sizeof(double)*2); for (ii = 0; ii < ranger->proxy->sensor_count; ii++) { *************** *** 241,246 **** // Draw empty space ! points[0][0] = ranger->proxy->sensor_poses[ii].px; ! points[0][1] = ranger->proxy->sensor_poses[ii].py; point_count = 1; current_angle = ranger->start_angle; --- 246,251 ---- // Draw empty space ! points[0] = ranger->proxy->sensor_poses[ii].px; ! points[1] = ranger->proxy->sensor_poses[ii].py; point_count = 1; current_angle = ranger->start_angle; *************** *** 248,252 **** for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++) { ! range_to_point(ranger, jj, ii, current_angle, points[point_count]); // Move round to the angle of the next range current_angle += ranger->resolution; --- 253,257 ---- for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++) { ! range_to_point(ranger, jj, ii, current_angle, &points[point_count*2]); // Move round to the angle of the next range current_angle += ranger->resolution; *************** *** 267,270 **** --- 272,278 ---- } } + free(points); + points = NULL; + } else Index: dev_ir.c =================================================================== RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_ir.c,v retrieving revision 1.3 retrieving revision 1.3.2.1 diff -C2 -d -r1.3 -r1.3.2.1 *** dev_ir.c 9 Jul 2007 17:18:02 -0000 1.3 --- dev_ir.c 10 Sep 2007 04:11:01 -0000 1.3.2.1 *************** *** 55,58 **** --- 55,59 ---- ir->drivername = strdup(drivername); ir->datatime = 0; + ir->mainwnd = mainwnd; snprintf(section, sizeof(section), "ir:%d", index); *************** *** 67,73 **** rtk_menuitem_check(ir->subscribe_item, subscribe); ! // Construct figures ! for (i = 0; i < PLAYERC_IR_MAX_SAMPLES; i++) ! ir->scan_fig[i] = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 1); return ir; --- 68,72 ---- rtk_menuitem_check(ir->subscribe_item, subscribe); ! ir->fig_count = 0; return ir; *************** *** 75,78 **** --- 74,90 ---- + void ir_allocate_figures(ir_t * ir, int fig_count) + { + int i; + if (ir->fig_count <= fig_count) + return; + ir->scan_fig = realloc(ir->scan_fig,fig_count*sizeof(ir->scan_fig[0])); + + // Construct figures + for (i = ir->fig_count; i < fig_count; i++) + ir->scan_fig[i] = rtk_fig_create(ir->mainwnd->canvas, ir->mainwnd->robot_fig, 1); + ir->fig_count = fig_count; + } + // Destroy an ir device void ir_destroy(ir_t *ir) *************** *** 84,89 **** playerc_ir_destroy(ir->proxy); ! for (i = 0; i < PLAYERC_IR_MAX_SAMPLES; i++) rtk_fig_destroy(ir->scan_fig[i]); rtk_menuitem_destroy(ir->subscribe_item); --- 96,102 ---- playerc_ir_destroy(ir->proxy); ! for (i = 0; i < ir->fig_count; i++) rtk_fig_destroy(ir->scan_fig[i]); + free(ir->scan_fig); rtk_menuitem_destroy(ir->subscribe_item); *************** *** 111,115 **** if (playerc_ir_get_geom(ir->proxy) != 0) PRINT_ERR1("get_geom failed : %s", playerc_error_str()); ! for (i = 0; i < ir->proxy->poses.poses_count; i++) { --- 124,128 ---- if (playerc_ir_get_geom(ir->proxy) != 0) PRINT_ERR1("get_geom failed : %s", playerc_error_str()); ! ir_allocate_figures(ir, ir->proxy->poses.poses_count); for (i = 0; i < ir->proxy->poses.poses_count; i++) { |