Revision: 7089
http://playerstage.svn.sourceforge.net/playerstage/?rev=7089&view=rev
Author: veedee
Date: 2008-10-10 11:58:27 +0000 (Fri, 10 Oct 2008)
Log Message:
-----------
modified SR3k to be compliant with the new libusbSR (1.0.10) and added the
possibility to provide data through a stereo interface
Modified Paths:
--------------
code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc
Modified: code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2008-10-08
03:57:22 UTC (rev 7088)
+++ code/player/trunk/server/drivers/mixed/sr3000/sr3000.cc 2008-10-10
11:58:27 UTC (rev 7089)
@@ -30,8 +30,11 @@
The sr3000 driver controls the Swiss Ranger SR3000 camera. A broad range of
camera option parameters are supported, via the libusbSR library. The driver
provides a @ref interface_pointcloud3d interface and two @ref interface_camera
-interfaces for both distance and intensity images.
+interfaces for both distance and intensity images, *or* a @ref interface_stereo
+interface.
+This version of the driver works with libusbSR v1.0.10+
+
@par Compile-time dependencies
- none
@@ -41,7 +44,9 @@
- @ref interface_pointcloud3d : the 3d point cloud generated by the SR3000
- @ref interface_camera : snapshot images (both distance and intensity) taken
by
the SR3000
-
+- @ref interface_stereo : intensity and distance images as left and right
+ channels, and the 3d point cloud generated by the
SR3000
+
@par Requires
- none
@@ -63,10 +68,7 @@
10MHz -> 15.0 m,
6.6MHz -> 22.5 m,
5MHz -> 30.0 m
- - sat_threshold (integer): Saturation threshold.
- amp_threshold (integer): Amplification threshold.
- - static_delay (double): Temporal IIR static delay (0.0-1.0).
- - dynamic_delay (double): Temporal IIR dynamic delay (0.0-1.0).
@par Configuration file options
@@ -78,7 +80,12 @@
driver
(
name "sr3000"
+
provides ["pointcloud3d:0" "distance:::camera:0" "intensity:::camera:1"]
+
+ # OR ...
+
+ provides ["stereo:0"]
)
@endverbatim
@@ -88,9 +95,11 @@
#include <stdlib.h>
#include <libplayercore/playercore.h>
-#include "libusbSR.h"
+#include <libusbSR.h>
-#define MODE (AM_COR_FIX_PTRN | AM_COR_LED_NON_LIN | AM_MEDIAN)
+// Older library: #define MODE (AM_COR_FIX_PTRN | AM_COR_LED_NON_LIN |
AM_MEDIAN)
+//#define MODE (AM_MEDIAN | AM_COR_FIX_PTRN | AM_CONV_GRAY | AM_SW_ANF |
AM_SR3K_2TAP_PROC | AM_MEDIANCROSS)
+#define MODE (AM_MEDIAN | AM_COR_FIX_PTRN | AM_SW_ANF | AM_SR3K_2TAP_PROC |
AM_MEDIANCROSS)
#define CAM_ROWS 144
#define CAM_COLS 176
@@ -115,32 +124,31 @@
int ProcessMessageCamera (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data,
- player_devaddr_t cam_addr);
+ player_devaddr_t);
virtual void Main ();
void RefreshData ();
// device identifier
- SwissrangerCam* srCam;
+ CMesaDevice* srCam;
// SR3000 specific values
- unsigned int rows, cols, bpp, inr;
- //ModulationFrq modulation_freq;
- size_t buffer_size, buffer_points_size;
- uint8_t *buffer;
- float *buffer_points, *xp, *yp, *zp;
+ unsigned int rows, cols, inr;
+
+ ImgEntry* imgEntryArray;
+ float *buffer, *xp, *yp, *zp;
// device bookkeeping
- player_devaddr_t pcloud_addr;
- player_devaddr_t d_cam_addr, i_cam_addr;
+ player_devaddr_t stereo_addr, pcloud_addr, d_cam_addr, i_cam_addr;
+
player_pointcloud3d_data_t pcloud_data;
player_camera_data_t d_cam_data, i_cam_data;
- bool providePCloud, provideDCam, provideICam;
+ player_stereo_data_t stereo_data;
protected:
// Properties
- IntProperty auto_illumination, integration_time, modulation_freq,
sat_threshold, amp_threshold;
- DoubleProperty static_delay, dynamic_delay;
-
+ IntProperty auto_illumination, integration_time, modulation_freq,
amp_threshold;
+
+ bool providePCloud, provideDCam, provideICam, provideStereo;
};
////////////////////////////////////////////////////////////////////////////////
@@ -169,61 +177,70 @@
auto_illumination ("auto_illumination", 0, 0),
integration_time ("integration_time", 0, 0),
modulation_freq ("modulation_freq", 0, 0),
- sat_threshold ("sat_threshold", 0, 0),
- amp_threshold ("amp_threshold", 0, 0),
- static_delay ("static_delay", 0, 0),
- dynamic_delay ("dynamic_delay", 0, 0)
+ amp_threshold ("amp_threshold", 0, 0)
{
+ memset (&this->stereo_addr, 0, sizeof (player_devaddr_t));
memset (&this->pcloud_addr, 0, sizeof (player_devaddr_t));
memset (&this->d_cam_addr, 0, sizeof (player_devaddr_t));
memset (&this->i_cam_addr, 0, sizeof (player_devaddr_t));
-
+
this->RegisterProperty ("auto_illumination", &this->auto_illumination, cf,
section);
this->RegisterProperty ("integration_time", &this->integration_time, cf,
section);
this->RegisterProperty ("modulation_freq", &this->modulation_freq, cf,
section);
- this->RegisterProperty ("sat_threshold", &this->sat_threshold, cf, section);
this->RegisterProperty ("amp_threshold", &this->amp_threshold, cf, section);
- this->RegisterProperty ("static_delay", &this->static_delay, cf, section);
- this->RegisterProperty ("dynamic_delay", &this->dynamic_delay, cf, section);
- providePCloud = FALSE; provideDCam = FALSE; provideICam = FALSE;
+ providePCloud = FALSE; provideDCam = FALSE; provideICam = FALSE;
provideStereo = FALSE;
- // Outgoing pointcloud interface
- if (cf->ReadDeviceAddr (&(this->pcloud_addr), section, "provides",
- PLAYER_POINTCLOUD3D_CODE, -1, NULL) == 0)
+ if (cf->ReadDeviceAddr (&(this->stereo_addr), section, "provides",
+ PLAYER_STEREO_CODE, -1, NULL) == 0)
{
- if (this->AddInterface (this->pcloud_addr) != 0)
+ if (this->AddInterface (this->stereo_addr) != 0)
{
this->SetError (-1);
return;
}
- providePCloud = TRUE;
+ provideStereo = TRUE;
}
-
- // Outgoing distance::camera:0 interface
- if (cf->ReadDeviceAddr (&(this->d_cam_addr), section, "provides",
- PLAYER_CAMERA_CODE, -1, "distance") == 0)
+ else
{
- if (this->AddInterface (this->d_cam_addr) != 0)
- {
- this->SetError (-1);
- return;
- }
- provideDCam = TRUE;
- }
-
- // Outgoing intensity::camera:1 interface
- if (cf->ReadDeviceAddr (&(this->i_cam_addr), section, "provides",
- PLAYER_CAMERA_CODE, -1, "intensity") == 0)
- {
- if (this->AddInterface (this->i_cam_addr) != 0)
+ // Outgoing pointcloud interface
+ if (cf->ReadDeviceAddr (&(this->pcloud_addr), section, "provides",
+ PLAYER_POINTCLOUD3D_CODE, -1, NULL) == 0)
{
- this->SetError (-1);
- return;
+ if (this->AddInterface (this->pcloud_addr) != 0)
+ {
+ this->SetError (-1);
+ return;
+ }
+ providePCloud = TRUE;
}
- provideICam = TRUE;
+
+ // Outgoing distance::camera:0 interface
+ if (cf->ReadDeviceAddr (&(this->d_cam_addr), section, "provides",
+ PLAYER_CAMERA_CODE, -1, "distance") == 0)
+ {
+ if (this->AddInterface (this->d_cam_addr) != 0)
+ {
+ this->SetError (-1);
+ return;
+ }
+ provideDCam = TRUE;
+ }
+
+ // Outgoing intensity::camera:1 interface
+ if (cf->ReadDeviceAddr (&(this->i_cam_addr), section, "provides",
+ PLAYER_CAMERA_CODE, -1, "intensity") == 0)
+ {
+ if (this->AddInterface (this->i_cam_addr) != 0)
+ {
+ this->SetError (-1);
+ return;
+ }
+ provideICam = TRUE;
+ }
+
+ provideStereo = FALSE;
}
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -239,38 +256,34 @@
{
int res;
// ---[ Open the camera ]---
- res = SR_Open (&srCam); //returns the device ID used in other calls
+ res = SR_OpenUSB (&srCam, 0); //returns the device ID used in other
calls
PLAYER_MSG0 (1, "> Connecting to SR3000... [done]");
- // ---[ Get the number of rows, cols, bpp, ... ]---
+ // ---[ Get the number of rows, cols, ... ]---
rows = SR_GetRows (srCam);
cols = SR_GetCols (srCam);
- bpp = SR_GetBytePerPix (srCam);
- inr = SR_GetNumImg (srCam);
+ inr = SR_GetImageList (srCam, &imgEntryArray);
modulation_freq = SR_GetModulationFrequency (srCam);
integration_time = SR_GetIntegrationTime (srCam);
- buffer_size = SR_GetBufferSize (srCam);
- PLAYER_MSG5 (2, ">> Expecting %dx%dx%dx%d (%d bytes)", cols, rows, bpp, inr,
buffer_size);
+ PLAYER_MSG3 (2, ">> Expecting %dx%dx%d", cols, rows, inr);
- if ((cols != CAM_COLS) || (rows != CAM_ROWS) || ((unsigned int)buffer_size <
0))
+ if ( (cols != CAM_COLS) || (rows != CAM_ROWS) || (inr < 1) || (imgEntryArray
== 0) )
{
PLAYER_ERROR ("> Error while connecting to camera!");
SR_Close (srCam);
return (-1);
}
- // ---[ Alloc memory for the buffer ]---
- buffer = (uint8_t*)malloc (buffer_size);
- memset (buffer, 0, buffer_size);
-
- // ---[ Set the buffer ]---
- SR_SetBuffer (srCam, buffer, buffer_size);
-
- buffer_points_size = rows * cols * 3 * sizeof (float);
- buffer_points = (float*)malloc (buffer_points_size);
- memset (buffer_points, 0, buffer_points_size);
- xp = buffer_points;
+ // ---[ Set the acquisition mode ]---
+ SR_SetMode (srCam, MODE);
+
+ // Points array
+ size_t buffer_size = rows * cols * 3 * sizeof (float);
+ buffer = (float*)malloc (buffer_size);
+ memset (buffer, 0xaf, buffer_size);
+
+ xp = buffer;
yp = &xp[rows*cols];
zp = &yp[rows*cols];
@@ -293,7 +306,6 @@
// ---[ Free the allocated memory buffer ]---
free (buffer);
- free (buffer_points);
return (0);
}
@@ -304,17 +316,14 @@
SR3000::ProcessMessageCamera (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data,
- player_devaddr_t cam_addr)
+ player_devaddr_t stereo_addr)
{
int res;
- //Property *property = NULL;
// Check for properties
- if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ,
cam_addr))
+ if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ,
stereo_addr))
{
player_intprop_req_t req = *reinterpret_cast<player_intprop_req_t*> (data);
-// if ((property = propertyBag.GetProperty (req.key)) == NULL)
-// return (-1);
if (auto_illumination.KeyIsEqual (req.key))
{
// ---[ Set Autoillumination
@@ -327,11 +336,11 @@
if (res == 0)
{
auto_illumination = req.value;
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
else
{
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
return (0);
}
@@ -343,13 +352,12 @@
// Check the error code
if (res == 0)
{
- //property->SetValueFromMessage (reinterpret_cast<void*> (&req));
integration_time = req.value;
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
else
{
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
return (0);
}
@@ -361,35 +369,15 @@
// Check the error code
if (res == 0)
{
- //property->SetValueFromMessage (reinterpret_cast<void*> (&req));
modulation_freq = req.value;
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
else
{
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
return (0);
}
- else if (sat_threshold.KeyIsEqual (req.key))
- {
- // ---[ Set saturation threshold
- res = SR_SetSaturationThreshold (srCam, req.value);
-
- // Check the error code
- if (res == 0)
- {
- //property->SetValueFromMessage (reinterpret_cast<void*> (&req));
- sat_threshold = req.value;
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
-
- }
- else
- {
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
- }
- return (0);
- }
else if (amp_threshold.KeyIsEqual (req.key))
{
// ---[ Set amplitude threshold
@@ -398,29 +386,26 @@
// Check the error code
if (res == 0)
{
- //property->SetValueFromMessage (reinterpret_cast<void*> (&req));
amp_threshold = req.value;
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
else
{
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ Publish (stereo_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
}
return (0);
}
return (-1); // Let the default property handling handle it
}
- else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_GET_INTPROP_REQ, device_addr))
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_GET_INTPROP_REQ, device_addr))
{
player_intprop_req_t req = *reinterpret_cast<player_intprop_req_t*> (data);
-/* if ((property = propertyBag.GetProperty (req.key)) == NULL)
- return (-1);*/
if (modulation_freq.KeyIsEqual (req.key))
{
// ---[ Get modulation frequency
modulation_freq.SetValue (SR_GetModulationFrequency (srCam));
modulation_freq.GetValueToMessage (reinterpret_cast<void*> (&req));
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req),
sizeof(player_intprop_req_t), NULL);
+ Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req),
sizeof(player_intprop_req_t), NULL);
return (0);
}
else if (integration_time.KeyIsEqual (req.key))
@@ -428,55 +413,12 @@
// ---[ Get integration time
integration_time.SetValue (SR_GetIntegrationTime (srCam));
integration_time.GetValueToMessage (reinterpret_cast<void*> (&req));
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req),
sizeof(player_intprop_req_t), NULL);
+ Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_GET_INTPROP_REQ, reinterpret_cast<void*> (&req),
sizeof(player_intprop_req_t), NULL);
return (0);
}
return (-1); // Let the default property handling handle it
}
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SET_DBLPROP_REQ, cam_addr))
- {
- player_dblprop_req_t req = *reinterpret_cast<player_dblprop_req_t*> (data);
-/* if ((property = propertyBag.GetProperty (req.key)) == NULL)
- return (-1);*/
- if (static_delay.KeyIsEqual (req.key))
- {
- // ---[ Set IIR static delay
- //DoubleProperty *dynamic_delay = propertyBag.GetProperty
("dynamic_delay");
- res = SR_SetTemporalIIR (srCam, req.value, dynamic_delay);
- // Check the error code
- if (res == 0)
- {
- static_delay.SetValueFromMessage (reinterpret_cast<void*> (&req));
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
- }
- else
- {
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
- }
- return (0);
- }
- else if (dynamic_delay.KeyIsEqual (req.key))
- {
- // ---[ Set IIR dynamic delay
- //DoubleProperty *static_delay = propertyBag.GetProperty
("static_delay");
- res = SR_SetTemporalIIR (srCam, static_delay, req.value);
-
- // Check the error code
- if (res == 0)
- {
- dynamic_delay.SetValueFromMessage (reinterpret_cast<void*> (&req));
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
- }
- else
- {
- Publish(cam_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
- }
- return (0);
- }
- return (-1); // Let the default property handling handle it
- }
-
return (0);
}
@@ -489,8 +431,13 @@
assert (hdr);
assert (data);
- ProcessMessageCamera (resp_queue, hdr, data, d_cam_addr);
- ProcessMessageCamera (resp_queue, hdr, data, i_cam_addr);
+ if (provideStereo)
+ ProcessMessageCamera (resp_queue, hdr, data, stereo_addr);
+ else
+ {
+ ProcessMessageCamera (resp_queue, hdr, data, d_cam_addr);
+ ProcessMessageCamera (resp_queue, hdr, data, i_cam_addr);
+ }
return (0);
}
@@ -502,6 +449,7 @@
{
timespec sleepTime = {0, 10000};
+ memset (&stereo_data, 0, sizeof (stereo_data));
memset (&pcloud_data, 0, sizeof (pcloud_data));
memset (&d_cam_data, 0, sizeof (d_cam_data ));
memset (&i_cam_data, 0, sizeof (i_cam_data ));
@@ -526,39 +474,33 @@
int res;
unsigned int i;
- res = SR_Acquire (srCam, MODE);
+ res = SR_Acquire (srCam);
- // Publish pointcloud3d data if subscribed
- if (providePCloud)
+ uint8_t *distance_image = (unsigned char*)imgEntryArray->data;
+ uint8_t *intensity_image = (unsigned char*)imgEntryArray->data +
(imgEntryArray->width * imgEntryArray->height * 2);
+// buffer_size/2;
+
+ // Points array
+ res = SR_CoordTrfFlt (srCam, xp, yp, zp, sizeof (float), sizeof (float),
sizeof (float));
+
+ if (provideStereo)
{
- res = SR_CoordTrfFlt (srCam, xp, yp, zp, sizeof (float), sizeof (float),
sizeof (float));
-
- pcloud_data.points_count = rows * cols;
- pcloud_data.points = new
player_pointcloud3d_element_t[pcloud_data.points_count];
+ stereo_data.points_count = rows * cols;
+ stereo_data.points = new
player_pointcloud3d_stereo_element_t[stereo_data.points_count];
for (i = 0; i < rows*cols; i++)
{
- player_pointcloud3d_element_t element;
- element.point.px = xp[i];
- element.point.py = yp[i];
- element.point.pz = zp[i];
+ player_pointcloud3d_stereo_element_t element;
+ element.px = xp[i];
+ element.py = yp[i];
+ element.pz = zp[i];
- element.color.alpha = 255;
- element.color.red = 255;
- element.color.green = 255;
- element.color.blue = 255;
- pcloud_data.points[i] = element;
+ element.red = intensity_image[i*2 + 1];
+ element.green = intensity_image[i*2 + 1];
+ element.blue = intensity_image[i*2 + 1];
+ stereo_data.points[i] = element;
}
- // Write the Pointcloud3d data
- Publish (pcloud_addr, PLAYER_MSGTYPE_DATA, PLAYER_POINTCLOUD3D_DATA_STATE,
- &pcloud_data, 4 + pcloud_data.points_count*sizeof
(player_pointcloud3d_element_t),
- NULL);
- delete [] pcloud_data.points;
- }
-
- // Publish distance camera data if subscribed
- if (provideDCam)
- {
+ // Prepare distance camera data
d_cam_data.width = cols;
d_cam_data.height = rows;
d_cam_data.bpp = 16;
@@ -566,17 +508,11 @@
d_cam_data.fdiv = 1;
d_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
d_cam_data.image_count = rows*cols*2;
- d_cam_data.image = buffer;
- //memcpy (d_cam_data.image, (unsigned char*)buffer, rows*cols*2);
+ d_cam_data.image = distance_image;
- // Write the distance camera data
- Publish (d_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
- &d_cam_data, 28+(d_cam_data.image_count), NULL);
- }
+ stereo_data.left_channel = d_cam_data;
- // Publish intensity camera data if subscribed
- if (provideICam)
- {
+ // Prepare intensity camera data
i_cam_data.width = cols;
i_cam_data.height = rows;
i_cam_data.bpp = 16;
@@ -584,13 +520,81 @@
i_cam_data.fdiv = 1;
i_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
i_cam_data.image_count = rows*cols*2;
- i_cam_data.image = (unsigned char*)buffer + buffer_size/2;
- //memcpy (i_cam_data.image, (unsigned char*)buffer + buffer_size/2,
rows*cols*2);
+ i_cam_data.image = intensity_image;
- // Write the intensity camera data
- Publish (i_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
- &i_cam_data, 28+(i_cam_data.image_count), NULL);
+ stereo_data.right_channel = i_cam_data;
+
+ // Publish the stereo data
+ // int total_size = 28+(i_cam_data.image_count) +
+ // 28+(d_cam_data.image_count) +
+ // 4 + pcloud_data.points_count*sizeof
(player_pointcloud3d_element_t);
+
+ Publish (stereo_addr, PLAYER_MSGTYPE_DATA, PLAYER_STEREO_DATA_STATE,
&stereo_data); //, total_size, NULL);
+
+ delete [] stereo_data.points;
}
+ else
+ {
+ // Publish pointcloud3d data if subscribed
+ if (providePCloud)
+ {
+ pcloud_data.points_count = rows * cols;
+ pcloud_data.points = new
player_pointcloud3d_element_t[pcloud_data.points_count];
+ for (i = 0; i < rows*cols; i++)
+ {
+ player_pointcloud3d_element_t element;
+ element.point.px = xp[i];
+ element.point.py = yp[i];
+ element.point.pz = zp[i];
+
+ element.color.alpha = 255;
+ element.color.red = intensity_image[i*2 + 1];
+ element.color.green = intensity_image[i*2 + 1];
+ element.color.blue = intensity_image[i*2 + 1];
+ pcloud_data.points[i] = element;
+ }
+
+ // Write the Pointcloud3d data
+ Publish (pcloud_addr, PLAYER_MSGTYPE_DATA,
PLAYER_POINTCLOUD3D_DATA_STATE,
+ &pcloud_data); //, 4 + pcloud_data.points_count*sizeof
(player_pointcloud3d_element_t), NULL);
+ delete [] pcloud_data.points;
+ }
+
+ // Publish distance camera data if subscribed
+ if (provideDCam)
+ {
+ d_cam_data.width = cols;
+ d_cam_data.height = rows;
+ d_cam_data.bpp = 16;
+ d_cam_data.format = PLAYER_CAMERA_FORMAT_MONO16;
+ d_cam_data.fdiv = 1;
+ d_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
+ d_cam_data.image_count = rows*cols*2;
+ d_cam_data.image = distance_image;
+
+ // Write the distance camera data
+ Publish (d_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
+ &d_cam_data); //, 28+(d_cam_data.image_count), NULL);
+ }
+
+ // Publish intensity camera data if subscribed
+ if (provideICam)
+ {
+ i_cam_data.width = cols;
+ i_cam_data.height = rows;
+ i_cam_data.bpp = 16;
+ i_cam_data.format = PLAYER_CAMERA_FORMAT_MONO16;
+ i_cam_data.fdiv = 1;
+ i_cam_data.compression = PLAYER_CAMERA_COMPRESS_RAW;
+ i_cam_data.image_count = rows*cols*2;
+ i_cam_data.image = intensity_image;
+
+ // Write the intensity camera data
+ Publish (i_cam_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
+ &i_cam_data); //, 28+(i_cam_data.image_count), NULL);
+ }
+ } // if (Stereo) ... else
+
return;
}
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 the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
