Revision: 9009
http://playerstage.svn.sourceforge.net/playerstage/?rev=9009&view=rev
Author: jpgr87
Date: 2010-12-13 21:01:32 +0000 (Mon, 13 Dec 2010)
Log Message:
-----------
Rangertolaser now publishes range and intensity measurements in the same
messages
Modified Paths:
--------------
code/player/trunk/server/drivers/ranger/rangertolaser.cc
Modified: code/player/trunk/server/drivers/ranger/rangertolaser.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/rangertolaser.cc 2010-12-12
20:15:49 UTC (rev 9008)
+++ code/player/trunk/server/drivers/ranger/rangertolaser.cc 2010-12-13
21:01:32 UTC (rev 9009)
@@ -112,7 +112,10 @@
bool receivedCfgResp, receivedIntnsResp, setConfigFailed;
player_laser_config_t config;
+ player_laser_data_scanpose_t scanData;
+
bool startup;
+
};
// Initialisation function
@@ -171,12 +174,27 @@
// Request the ranger device's configuration
inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL);
+
+ memset (&scanData, 0, sizeof (scanData));
+ scanData.scan.ranges = NULL;
+ scanData.scan.intensity = NULL;
+
return 0;
}
// Shutdown function
int RangerToLaser::Shutdown (void)
{
+ if (scanData.scan.ranges)
+ {
+ delete[] scanData.scan.ranges;
+ scanData.scan.ranges = NULL;
+ }
+ if (scanData.scan.intensity)
+ {
+ delete[] scanData.scan.intensity;
+ scanData.scan.intensity = NULL;
+ }
// Unsubscribe from the ranger device
inputDevice->Unsubscribe (InQueue);
@@ -190,13 +208,10 @@
int RangerToLaser::ConvertData (player_msghdr *hdr, void *data)
{
- player_laser_data_scanpose_t scanData;
double *rangeData = NULL, *intensityData = NULL;
int rangeCount = 0, intensityCount = 0;
player_ranger_geom_t *pose = NULL;
- memset (&scanData, 0, sizeof (scanData));
-
switch (hdr->subtype)
{
case PLAYER_RANGER_DATA_RANGE:
@@ -237,6 +252,11 @@
// Copy the data into the laser message format
if (rangeData != NULL)
{
+ if (scanData.scan.ranges != NULL)
+ {
+ delete[] scanData.scan.ranges;
+ scanData.scan.ranges = NULL;
+ }
scanData.scan.ranges_count = rangeCount;
if ((scanData.scan.ranges = new float[rangeCount]) == NULL)
{
@@ -248,6 +268,11 @@
}
if (intensityData != NULL)
{
+ if (scanData.scan.intensity != NULL)
+ {
+ delete[] scanData.scan.intensity;
+ scanData.scan.intensity = NULL;
+ }
scanData.scan.intensity_count = intensityCount;
if ((scanData.scan.intensity = new uint8_t[intensityCount]) ==
NULL)
{
@@ -275,13 +300,10 @@
if (pose != NULL)
Publish (device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCANPOSE, reinterpret_cast<void*> (&scanData), sizeof
(scanData), NULL);
else
+ {
Publish (device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCAN, reinterpret_cast<void*> (&scanData.scan), sizeof
(scanData.scan), NULL);
+ }
- if (scanData.scan.ranges != NULL)
- delete[] scanData.scan.ranges;
- if (scanData.scan.intensity != NULL)
- delete[] scanData.scan.intensity;
-
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Lotusphere 2011
Register now for Lotusphere 2011 and learn how
to connect the dots, take your collaborative environment
to the next level, and enter the era of Social Business.
http://p.sf.net/sfu/lotusphere-d2d
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
