[Playerstage-commit] SF.net SVN: playerstage:[6911] code/player/trunk

[<th...@us...>]

Revision: 6911
          http://playerstage.svn.sourceforge.net/playerstage/?rev=6911&view=rev
Author:   thjc
Date:     2008-07-23 06:34:31 +0000 (Wed, 23 Jul 2008)

Log Message:
-----------
Merged 6834 from 2-1
Added scan point calculation to the ranger device

Modified Paths:
--------------
    code/player/trunk/client_libs/libplayerc/dev_ranger.c
    code/player/trunk/client_libs/libplayerc/playerc.h

Property Changed:
----------------
    code/player/trunk/


Property changes on: code/player/trunk
___________________________________________________________________
Modified: svn:mergeinfo
   - /code/player/branches/release-2-1-patches:6672-6673,6738
   + /code/player/branches/release-2-1-patches:6672-6673,6738,6834

Modified: code/player/trunk/client_libs/libplayerc/dev_ranger.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_ranger.c	2008-07-23 06:22:57 UTC (rev 6910)
+++ code/player/trunk/client_libs/libplayerc/dev_ranger.c	2008-07-23 06:34:31 UTC (rev 6911)
@@ -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/trunk/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/trunk/client_libs/libplayerc/playerc.h	2008-07-23 06:22:57 UTC (rev 6910)
+++ code/player/trunk/client_libs/libplayerc/playerc.h	2008-07-23 06:34:31 UTC (rev 6911)
@@ -2979,6 +2979,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.