Revision: 6834
http://playerstage.svn.sourceforge.net/playerstage/?rev=6834&view=rev
Author: gbiggs
Date: 2008-07-09 19:09:02 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
Added scan point calculation to the ranger device
Modified Paths:
--------------
code/player/branches/release-2-1-patches/client_libs/libplayerc/dev_ranger.c
code/player/branches/release-2-1-patches/client_libs/libplayerc/playerc.h
Modified:
code/player/branches/release-2-1-patches/client_libs/libplayerc/dev_ranger.c
===================================================================
---
code/player/branches/release-2-1-patches/client_libs/libplayerc/dev_ranger.c
2008-07-10 01:34:50 UTC (rev 6833)
+++
code/player/branches/release-2-1-patches/client_libs/libplayerc/dev_ranger.c
2008-07-10 02:09:02 UTC (rev 6834)
@@ -38,6 +38,7 @@
*/
#include <string.h>
+#include <math.h>
#include "playerc.h"
#include "error.h"
@@ -72,6 +73,10 @@
free(device->sensor_poses);
if(device->sensor_sizes != NULL)
free(device->sensor_sizes);
+ if(device->bearings != NULL)
+ free(device->bearings);
+ if(device->points != NULL)
+ free(device->points);
free(device);
}
@@ -90,16 +95,74 @@
}
+// Calculate bearings
+void playerc_ranger_calculate_bearings(playerc_ranger_t *device)
+{
+ device->bearings_count = device->ranges_count;
+ if (device->bearings_count == 0 && device->bearings != NULL)
+ {
+ free(device->bearings);
+ device->bearings = NULL;
+ }
+ else
+ {
+ if((device->bearings = (double *) realloc(device->bearings,
device->bearings_count * sizeof(double))) == NULL)
+ {
+ device->bearings_count = 0;
+ PLAYER_ERROR("Failed to allocate space to store bearings");
+ return;
+ }
+
+ double b = device->min_angle;
+ uint32_t ii;
+ for (ii = 0; ii < device->bearings_count; ii++)
+ {
+ device->bearings[ii] = b;
+ b += device->resolution;
+ }
+ }
+}
+
+
+// Calculate scan points
+void playerc_ranger_calculate_points(playerc_ranger_t *device)
+{
+ device->points_count = device->ranges_count;
+ if (device->points_count == 0 && device->points != NULL)
+ {
+ free(device->points);
+ device->points = NULL;
+ }
+ else
+ {
+ if((device->points = (player_point_3d_t *) realloc(device->points,
device->points_count * sizeof(player_point_3d_t))) == NULL)
+ {
+ device->points_count = 0;
+ PLAYER_ERROR("Failed to allocate space to store points");
+ return;
+ }
+
+ double b = device->min_angle;
+ uint32_t ii;
+ for (ii = 0; ii < device->points_count; ii++)
+ {
+ double r = device->ranges[ii];
+ device->points[ii].px = r * cos(b);
+ device->points[ii].py = r * sin(b);
+ device->points[ii].pz = 0.0;
+ b += device->resolution;
+ }
+ }
+}
+
+
// Copy range data to the device
void playerc_ranger_copy_range_data(playerc_ranger_t *device,
player_ranger_data_range_t *data)
{
if (device->ranges_count != data->ranges_count || device->ranges == NULL)
{
- // The number of data has changed, so delete any old data
- if(device->ranges != NULL)
- free(device->ranges);
// Allocate memory for the new data
- if((device->ranges = (double *) malloc(data->ranges_count *
sizeof(double))) == NULL)
+ if((device->ranges = (double *) realloc(device->ranges, data->ranges_count
* sizeof(double))) == NULL)
{
device->ranges_count = 0;
PLAYER_ERROR("Failed to allocate space to store range data");
@@ -117,11 +180,8 @@
{
if (device->intensities_count != data->intensities_count ||
device->intensities == NULL)
{
- // The number of data has changed, so delete any old data
- if(device->intensities != NULL)
- free(device->intensities);
// Allocate memory for the new data
- if((device->intensities = (double *) malloc(data->intensities_count *
sizeof(double))) == NULL)
+ if((device->intensities = (double *) realloc(device->intensities,
data->intensities_count * sizeof(double))) == NULL)
{
device->intensities_count = 0;
PLAYER_ERROR("Failed to allocate space to store intensity data");
@@ -189,11 +249,15 @@
if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype ==
PLAYER_RANGER_DATA_RANGE))
{
playerc_ranger_copy_range_data(device, (player_ranger_data_range_t *)
data);
+ playerc_ranger_calculate_bearings(device);
+ playerc_ranger_calculate_points(device);
}
else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype ==
PLAYER_RANGER_DATA_RANGEPOSE))
{
playerc_ranger_copy_range_data(device, &((player_ranger_data_rangepose_t
*) data)->data);
playerc_ranger_copy_geom(device, &((player_ranger_data_rangepose_t *)
data)->geom);
+ playerc_ranger_calculate_bearings(device);
+ playerc_ranger_calculate_points(device);
}
else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype ==
PLAYER_RANGER_DATA_INTNS))
{
Modified:
code/player/branches/release-2-1-patches/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/branches/release-2-1-patches/client_libs/libplayerc/playerc.h
2008-07-10 01:34:50 UTC (rev 6833)
+++ code/player/branches/release-2-1-patches/client_libs/libplayerc/playerc.h
2008-07-10 02:09:02 UTC (rev 6834)
@@ -2980,6 +2980,16 @@
properties include intns_on for laser devices and volt_on for IR devices. */
double *intensities;
+ /** Number of individual scan bearings. */
+ uint32_t bearings_count;
+ /** Scan bearings [radians]. */
+ double *bearings;
+
+ /** Number of scan points. */
+ uint32_t points_count;
+ /** Scan points (x, y, z). */
+ player_point_3d_t *points;
+
} playerc_ranger_t;
/** @brief Create a ranger proxy. */
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Sponsored by: SourceForge.net Community Choice Awards: VOTE NOW!
Studies have shown that voting for your favorite open source project,
along with a healthy diet, reduces your potential for chronic lameness
and boredom. Vote Now at http://www.sourceforge.net/community/cca08
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
