Revision: 8797
http://playerstage.svn.sourceforge.net/playerstage/?rev=8797&view=rev
Author: jpgr87
Date: 2010-06-28 01:36:37 +0000 (Mon, 28 Jun 2010)
Log Message:
-----------
Applied patch 3018514: Localize interface visualization in playerv
Modified Paths:
--------------
code/player/trunk/utils/playerv/CMakeLists.txt
code/player/trunk/utils/playerv/playerv.h
code/player/trunk/utils/playerv/pv_dev_localize.c
code/player/trunk/utils/playerv/registry.c
Modified: code/player/trunk/utils/playerv/CMakeLists.txt
===================================================================
--- code/player/trunk/utils/playerv/CMakeLists.txt 2010-06-28 01:28:57 UTC
(rev 8796)
+++ code/player/trunk/utils/playerv/CMakeLists.txt 2010-06-28 01:36:37 UTC
(rev 8797)
@@ -17,6 +17,7 @@
pv_dev_fiducial.c
pv_dev_ir.c
pv_dev_laser.c
+ pv_dev_localize.c
pv_dev_map.c
pv_dev_position2d.c
pv_dev_power.c
Modified: code/player/trunk/utils/playerv/playerv.h
===================================================================
--- code/player/trunk/utils/playerv/playerv.h 2010-06-28 01:28:57 UTC (rev
8796)
+++ code/player/trunk/utils/playerv/playerv.h 2010-06-28 01:36:37 UTC (rev
8797)
@@ -54,7 +54,8 @@
#define COLOR_LASER 0x0000C0
#define COLOR_LASER_EMP 0xD0D0FF
#define COLOR_LASER_OCC 0x0000C0
-#define COLOR_LOCALIZE 0xFF0000
+#define COLOR_LOCALIZE 0xFF00FF
+#define COLOR_LOCALIZE_PARTICLES 0x0000FF
#define COLOR_FIDUCIAL 0xF000F0
#define COLOR_POSITION_ROBOT 0xC00000
#define COLOR_POSITION_CONTROL 0xFF0000
@@ -734,22 +735,17 @@
rtk_menu_t *menu;
rtk_menuitem_t *subscribe_item;
rtk_menuitem_t *reset_item;
- rtk_menuitem_t *showmap_item;
+ rtk_menuitem_t *showparticles_item;
rtk_menuitem_t *frame_item;
// localize device proxy
playerc_localize_t *proxy;
// Figures
- rtk_fig_t *map_fig;
rtk_fig_t *hypoth_fig;
-
+ rtk_fig_t *particles_fig;
// Map image
- uint16_t *map_image;
- // Map magnification factor (1 = full size, 2 = half size, etc)
- int map_mag;
-
// Timestamp on most recent data
double datatime;
Modified: code/player/trunk/utils/playerv/pv_dev_localize.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_localize.c 2010-06-28 01:28:57 UTC
(rev 8796)
+++ code/player/trunk/utils/playerv/pv_dev_localize.c 2010-06-28 01:36:37 UTC
(rev 8797)
@@ -36,12 +36,11 @@
// Reset the pose
void localize_reset_pose(localize_t *localize);
-// Draw the map
-void localize_draw_map(localize_t *localize);
-
// Draw the hypotheses
void localize_draw_hypoth(localize_t *localize);
+// Draw particles
+void localize_draw_particles(localize_t *localize);
// Create a localize device
localize_t *localize_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t
*client,
@@ -65,7 +64,7 @@
self->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
self->subscribe_item = rtk_menuitem_create(self->menu, "Subscribe", 1);
self->reset_item = rtk_menuitem_create(self->menu, "Reset", 0);
- self->showmap_item = rtk_menuitem_create(self->menu, "Show Map", 1);
+ self->showparticles_item = rtk_menuitem_create(self->menu, "Show Particles",
1);
// We can use this device to give us a coordinate system
snprintf(label, sizeof(label), "Frame localize:%d (%s)", index,
self->drivername);
@@ -73,16 +72,13 @@
// Set the initial menu state
rtk_menuitem_check(self->subscribe_item, subscribe);
- rtk_menuitem_check(self->showmap_item, 1);
+ rtk_menuitem_check(self->showparticles_item, 1);
rtk_menuitem_check(self->frame_item, 0);
// Construct figures
- self->map_fig = rtk_fig_create(mainwnd->canvas, NULL, -80);
- self->hypoth_fig = rtk_fig_create(mainwnd->canvas, self->map_fig, 85);
+ self->hypoth_fig = rtk_fig_create(mainwnd->canvas, NULL, 85);
+ self->particles_fig = rtk_fig_create(mainwnd->canvas, self->hypoth_fig, 85);
- // Default magnification for the map (1/8th full size)
- self->map_mag = 8;
-
return self;
}
@@ -96,17 +92,17 @@
playerc_localize_destroy(self->proxy);
// Destroy figures
- rtk_fig_destroy(self->map_fig);
+ rtk_fig_destroy(self->hypoth_fig);
+ rtk_fig_destroy(self->particles_fig);
// Destroy menu items
rtk_menuitem_destroy(self->subscribe_item);
rtk_menuitem_destroy(self->reset_item);
- rtk_menuitem_destroy(self->showmap_item);
+ rtk_menuitem_destroy(self->showparticles_item);
rtk_menu_destroy(self->menu);
// Free memory
free(self->drivername);
- free(self->map_image);
free(self);
return;
@@ -123,25 +119,8 @@
{
if (!self->proxy->info.subscribed)
{
- if (playerc_localize_subscribe(self->proxy, PLAYER_READ_MODE) != 0)
+ if (playerc_localize_subscribe(self->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("subscribe failed : %s", playerc_error_str());
-
- // Load the map
- /*
- printf("Loading map (this may take some time)...");
- fflush(stdout);
- if (playerc_localize_get_map(self->proxy) != 0)
- PRINT_ERR1("get_map_header failed : %s", playerc_error_str());
- printf("done\n");
- */
-
- // deprecated: get the map from the map interface now
-#if 0
- if (playerc_localize_get_map_info(self->proxy) != 0)
- PRINT_ERR1("get_map_info failed : %s", playerc_error_str());
- // Draw the map
- localize_draw_map(self);
-#endif
}
}
else
@@ -161,24 +140,22 @@
// update the screen
if (self->proxy->info.subscribed)
{
+
// Show the figures
- rtk_fig_show(self->map_fig, rtk_menuitem_ischecked(self->showmap_item));
+ rtk_fig_show(self->particles_fig,
rtk_menuitem_ischecked(self->showparticles_item));
rtk_fig_show(self->hypoth_fig, 1);
- // deprecated: get the map from the map interface now
-#if 0
- // Draw the map if we got more map data
- if (playerc_localize_get_map_tile(self->proxy) == 0)
- localize_draw_map(self);
-#endif
- // Draw the map if the frame has changed
- if (rtk_menuitem_isactivated(self->frame_item))
- localize_draw_map(self);
-
- // Draw in the localize hypothesis if it has been changed.
+ // Draw in the localize hypothesis and particles if they have been changed.
if (self->proxy->info.datatime != self->datatime)
+ {
localize_draw_hypoth(self);
+ }
+ if(rtk_menuitem_ischecked(self->showparticles_item))
+ if (playerc_localize_get_particles(self->proxy)==0)
+ localize_draw_particles(self);
+
+
self->datatime = self->proxy->info.datatime;
// Set the global robot pose
@@ -186,9 +163,9 @@
{
if (self->proxy->hypoth_count > 0)
{
- ox = self->proxy->hypoths[0].mean[0];
- oy = self->proxy->hypoths[0].mean[1];
- oa = self->proxy->hypoths[0].mean[2];
+ ox = self->proxy->hypoths[0].mean.px;
+ oy = self->proxy->hypoths[0].mean.py;
+ oa = self->proxy->hypoths[0].mean.pa;
rtk_fig_origin(self->mainwnd->robot_fig, ox, oy, oa);
}
}
@@ -196,8 +173,8 @@
else
{
// Hide the figures
- rtk_fig_show(self->map_fig, 0);
rtk_fig_show(self->hypoth_fig, 0);
+ rtk_fig_show(self->particles_fig, 0);
self->datatime = 0;
}
}
@@ -207,132 +184,32 @@
void localize_reset_pose(localize_t *self)
{
double pose[3];
- double cov[3][3];
+ double cov[3];
pose[0] = 0.0;
pose[1] = 0.0;
pose[2] = 0.0;
- cov[0][0] = 1e3 * 1e3;
- cov[0][1] = 0.0;
- cov[0][2] = 0.0;
+ cov[0] = 1e3 * 1e3;
+ cov[1] = 1e3 * 1e3;
+ cov[2] = 1e3 * 1e3;
- cov[1][0] = 0.0;
- cov[1][1] = 1e3 * 1e3;
- cov[1][2] = 0.0;
-
- cov[2][0] = 0.0;
- cov[2][1] = 0.0;
- cov[2][2] = 1e3 * 1e3;
-
if (playerc_localize_set_pose(self->proxy, pose, cov) != 0)
PRINT_ERR1("set pose failed : %s", playerc_error_str());
return;
}
-
-// Draw the map
-void localize_draw_map(localize_t *self)
-{
- int i, j, mag;
- double scale;
- uint16_t col;
- uint16_t *image;
- int size_x, size_y;
- int ssize_x, ssize_y;
- int csize_x, csize_y;
- double cscale_x, cscale_y;
- scale = self->proxy->map_scale;
-
- if (rtk_menuitem_ischecked(self->frame_item) == 1)
- mag = 1.0;
- else
- mag = self->map_mag;
-
- // Original map dimensions
- size_x = self->proxy->map_size_x;
- size_y = self->proxy->map_size_y;
-
- // Scaled map dimensions
- ssize_x = (int) ((double) size_x / mag + 0.5);
- ssize_y = (int) ((double) size_y / mag + 0.5);
-
- // Canvas dimensions
- rtk_canvas_get_size(self->map_fig->canvas, &csize_x, &csize_y);
- rtk_canvas_get_scale(self->map_fig->canvas, &cscale_x, &cscale_y);
-
- // Set the initial pose of the map
- if (rtk_menuitem_ischecked(self->frame_item) == 1)
- {
- rtk_fig_movemask(self->map_fig, 0);
- rtk_fig_origin(self->map_fig, 0, 0, 0);
- }
- else
- {
- rtk_fig_movemask(self->map_fig, RTK_MOVE_TRANS);
- rtk_fig_origin(self->map_fig,
- -csize_x / 2 * cscale_x + ssize_x * scale / 2,
- +csize_y / 2 * cscale_y - ssize_y * scale / 2, 0);
- }
-
- // Construct an image representing the map
- image = malloc(ssize_x * ssize_y * sizeof(uint16_t));
-
- for (j = 0; j < ssize_y; j++)
- for (i = 0; i < ssize_x; i++)
- image[i + j * ssize_x] = RTK_RGB16(255, 255, 255);
-
- for (j = 0; j < size_y; j++)
- {
- for (i = 0; i < size_x; i++)
- {
- switch (self->proxy->map_cells[i + j * size_x])
- {
- case -1:
- col = RTK_RGB16(255, 255, 255);
- break;
- case 0:
- col = RTK_RGB16(192, 192, 192);
- break;
- case +1:
- col = RTK_RGB16(0, 0, 0);
- break;
- }
- if (col < image[i / mag + j / mag * ssize_x])
- image[i / mag + j / mag * ssize_x] = col;
- }
- }
-
- // Draw the image
- rtk_fig_show(self->map_fig, 1);
- rtk_fig_clear(self->map_fig);
- rtk_fig_image(self->map_fig, 0, 0, 0, scale, ssize_x, ssize_y, 16, image,
NULL);
- rtk_fig_color(self->map_fig, 0, 0, 0);
- rtk_fig_rectangle(self->map_fig, 0, 0, 0, ssize_x * scale, ssize_y * scale,
0);
-
- free(image);
-
- return;
-}
-
-
// Draw the hypotheses
void localize_draw_hypoth(localize_t *self)
{
int i;
- double mag;
double ox, oy, oa;
double sx, sy;
double cov[2][2], eval[2], evec[2][2];
double max_weight;
- playerc_localize_hypoth_t *hypoth;
+ player_localize_hypoth_t *hypoth;
- if (rtk_menuitem_ischecked(self->frame_item) == 1)
- mag = 1.0;
- else
- mag = self->map_mag;
-
max_weight = 0.0;
rtk_fig_clear(self->hypoth_fig);
@@ -343,18 +220,19 @@
hypoth = self->proxy->hypoths + i;
// Get eigen values/vectors
- cov[0][0] = hypoth->cov[0][0];
- cov[0][1] = hypoth->cov[0][1];
- cov[1][0] = hypoth->cov[1][0];
- cov[1][1] = hypoth->cov[1][1];
+ cov[0][0] = hypoth->cov[0];
+ cov[0][1] = 0;
+ cov[1][0] = 0;
+ cov[1][1] = hypoth->cov[1];
eigen(cov, eval, evec);
- ox = hypoth->mean[0] / mag;
- oy = hypoth->mean[1] / mag;
+ ox = hypoth->mean.px ;
+ oy = hypoth->mean.py ;
+
oa = atan2(evec[0][1], evec[0][0]);
- sx = 6 * sqrt(eval[0]) / mag;
- sy = 6 * sqrt(eval[1]) / mag;
+ sx = 6 * sqrt(eval[0]);
+ sy = 6 * sqrt(eval[1]);
if (sx < 0.10)
sx = 0.10;
@@ -372,6 +250,26 @@
return;
}
+// Draw the particles
+void localize_draw_particles(localize_t *self)
+{
+ int i;
+ double ox, oy, oa;
+ i=0;
+ rtk_fig_clear(self->particles_fig);
+ rtk_fig_color_rgb32(self->particles_fig, COLOR_LOCALIZE_PARTICLES);
+ for (i = 0; i < self->proxy->num_particles; i++)
+ {
+ playerc_localize_particle_t particle=self->proxy->particles[i];
+ ox=particle.pose[0];
+ oy=particle.pose[1];
+ oa=particle.pose[2];
+ rtk_fig_line_ex(self->particles_fig, ox, oy, oa, 0.03);
+ rtk_fig_line_ex(self->particles_fig, ox, oy, oa + M_PI_2, 0.03);
+ }
+
+ return;
+}
// Compute eigen values and eigenvectors of a 2x2 covariance matrix
static void eigen(double cm[][2], double values[], double vectors[][2])
Modified: code/player/trunk/utils/playerv/registry.c
===================================================================
--- code/player/trunk/utils/playerv/registry.c 2010-06-28 01:28:57 UTC (rev
8796)
+++ code/player/trunk/utils/playerv/registry.c 2010-06-28 01:36:37 UTC (rev
8797)
@@ -193,18 +193,16 @@
break;
-#if 0
+
case PLAYER_LOCALIZE_CODE:
device->proxy = localize_create(mainwnd, opt, client,
- device->index, device->drivername,
device->subscribe);
+ device->addr.index,
+ device->drivername,
+ device->subscribe);
device->fndestroy = (fndestroy_t) localize_destroy;
device->fnupdate = (fnupdate_t) localize_update;
break;
-
-
-#endif
-
default:
device->proxy = NULL;
device->fndestroy = NULL;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by Sprint
What will you do first with EVO, the first 4G phone?
Visit sprint.com/first -- http://p.sf.net/sfu/sprint-com-first
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
