Revision: 8965
http://playerstage.svn.sourceforge.net/playerstage/?rev=8965&view=rev
Author: jpgr87
Date: 2010-11-12 04:37:27 +0000 (Fri, 12 Nov 2010)
Log Message:
-----------
Applied patch #3055958: Adds uncertrainty ellipsis for localization to utils
Modified Paths:
--------------
code/player/trunk/CMakeLists.txt
code/player/trunk/utils/playernav/CMakeLists.txt
code/player/trunk/utils/playernav/gui.c
code/player/trunk/utils/playernav/playernav.c
code/player/trunk/utils/playernav/playernav.h
code/player/trunk/utils/playerv/CMakeLists.txt
code/player/trunk/utils/playerv/pv_dev_localize.c
Modified: code/player/trunk/CMakeLists.txt
===================================================================
--- code/player/trunk/CMakeLists.txt 2010-11-12 04:13:53 UTC (rev 8964)
+++ code/player/trunk/CMakeLists.txt 2010-11-12 04:37:27 UTC (rev 8965)
@@ -79,6 +79,7 @@
ADD_SUBDIRECTORY (libplayerjpeg)
ADD_SUBDIRECTORY (libplayertcp)
ADD_SUBDIRECTORY (libplayersd)
+ADD_SUBDIRECTORY (libplayerutil)
ADD_SUBDIRECTORY (rtk2)
ADD_SUBDIRECTORY (server)
ADD_SUBDIRECTORY (examples)
Modified: code/player/trunk/utils/playernav/CMakeLists.txt
===================================================================
--- code/player/trunk/utils/playernav/CMakeLists.txt 2010-11-12 04:13:53 UTC
(rev 8964)
+++ code/player/trunk/utils/playernav/CMakeLists.txt 2010-11-12 04:37:27 UTC
(rev 8965)
@@ -7,7 +7,7 @@
${GNOMECANVAS_PKG_INCLUDE_DIRS})
LINK_DIRECTORIES (${GNOMECANVAS_PKG_LIBRARY_DIRS})
PLAYER_ADD_EXECUTABLE (playernav ${playernavSrcs})
- TARGET_LINK_LIBRARIES (playernav playerc playerinterface playercommon
+ TARGET_LINK_LIBRARIES (playernav playerc playerinterface playercommon
playerutil
${PLAYERC_EXTRA_LINK_LIBRARIES} ${GNOMECANVAS_PKG_LIBRARIES})
SET_SOURCE_FILES_PROPERTIES (${playernavSrcs} PROPERTIES
COMPILE_FLAGS "${GNOMECANVAS_CFLAGS}")
Modified: code/player/trunk/utils/playernav/gui.c
===================================================================
--- code/player/trunk/utils/playernav/gui.c 2010-11-12 04:13:53 UTC (rev
8964)
+++ code/player/trunk/utils/playernav/gui.c 2010-11-12 04:37:27 UTC (rev
8965)
@@ -31,6 +31,7 @@
#include <gdk/gdkkeysyms.h>
#include <libplayercommon/playercommon.h>
+#include <libplayerutil/localization.h>
#include "playernav.h"
// flag and index for robot currently being moved by user (if any)
@@ -53,6 +54,7 @@
extern int dumpp;
extern int showparticlesp;
+extern int showuncertaintyp;
int show_robot_names;
/*
@@ -159,7 +161,56 @@
return(TRUE);
}
+static gboolean
+_show_uncertainty(GtkWidget *widget,
+ GdkEvent *event,
+ gpointer data)
+{
+ int rob_i, hyp_i;
+ gui_data_t* gui_data = (gui_data_t*)widget;
+ gboolean onmap;
+ player_pose2d_t hyp_pose;
+ showuncertaintyp = !showuncertaintyp;
+ if(showuncertaintyp)
+ {
+ for(rob_i = 0; rob_i<gui_data->num_robots;++rob_i)
+ {
+ onmap = false;
+ if(!gui_data->localizes[rob_i] ||
+ (gui_data->localizes[rob_i]->hypoth_count <= 0) )
+ continue;
+
+ // Check if at least one hypotheses is on the map
+ for(hyp_i = 0; hyp_i<gui_data->localizes[rob_i]->hypoth_count; ++hyp_i)
+ {
+ hyp_pose = gui_data->localizes[rob_i]->hypoths[hyp_i].mean;
+
+ onmap |= ((fabs(hyp_pose.px) <
+ (gui_data->mapdev->width *
+ gui_data->mapdev->resolution / 2.0)) &&
+ (fabs(hyp_pose.py) <
+ (gui_data->mapdev->height *
+ gui_data->mapdev->resolution / 2.0)));
+ }
+
+ if(onmap)
+ {
+ draw_uncertainty(gui_data,rob_i);
+ }
+ }
+ }
+ else
+ {
+ for(rob_i=0; rob_i<gui_data->num_robots; ++rob_i)
+ {
+ if(gui_data->robot_uncertainty[rob_i])
+ gnome_canvas_item_hide(gui_data->robot_uncertainty[rob_i]);
+ }
+ }
+ return(TRUE);
+}
+
static gboolean
_stop_all_robots(GtkWidget *widget,
GdkEvent *event,
@@ -470,6 +521,7 @@
GtkMenuItem* view_item;
GtkCheckMenuItem* show_names_item;
GtkCheckMenuItem* show_particles_item;
+ GtkCheckMenuItem* show_uncertainty_item;
GtkCheckMenuItem* refresh_map_item;
GtkMenu* stop_menu;
@@ -492,6 +544,7 @@
dump_item = (GtkCheckMenuItem*)gtk_check_menu_item_new_with_label("Capture
stills");
show_names_item =
(GtkCheckMenuItem*)gtk_check_menu_item_new_with_label("Show robot names");
show_particles_item =
(GtkCheckMenuItem*)gtk_check_menu_item_new_with_label("Show particles");
+ show_uncertainty_item =
(GtkCheckMenuItem*)gtk_check_menu_item_new_with_label("Show uncertainty");
refresh_map_item = (GtkCheckMenuItem*)gtk_menu_item_new_with_label("Refresh
map");
stop_all_item = (GtkMenuItem*)gtk_menu_item_new_with_label("Stop all
robots");
go_all_item = (GtkMenuItem*)gtk_menu_item_new_with_label("Go all robots");
@@ -530,6 +583,7 @@
gtk_menu_shell_append (GTK_MENU_SHELL(file_menu), (GtkWidget*)quit_item);
gtk_menu_shell_append (GTK_MENU_SHELL(view_menu),
(GtkWidget*)show_names_item);
gtk_menu_shell_append (GTK_MENU_SHELL(view_menu),
(GtkWidget*)show_particles_item);
+ gtk_menu_shell_append (GTK_MENU_SHELL(view_menu),
(GtkWidget*)show_uncertainty_item);
gtk_menu_shell_append (GTK_MENU_SHELL(view_menu),
(GtkWidget*)refresh_map_item);
gtk_menu_shell_append (GTK_MENU_SHELL(stop_menu), (GtkWidget*)stop_all_item);
gtk_menu_shell_append (GTK_MENU_SHELL(stop_menu), (GtkWidget*)go_all_item);
@@ -548,6 +602,9 @@
g_signal_connect_swapped(G_OBJECT (show_particles_item), "activate",
G_CALLBACK(_show_particles),
(gpointer) gui_data);
+ g_signal_connect_swapped(G_OBJECT (show_uncertainty_item), "activate",
+ G_CALLBACK(_show_uncertainty),
+ (gpointer) gui_data);
g_signal_connect_swapped(G_OBJECT (refresh_map_item), "activate",
G_CALLBACK(_refresh_map),
(gpointer) gui_data);
@@ -563,6 +620,7 @@
gtk_widget_show((GtkWidget*)quit_item);
gtk_widget_show((GtkWidget*)show_names_item);
gtk_widget_show((GtkWidget*)show_particles_item);
+ gtk_widget_show((GtkWidget*)show_uncertainty_item);
gtk_widget_show((GtkWidget*)refresh_map_item);
gtk_widget_show((GtkWidget*)stop_all_item);
gtk_widget_show((GtkWidget*)go_all_item);
@@ -1019,6 +1077,52 @@
}
void
+draw_uncertainty(gui_data_t* gui_data, int idx) {
+ int hyp_i;
+ player_pose2d_t ellipse_pose;
+ pose_t ep;
+ player_localize_hypoth_t* hypo;
+ double radius_x,radius_y;
+ GnomeCanvasItem* ellipse;
+
+
+ if(gui_data->robot_uncertainty[idx])
+ {
+ gtk_object_destroy(GTK_OBJECT(gui_data->robot_uncertainty[idx]));
+ gui_data->robot_uncertainty[idx] = NULL;
+ }
+
+ if(gui_data->localizes[idx]->hypoth_count)
+ {
+ g_assert((gui_data->robot_uncertainty[idx] =
+ gnome_canvas_item_new(gnome_canvas_root(gui_data->map_canvas),
+ gnome_canvas_group_get_type(),
+ "x", 0.0, "y", 0.0,
+ NULL)));
+ for(hyp_i = 0; hyp_i<gui_data->localizes[idx]->hypoth_count; ++hyp_i)
+ {
+ hypo = &(gui_data->localizes[idx]->hypoths[hyp_i]);
+ derive_uncertainty_ellipsis2d(&ellipse_pose, &radius_x, &radius_y,
+ hypo, 0.68);
+ g_assert((ellipse =
+
gnome_canvas_item_new((GnomeCanvasGroup*)gui_data->robot_uncertainty[idx],
+ gnome_canvas_ellipse_get_type(),
+ "x1", -radius_x,
+ "y1", -radius_y,
+ "x2", radius_x,
+ "y2", radius_y,
+ "fill-color-rgba",
+ robot_colors[idx % num_robot_colors],
+ NULL)));
+ ep.px = ellipse_pose.px;
+ ep.py = ellipse_pose.py;
+ ep.pa = ellipse_pose.pa;
+ move_item(ellipse,ep,0);
+ }
+ }
+}
+
+void
draw_waypoints(gui_data_t* gui_data, int idx)
{
int i;
Modified: code/player/trunk/utils/playernav/playernav.c
===================================================================
--- code/player/trunk/utils/playernav/playernav.c 2010-11-12 04:13:53 UTC
(rev 8964)
+++ code/player/trunk/utils/playernav/playernav.c 2010-11-12 04:37:27 UTC
(rev 8965)
@@ -41,7 +41,8 @@
and dropping them in the map. You can set global goals the same way,
and see the planned paths and the robots' progress toward the goals.
playernav can also display (a subset of) the localization system's current
-particle set, which may help in debugging localization. You can think
+particle set, which may help in debugging localization. Furthermore is is able
+to visualize the uncertainty of the estimated postition. You can think
of playernav as an Operator Control Unit (OCU). playernav can also be
used just to view a map.
@@ -58,7 +59,7 @@
- -fps <dumprate> : when requested, dump screenshots at this rate in Hz
(default: 5Hz)
- -zoom <zoom> : initial level of zoom in the display (default: 1)
- -aa {0|1} : whether to use anti-aliased canvas for display; the anti-aliased
canvas looks nicer but may require more processor cycles (default: 1)
-- -map <map_idx> : the index of the map to be requested and displayed
(default: 0)
+- -mapidx <map_idx> : the index of the map to be requested and
displayed (default: 0)
playernav will connect to Player at each host:port combination given on
the command line. For each one, playernav will attempt to subscribe to
@@ -149,6 +150,7 @@
double dumpfreq;
int dumpp;
int showparticlesp;
+int showuncertaintyp;
double mapupdatefreq=0.0;
int get_waypoints=1;
@@ -275,6 +277,12 @@
playerc_localize_get_particles(gui_data->localizes[i]);
draw_particles(gui_data,i);
}
+
+ // If we have a localizer retrive and draw uncertainty ellipsis
+ if(showuncertaintyp && gui_data->localizes[i])
+ {
+ draw_uncertainty(gui_data,i);
+ }
}
}
Modified: code/player/trunk/utils/playernav/playernav.h
===================================================================
--- code/player/trunk/utils/playernav/playernav.h 2010-11-12 04:13:53 UTC
(rev 8964)
+++ code/player/trunk/utils/playernav/playernav.h 2010-11-12 04:37:27 UTC
(rev 8965)
@@ -71,6 +71,7 @@
GnomeCanvasItem* robot_goals[MAX_NUM_ROBOTS];
GnomeCanvasItem* robot_paths[MAX_NUM_ROBOTS];
GnomeCanvasItem* robot_particles[MAX_NUM_ROBOTS];
+ GnomeCanvasItem* robot_uncertainty[MAX_NUM_ROBOTS];
playerc_mclient_t* mclient;
playerc_client_t* clients[MAX_NUM_ROBOTS];
playerc_map_t* maps[MAX_NUM_ROBOTS];
@@ -97,6 +98,8 @@
void draw_waypoints(gui_data_t* gui_data, int idx);
void draw_goal(gui_data_t* gui_data, int idx);
void draw_particles(gui_data_t* gui_data, int idx);
+void draw_uncertainty(gui_data_t* gui_data, int idx);
+
void dump_screenshot(gui_data_t* gui_data);
playerc_mclient_t* init_player(playerc_client_t** clients,
Modified: code/player/trunk/utils/playerv/CMakeLists.txt
===================================================================
--- code/player/trunk/utils/playerv/CMakeLists.txt 2010-11-12 04:13:53 UTC
(rev 8964)
+++ code/player/trunk/utils/playerv/CMakeLists.txt 2010-11-12 04:37:27 UTC
(rev 8965)
@@ -31,7 +31,7 @@
INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR}/client_libs
${GTK_PKG_INCLUDE_DIRS})
LINK_DIRECTORIES (${GTK_PKG_LIBRARY_DIRS})
PLAYER_ADD_EXECUTABLE (playerv ${playervSrcs})
- TARGET_LINK_LIBRARIES (playerv playerc playerinterface playercommon
+ TARGET_LINK_LIBRARIES (playerv playerc playerinterface playercommon
playerutil
rtk ${PLAYERC_EXTRA_LINK_LIBRARIES} ${GTK_PKG_LIBRARIES})
IF (PLAYER_OS_SOLARIS)
TARGET_LINK_LIBRARIES (playerv rt)
Modified: code/player/trunk/utils/playerv/pv_dev_localize.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_localize.c 2010-11-12 04:13:53 UTC
(rev 8964)
+++ code/player/trunk/utils/playerv/pv_dev_localize.c 2010-11-12 04:37:27 UTC
(rev 8965)
@@ -28,6 +28,7 @@
#include <math.h>
#include <stdlib.h>
#include <string.h>
+#include <libplayerutil/localization.h>
#include "playerv.h"
#if defined (WIN32)
@@ -208,14 +209,10 @@
void localize_draw_hypoth(localize_t *self)
{
int i;
- double ox, oy, oa;
+ player_pose2d_t epose;
double sx, sy;
- double cov[2][2], eval[2], evec[2][2];
- double max_weight;
player_localize_hypoth_t *hypoth;
- max_weight = 0.0;
-
rtk_fig_clear(self->hypoth_fig);
rtk_fig_color_rgb32(self->hypoth_fig, COLOR_LOCALIZE);
@@ -223,21 +220,9 @@
{
hypoth = self->proxy->hypoths + i;
- // Get eigen values/vectors
- 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.px ;
- oy = hypoth->mean.py ;
+ derive_uncertainty_ellipsis2d(&epose, &sx, &sy,
+ hypoth, 0.68);
- oa = atan2(evec[0][1], evec[0][0]);
-
- sx = 6 * sqrt(eval[0]);
- sy = 6 * sqrt(eval[1]);
-
if (sx < 0.10)
sx = 0.10;
if (sy < 0.10)
@@ -245,9 +230,10 @@
if (sx > 1e-3 && sy > 1e-3)
{
- rtk_fig_line_ex(self->hypoth_fig, ox, oy, oa, sx);
- rtk_fig_line_ex(self->hypoth_fig, ox, oy, oa + M_PI / 2, sy);
- rtk_fig_ellipse(self->hypoth_fig, ox, oy, oa, sx, sy, 0);
+ rtk_fig_line_ex(self->hypoth_fig, epose.px, epose.py, epose.pa, sx);
+ rtk_fig_line_ex(self->hypoth_fig, epose.px, epose.py,
+ epose.pa + M_PI / 2, sy);
+ rtk_fig_ellipse(self->hypoth_fig, epose.px, epose.py, epose.pa, sx,sy,
0);
}
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Centralized Desktop Delivery: Dell and VMware Reference Architecture
Simplifying enterprise desktop deployment and management using
Dell EqualLogic storage and VMware View: A highly scalable, end-to-end
client virtualization framework. Read more!
http://p.sf.net/sfu/dell-eql-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
