Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[442] gearbox/trunk
|
From: <gb...@us...> - 2009-09-04 08:50:23
|
Revision: 442
http://gearbox.svn.sourceforge.net/gearbox/?rev=442&view=rev
Author: gbiggs
Date: 2009-09-04 08:50:14 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Fixed bugs in reading intensity data. Added -i option to example.
Modified Paths:
--------------
gearbox/trunk/doc/history.dox
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/test/example.cpp
Modified: gearbox/trunk/doc/history.dox
===================================================================
--- gearbox/trunk/doc/history.dox 2009-09-02 08:27:54 UTC (rev 441)
+++ gearbox/trunk/doc/history.dox 2009-09-04 08:50:14 UTC (rev 442)
@@ -35,6 +35,10 @@
- Bugfix: matched GPS status/solution enums to Novatel's internal types, there were gaps due to reserved values before.
Note: this will create mismatches with old log-files (not data, status/solution-type only)! (MichaelM, patch by Ian Mahon)
+- libhokuyo_aist
+ - Bugfix: Errors in the way intensity data is retrieved have been fixed. (gbiggs, bug report by martimorta)
+ - New feature: The example now has an option, -i, to get intensity data.
+
@section gbx_doc_history_907 Changes in Release 9.07
@par Updated libraries
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2009-09-02 08:27:54 UTC (rev 441)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2009-09-04 08:50:14 UTC (rev 442)
@@ -2467,7 +2467,7 @@
if (currentStep != numSteps)
{
throw HokuyoError (HOKUYO_ERR_PROTOCOL,
- "Read a different number of range readings than were asked for.");
+ "Read a different number of range readings than were asked for.");
}
}
@@ -2485,7 +2485,7 @@
// 3 byte data is a pain because it crosses the line boundary, it may overlap by 0, 1 or 2 bytes
char buffer[SCIP2_LINE_LENGTH];
- unsigned int currentStep = 0;
+ unsigned int currentRange = 0, currentIntensity = 0;
int numBytesInLine = 0, splitCount = 0;
char splitValue[3];
bool nextIsIntensity = false;
@@ -2523,40 +2523,43 @@
{
splitValue[2] = buffer[ii++];
if (nextIsIntensity)
- data->_intensities[currentStep] = Decode3ByteValue (splitValue);
+ data->_intensities[currentIntensity] = Decode3ByteValue (splitValue);
else
- data->_ranges[currentStep] = Decode3ByteValue (splitValue);
+ data->_ranges[currentRange] = Decode3ByteValue (splitValue);
}
else if (splitCount == 2)
{
splitValue[1] = buffer[ii++];
splitValue[2] = buffer[ii++];
if (nextIsIntensity)
- data->_intensities[currentStep] = Decode3ByteValue (splitValue);
+ data->_intensities[currentIntensity] = Decode3ByteValue (splitValue);
else
- data->_ranges[currentStep] = Decode3ByteValue (splitValue);
+ data->_ranges[currentRange] = Decode3ByteValue (splitValue);
}
else
{
if (nextIsIntensity)
- data->_intensities[currentStep] = Decode3ByteValue (&buffer[ii]);
+ data->_intensities[currentIntensity] = Decode3ByteValue (&buffer[ii]);
else
- data->_ranges[currentStep] = Decode3ByteValue (&buffer[ii]);
+ data->_ranges[currentRange] = Decode3ByteValue (&buffer[ii]);
ii += 3;
}
- if (data->_ranges[currentStep] > _maxRange && !nextIsIntensity)
+ if (data->_ranges[currentRange] > _maxRange && !nextIsIntensity)
{
cerr << "WARNING: HokuyoLaser::" << __func__ <<
- "() Value at step " << currentStep << " beyond maximum range: " <<
- data->_ranges[currentStep] << " (raw bytes: ";
+ "() Value at step " << currentRange << " beyond maximum range: " <<
+ data->_ranges[currentRange] << " (raw bytes: ";
if (splitCount != 0)
cerr << splitValue[0] << splitValue[1] << splitValue[2] << ")" << endl;
else
cerr << buffer[0] << buffer[1] << buffer[2] << ")" << endl;
}
- else if (data->_ranges[currentStep] < 20)
+ else if (data->_ranges[currentRange] < 20)
data->_error = true;
- currentStep++;
+ if (nextIsIntensity)
+ currentIntensity++;
+ else
+ currentRange++;
splitCount = 0; // Reset this here now that it's been used
nextIsIntensity = !nextIsIntensity; // Alternate between range and intensity values
}
@@ -2566,13 +2569,13 @@
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Read " << currentStep <<
- " ranges and intensities." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Read " << currentRange << " ranges and "
+ << currentIntensity << " intensities (expected " << numSteps << ")." << endl;
}
- if (currentStep != numSteps)
+ if (currentRange != numSteps || currentIntensity != numSteps)
{
throw HokuyoError (HOKUYO_ERR_PROTOCOL,
- "Read a different number of range and intensity readings than were asked for.");
+ "Read a different number of range or intensity readings than were asked for.");
}
}
Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp 2009-09-02 08:27:54 UTC (rev 441)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp 2009-09-04 08:50:14 UTC (rev 442)
@@ -38,14 +38,14 @@
double startAngle = 0.0, endAngle = 0.0;
int firstStep = -1, lastStep = -1;
unsigned int baud = 19200, speed = 0, clusterCount = 1;
- bool getNew = false, verbose = false;
+ bool getIntensities = false, getNew = false, verbose = false;
#if defined (WIN32)
portOptions = "type=serial,device=COM3,timeout=1";
#else
int opt;
// Get some options from the command line
- while ((opt = getopt(argc, argv, "b:c:e:f:l:m:no:s:vh")) != -1)
+ while ((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:vh")) != -1)
{
switch (opt)
{
@@ -61,6 +61,9 @@
case 'f':
sscanf (optarg, "%d", &firstStep);
break;
+ case 'i':
+ getIntensities = true;
+ break;
case 'l':
sscanf (optarg, "%d", &lastStep);
break;
@@ -87,6 +90,7 @@
cout << "-c count\tCluster count." << endl;
cout << "-e angle\tEnd angle to get ranges to." << endl;
cout << "-f step\t\tFirst step to get ranges from." << endl;
+ cout << "-i\t\tGet intensity data along with ranges." << endl;
cout << "-l step\t\tLast step to get ranges to." << endl;
cout << "-m speed\tMotor speed." << endl;
cout << "-n\t\tGet new ranges instead of latest ranges." << endl;
@@ -142,6 +146,8 @@
// Get all ranges
if (getNew)
laser.GetNewRanges (&data, -1, -1, clusterCount);
+ else if (getIntensities)
+ laser.GetNewRangesAndIntensities (&data, -1, -1, clusterCount);
else
laser.GetRanges (&data, -1, -1, clusterCount);
}
@@ -150,6 +156,8 @@
// Get by step
if (getNew)
laser.GetNewRanges (&data, firstStep, lastStep, clusterCount);
+ else if (getIntensities)
+ laser.GetNewRangesAndIntensities (&data, firstStep, lastStep, clusterCount);
else
laser.GetRanges (&data, firstStep, lastStep, clusterCount);
}
@@ -158,6 +166,8 @@
// Get by angle
if (getNew)
laser.GetNewRangesByAngle (&data, startAngle, endAngle, clusterCount);
+ else if (getIntensities)
+ laser.GetNewRangesAndIntensitiesByAngle (&data, startAngle, endAngle, clusterCount);
else
laser.GetRangesByAngle (&data, startAngle, endAngle, clusterCount);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|