Update of /cvsroot/playerstage/code/player/utils/playerv
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22655/utils/playerv
Modified Files:
Makefile.am dev_laser.c playerv.c playerv.h registry.c
Added Files:
dev_ranger.c
Log Message:
added geoffs ranger interface
Index: registry.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/registry.c,v
retrieving revision 1.27
retrieving revision 1.28
diff -C2 -d -r1.27 -r1.28
*** registry.c 3 May 2006 12:29:12 -0000 1.27
--- registry.c 20 May 2007 00:30:15 -0000 1.28
***************
*** 101,104 ****
--- 101,113 ----
break;
+ case PLAYER_RANGER_CODE:
+ device->proxy = ranger_create(mainwnd, opt, client,
+ device->addr.index,
+ device->drivername,
+ device->subscribe);
+ device->fndestroy = (fndestroy_t) ranger_destroy;
+ device->fnupdate = (fnupdate_t) ranger_update;
+ break;
+
case PLAYER_SONAR_CODE:
device->proxy = sonar_create(mainwnd, opt, client,
Index: playerv.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/playerv.h,v
retrieving revision 1.48
retrieving revision 1.49
diff -C2 -d -r1.48 -r1.49
*** playerv.h 31 Jan 2007 22:09:57 -0000 1.48
--- playerv.h 20 May 2007 00:30:15 -0000 1.49
***************
*** 230,233 ****
--- 230,234 ----
rtk_menuitem_t *res025_item, *res050_item, *res100_item;
rtk_menuitem_t *range_mm_item, *range_cm_item, *range_dm_item;
+ rtk_menuitem_t *style_item;
// Figure for drawing the scan
rtk_fig_t *scan_fig;
***************
*** 534,537 ****
--- 535,583 ----
/***************************************************************************
+ * Ranger
+ ***************************************************************************/
+
+ // Ranger device info
+ typedef struct
+ {
+ // Driver name
+ char *drivername;
+
+ // Ranger device proxy
+ playerc_ranger_t *proxy;
+
+ // Timestamp on most recent data
+ double datatime;
+
+ // Menu stuff
+ rtk_menu_t *menu;
+ rtk_menuitem_t *subscribe_item;
+ rtk_menuitem_t *style_item;
+ rtk_menuitem_t *intns_item;
+ rtk_menuitem_t *device_item;
+ // Figure for drawing the scan
+ rtk_fig_t **scan_fig;
+ // Need to track this for creating figures
+ mainwnd_t *mainwnd;
+
+ // Properties from the device that may be necessary
+ double start_angle;
+ double resolution;
+
+ } ranger_t;
+
+
+ // Create a ranger device
+ ranger_t *ranger_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t
*client,
+ int index, const char *drivername, int subscribe);
+
+ // Destroy a ranger device
+ void ranger_destroy(ranger_t *ranger);
+
+ // Update a ranger device
+ void ranger_update(ranger_t *ranger);
+
+
+ /***************************************************************************
* Blobfinder device
***************************************************************************/
Index: dev_laser.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/dev_laser.c,v
retrieving revision 1.26
retrieving revision 1.27
diff -C2 -d -r1.26 -r1.27
*** dev_laser.c 8 Oct 2005 02:38:34 -0000 1.26
--- dev_laser.c 20 May 2007 00:30:15 -0000 1.27
***************
*** 59,62 ****
--- 59,63 ----
laser->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
laser->subscribe_item = rtk_menuitem_create(laser->menu, "Subscribe", 1);
+ laser->style_item = rtk_menuitem_create(laser->menu, "Filled", 1);
#if 0
laser->res025_item = rtk_menuitem_create(laser->menu, "0.25 deg
resolution", 1);
***************
*** 71,74 ****
--- 72,76 ----
// Set the initial menu state
rtk_menuitem_check(laser->subscribe_item, subscribe);
+ rtk_menuitem_check(laser->style_item, 1);
// Construct figures
***************
*** 97,100 ****
--- 99,103 ----
#endif
rtk_menuitem_destroy(laser->subscribe_item);
+ rtk_menuitem_destroy(laser->style_item);
rtk_menu_destroy(laser->menu);
***************
*** 226,236 ****
rtk_fig_clear(laser->scan_fig);
! // TESTING (should use menu option)
! if (laser->proxy->info.addr.index == 0)
! style = 1;
! else
! style = 0;
!
! if (style == 0)
{
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
--- 229,233 ----
rtk_fig_clear(laser->scan_fig);
! if (!rtk_menuitem_ischecked(laser->style_item))
{
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
***************
*** 251,255 ****
}
}
! else if (style == 1)
{
res = laser->proxy->scan_res / 2;
--- 248,252 ----
}
}
! else
{
res = laser->proxy->scan_res / 2;
Index: playerv.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/playerv.c,v
retrieving revision 1.47
retrieving revision 1.48
diff -C2 -d -r1.47 -r1.48
*** playerv.c 19 Aug 2006 01:17:26 -0000 1.47
--- playerv.c 20 May 2007 00:30:15 -0000 1.48
***************
*** 88,91 ****
--- 88,92 ----
- @ref interface_power
- @ref interface_ptz
+ - @ref interface_ranger
- @ref interface_sonar
- @ref interface_wifi
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/utils/playerv/Makefile.am,v
retrieving revision 1.42
retrieving revision 1.43
diff -C2 -d -r1.42 -r1.43
*** Makefile.am 3 May 2006 12:29:12 -0000 1.42
--- Makefile.am 20 May 2007 00:30:15 -0000 1.43
***************
*** 29,33 ****
dev_sonar.c \
dev_wifi.c \
! dev_gripper.c
#dev_localize.c
--- 29,34 ----
dev_sonar.c \
dev_wifi.c \
! dev_gripper.c \
! dev_ranger.c
#dev_localize.c
--- NEW FILE: dev_ranger.c ---
/*
* PlayerViewer
* Copyright (C) Andrew Howard 2002
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
*/
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "playerv.h"
// Draw the ranger scan
void ranger_draw(ranger_t *ranger);
// Create a ranger device
ranger_t* ranger_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t
*client,
int index, const char *drivername, int subscribe)
{
char label[64];
char section[64];
ranger_t *ranger;
ranger = malloc(sizeof(ranger_t));
ranger->proxy = playerc_ranger_create(client, index);
ranger->drivername = strdup(drivername);
ranger->datatime = 0;
snprintf(section, sizeof(section), "ranger:%d", index);
// Construct the menu
snprintf(label, sizeof(label), "ranger:%d (%s)", index, ranger->drivername);
ranger->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
ranger->subscribe_item = rtk_menuitem_create(ranger->menu, "Subscribe", 1);
ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 1);
ranger->intns_item = rtk_menuitem_create(ranger->menu, "Draw intensity data",
1);
ranger->device_item = rtk_menuitem_create(ranger->menu, "Singular", 1);
// Set the initial menu state
rtk_menuitem_check(ranger->subscribe_item, subscribe);
rtk_menuitem_check(ranger->style_item, 1);
rtk_menuitem_check(ranger->intns_item, 1);
ranger->mainwnd = mainwnd;
ranger->scan_fig = NULL;
return ranger;
}
void ranger_delete_figures(ranger_t *ranger)
{
int ii;
if (ranger->scan_fig != NULL)
{
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
rtk_fig_destroy(ranger->scan_fig[ii]);
free(ranger->scan_fig);
ranger->scan_fig = NULL;
}
}
// Destroy a ranger device
void ranger_destroy(ranger_t *ranger)
{
ranger_delete_figures(ranger);
if (ranger->proxy->info.subscribed)
playerc_ranger_unsubscribe(ranger->proxy);
playerc_ranger_destroy(ranger->proxy);
rtk_menuitem_destroy(ranger->subscribe_item);
rtk_menuitem_destroy(ranger->style_item);
rtk_menuitem_destroy(ranger->intns_item);
rtk_menuitem_destroy(ranger->device_item);
rtk_menu_destroy(ranger->menu);
free(ranger->drivername);
free(ranger);
}
// Update a ranger device
void ranger_update(ranger_t *ranger)
{
int ii;
// Update the device subscription
if (rtk_menuitem_ischecked(ranger->subscribe_item))
{
if (!ranger->proxy->info.subscribed)
{
if (playerc_ranger_subscribe(ranger->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// Get the ranger geometry
if (playerc_ranger_get_geom(ranger->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// Try to get the min_angle and resolution properties (don't care if
can't get them)
if (playerc_device_get_dblprop(&ranger->proxy->info, "min_angle",
&ranger->start_angle) != 0)
ranger->start_angle = 0.0f;
if (playerc_device_get_dblprop(&ranger->proxy->info, "resolution",
&ranger->resolution) != 0)
ranger->resolution = 0.0f;
// Delete any current figures
ranger_delete_figures(ranger);
// Create the figures
if ((ranger->scan_fig = malloc(ranger->proxy->sensor_count *
sizeof(rtk_fig_t*))) == NULL )
{
PRINT_ERR1("Failed to allocate memory for %d figures to display
ranger", ranger->proxy->sensor_count);
return;
}
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
ranger->scan_fig[ii] = rtk_fig_create(ranger->mainwnd->canvas,
ranger->mainwnd->robot_fig, 1);
rtk_fig_origin(ranger->scan_fig[ii],
ranger->proxy->sensor_poses[ii].px,
ranger->proxy->sensor_poses[ii].py,
ranger->proxy->sensor_poses[ii].pyaw);
}
}
}
else
{
if (ranger->proxy->info.subscribed)
if (playerc_ranger_unsubscribe(ranger->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
ranger_delete_figures(ranger);
}
rtk_menuitem_check(ranger->subscribe_item, ranger->proxy->info.subscribed);
if (ranger->proxy->info.subscribed)
{
// Draw in the ranger scan if it has been changed
if (ranger->proxy->info.datatime != ranger->datatime)
{
ranger_draw(ranger);
ranger->datatime = ranger->proxy->info.datatime;
}
}
else
{
// Dont draw the scan
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
rtk_fig_show(ranger->scan_fig[ii], 0);
}
}
// Converts a range reading into a point in the CS of the ranger device
// based on the pose of the sensor the reading is likely to belong to
void range_to_point(ranger_t *ranger, int index, int sensor_num, double angle,
double *point)
{
// Point position = point from range -> rotate by range angle// -> rotate by
sensor yaw -> translate by sensor position
point[0] = ranger->proxy->ranges[index] * cos(angle);// +
ranger->proxy->sensor_poses[sensor_num].pyaw) +
ranger->proxy->sensor_poses[sensor_num].px;
point[1] = ranger->proxy->ranges[index] * sin(angle);// +
ranger->proxy->sensor_poses[sensor_num].pyaw) +
ranger->proxy->sensor_poses[sensor_num].py;
}
// Draw the ranger scan
void ranger_draw(ranger_t *ranger)
{
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;
// Drawing type depends on the selected sensor type
// Singular sensors (e.g. sonar sensors):
// Draw a cone for the first range scan of each sensor
// Non-singular sensors (e.g. laser scanner):
// Draw the edge of the scan and empty space
// Calculate the number of ranges per sensor
if (ranger->proxy->sensor_count == 0)
ranges_per_sensor = ranger->proxy->ranges_count;
else
ranges_per_sensor = ranger->proxy->ranges_count /
ranger->proxy->sensor_count;
if (rtk_menuitem_ischecked(ranger->device_item))
{
// Draw sonar-like
temp = 20.0f * M_PI / 180.0f / 2.0f;
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN);
// 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
{
// Draw laser-like
if (rtk_menuitem_ischecked(ranger->style_item))
{
// Draw each sensor in turn
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
// 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;
// Loop over the ranges
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;
point_count++;
}
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1);
// Draw occupied space
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor;
jj++)
{
range_to_point(ranger, jj, ii, current_angle - ranger->resolution,
point1);
range_to_point(ranger, jj, ii, current_angle + ranger->resolution,
point2);
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0],
point2[1]);
current_angle += ranger->resolution;
}
}
}
else
{
// Draw a range scan for each individual sensor in the device
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
// Get the first point
range_to_point(ranger, ii * ranges_per_sensor, ii, ranger->start_angle,
point1);
// Loop over the rest of the ranges
for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) *
ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle, point2);
// Draw a line from point 1 (previous point) to point 2 (current
point)
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0],
point2[1]);
point1[0] = point2[0];
point1[1] = point2[1];
// Move round to the angle of the next range
current_angle += ranger->resolution;
}
}
}
// For each sensor...
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
if (rtk_menuitem_ischecked(ranger->intns_item))
{
// Draw an intensity scan
if (ranger->proxy->intensities_count > 0)
{
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor;
jj++)
{
if (ranger->proxy->intensities[0] != 0)
{
range_to_point(ranger, jj, ii, current_angle, point1);
rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0,
0.05, 0.05, 1);
}
current_angle += ranger->resolution;
}
}
}
// 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);
}
}
}
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
