Revision: 8838
http://playerstage.svn.sourceforge.net/playerstage/?rev=8838&view=rev
Author: jpgr87
Date: 2010-07-29 01:02:07 +0000 (Thu, 29 Jul 2010)
Log Message:
-----------
Applied patch #3035844: Add basic intensity reporting for sicks3000 driver
Modified Paths:
--------------
code/player/trunk/server/drivers/laser/sicks3000.cc
Modified: code/player/trunk/server/drivers/laser/sicks3000.cc
===================================================================
--- code/player/trunk/server/drivers/laser/sicks3000.cc 2010-07-29 00:58:47 UTC
(rev 8837)
+++ code/player/trunk/server/drivers/laser/sicks3000.cc 2010-07-29 01:02:07 UTC
(rev 8838)
@@ -170,6 +170,10 @@
IntProperty mirror;
+ // Is the laser upside-down? (if so, we'll reverse the ordering of the
+ // readings)
+ int invert;
+
bool recognisedScanner;
// Opaque Driver info
@@ -246,6 +250,8 @@
this->size[0] = 0.15;
this->size[1] = 0.15;
+ this->invert = cf->ReadInt(section, "invert", 0);
+
this->opaque = NULL;
// Must have an opaque device
if (cf->ReadDeviceAddr(&this->opaque_id, section, "requires",
@@ -480,15 +486,26 @@
SetScannerParams(data_count); // Set up parameters based on number
of results.
data_packet.ranges_count = data_count;
data_packet.ranges = new float [data_count];
+ data_packet.intensity_count = data_count;
+ data_packet.intensity = new uint8_t [data_count];
for (int ii = 0; ii < data_count; ++ii)
{
unsigned short Distance_CM = (*reinterpret_cast<unsigned short *>
(&data[4 + 2*ii]));
Distance_CM &= 0x1fff; // remove status bits
double distance_m = static_cast<double>(Distance_CM)/100.0;
- if (mirror == 1)
+ if (mirror == 1 || this->invert) {
data_packet.ranges[data_count - ii - 1] = static_cast<float>
(distance_m); // Reverse order.
- else
+ if (data[(4 + 2*ii)+1] & 32)
+ data_packet.intensity[data_count - ii - 1] = 0xff; //
Reflex found
+ else
+ data_packet.intensity[data_count - ii - 1] = 0x00; //
Reflex not found
+ } else {
data_packet.ranges[ii] = static_cast<float> (distance_m);
+ if (data[(4 + 2*ii)+1] & 32)
+ data_packet.intensity[ii] = 0xff; // Reflex found
+ else
+ data_packet.intensity[ii] = 0x00; // Reflex not found
+ }
}
this->Publish(this->device_addr,
@@ -496,6 +513,7 @@
PLAYER_LASER_DATA_SCAN,
(void*)&data_packet);
delete [] data_packet.ranges;
+ delete [] data_packet.intensity;
}
else if (data[0] == 0xCC)
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
The Palm PDK Hot Apps Program offers developers who use the
Plug-In Development Kit to bring their C/C++ apps to Palm for a share
of $1 Million in cash or HP Products. Visit us here for more details:
http://p.sf.net/sfu/dev2dev-palm
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
