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