Revision: 507
http://gearbox.svn.sourceforge.net/gearbox/?rev=507&view=rev
Author: gbiggs
Date: 2010-08-05 01:26:43 +0000 (Thu, 05 Aug 2010)
Log Message:
-----------
Support new multi-echo detection commands.
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
gearbox/trunk/src/hokuyo_aist/scan_data.cpp
gearbox/trunk/src/hokuyo_aist/scan_data.h
gearbox/trunk/src/hokuyo_aist/sensor.cpp
gearbox/trunk/src/hokuyo_aist/sensor.h
gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
gearbox/trunk/src/hokuyo_aist/sensor_info.h
gearbox/trunk/src/hokuyo_aist/test/example.cpp
gearbox/trunk/src/hokuyo_aist/test/example.readme
gearbox/trunk/src/hokuyo_aist/utils.h
Added Paths:
-----------
gearbox/trunk/src/hokuyo_aist/test/example_urg_04lx.logr
gearbox/trunk/src/hokuyo_aist/test/example_urg_04lx.logw
gearbox/trunk/src/hokuyo_aist/test/example_utm_30lx.logr
gearbox/trunk/src/hokuyo_aist/test/example_utm_30lx.logw
Removed Paths:
-------------
gearbox/trunk/src/hokuyo_aist/test/example.logr
gearbox/trunk/src/hokuyo_aist/test/example.logw
Modified: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,15 +1,15 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo URG laser scanner driver.
*
- * This distribution is licensed to you under the terms described in the LICENSE file included in
- * this distribution.
+ * This distribution is licensed to you under the terms described in the
+ * LICENSE file included in this distribution.
*
- * This work is a product of the National Institute of Advanced Industrial Science and Technology,
- * Japan. Registration number: H22PRO-1086.
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1086.
*
* This file is part of hokuyo_aist.
*
@@ -17,71 +17,73 @@
* http://www.opensource.org/licenses/eclipse-1.0.txt
*/
-#include <stdio.h>
-#include <math.h>
+#include <cstdio>
+#include <cmath>
#include <iostream>
-using namespace std;
#include <hokuyo_aist/hokuyo_aist.h>
#include <hokuyo_aist/hokuyo_errors.h>
int main(int argc, char **argv)
{
- string portOptions = "type=serial,device=/dev/ttyACM0,timeout=1";
- bool verbose = false;
+ std::string port_options="type=serial,device=/dev/ttyACM0,timeout=1";
+ bool verbose=false;
#if defined (WIN32)
- portOptions = "type=serial,device=COM3,timeout=1";
+ port_options = "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:il:m:no:s:vh")) != -1)
- {
- switch (opt)
- {
- case 'o':
- portOptions = optarg;
- break;
- case 'v':
- verbose = true;
- break;
- case '?':
- case 'h':
- default:
- cout << "Usage: " << argv[0] << " [options]" << endl << endl;
- cout << "-o options\tPort options (see flexiport library)." << endl;
- cout << "-v\t\tPut the hokuyo_aist library into verbose mode." << endl;
- return 1;
- }
- }
+ int opt;
+ // Get some options from the command line
+ while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:vh")) != -1)
+ {
+ switch(opt)
+ {
+ case 'o':
+ port_options = optarg;
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ case '?':
+ case 'h':
+ default:
+ std::cout << "Usage: " << argv[0] << " [options]\n\n";
+ std::cout <<
+ "-o options\tPort options (see flexiport library).\n";
+ std::cout <<
+ "-v\t\tPut the hokuyo_aist library into verbose mode.\n";
+ return 1;
+ }
+ }
#endif // defined (WIN32)
- try
- {
- hokuyo_aist::Sensor laser; // Laser scanner object
- // Set the laser to verbose mode (so we see more information in the console)
- if (verbose)
- laser.set_verbose (true);
+ try
+ {
+ hokuyo_aist::Sensor laser; // Laser scanner object
+ // Set the laser to verbose mode (so we see more information in the
+ // console)
+ if(verbose)
+ laser.set_verbose (true);
- // Open the laser
- laser.open (portOptions);
- // Turn the laser on
- laser.set_power (true);
+ // Open the laser
+ laser.open (port_options);
+ // Turn the laser on
+ laser.set_power (true);
- // Get some laser info
- //cout << "Laser sensor information:" << endl;
- hokuyo_aist::SensorInfo info;
- laser.get_sensor_info (&info);
- cout << info.serial << endl;
+ // Get some laser info
+ hokuyo_aist::SensorInfo info;
+ laser.get_sensor_info (&info);
+ std::cout << info.serial << '\n';
- // Close the laser
- laser.close ();
- }
- catch (hokuyo_aist::BaseError &e)
- {
- cerr << "Caught exception: " << e.what () << endl;
- return 1;
- }
+ // Close the laser
+ laser.close ();
+ }
+ catch(hokuyo_aist::BaseError &e)
+ {
+ std::cerr << "Caught exception: " << e.what () << '\n';
+ return 1;
+ }
- return 0;
+ return 0;
}
+
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -32,36 +32,36 @@
std::stringstream ss;
// Check for non-errors
- if (error[0] == '0' && error[1] == '0')
+ if(error[0] == '0' && error[1] == '0')
return "Status OK - 00";
- else if (error[0] == '9' && error[1] == '9')
+ else if(error[0] == '9' && error[1] == '9')
return "Status OK - 99";
// Check for universal errors
- if (error[0] == '0' && error[1] == 'A')
+ if(error[0] == '0' && error[1] == 'A')
return "Unable to create transmission data or reply command internally";
- else if (error[0] == '0' && error[1] == 'B')
+ else if(error[0] == '0' && error[1] == 'B')
return "Buffer shortage or command repeated that is already processed";
- else if (error[0] == '0' && error[1] == 'C')
+ else if(error[0] == '0' && error[1] == 'C')
return "Command with insufficient parameters 1";
- else if (error[0] == '0' && error[1] == 'D')
+ else if(error[0] == '0' && error[1] == 'D')
return "Undefined command 1";
- else if (error[0] == '0' && error[1] == 'E')
+ else if(error[0] == '0' && error[1] == 'E')
return "Undefined command 2";
- else if (error[0] == '0' && error[1] == 'F')
+ else if(error[0] == '0' && error[1] == 'F')
return "Command with insufficient parameters 2";
- else if (error[0] == '0' && error[1] == 'G')
+ else if(error[0] == '0' && error[1] == 'G')
return "String character in command exceeds 16 letters";
- else if (error[0] == '0' && error[1] == 'H')
+ else if(error[0] == '0' && error[1] == 'H')
return "String character has invalid letters";
- else if (error[0] == '0' && error[1] == 'I')
+ else if(error[0] == '0' && error[1] == 'I')
return "Sensor is now in firmware update mode";
int error_code = atoi(error);
- if (cmd[0] == 'B' && cmd[1] == 'M')
+ if(cmd[0] == 'B' && cmd[1] == 'M')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Unable to control due to laser malfunction";
@@ -70,9 +70,9 @@
}
}
// No info in the manual for this.
-// else if (cmd[0] == 'Q' && cmd[1] == 'T')
+// else if(cmd[0] == 'Q' && cmd[1] == 'T')
// {
-// switch (error_code)
+// switch(error_code)
// {
// default:
// std::stringstream ss;
@@ -81,11 +81,11 @@
// return ss.str ();
// }
// }
- else if (((cmd[0] == 'G' || cmd[0] == 'H') &&
+ else if(((cmd[0] == 'G' || cmd[0] == 'H') &&
(cmd[1] == 'D' || cmd[1] == 'E')) ||
(cmd[0] == 'G' && cmd[1] == 'S'))
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Starting step has non-numeric value";
@@ -100,7 +100,7 @@
case 10:
return "Laser is off";
default:
- if (error_code >= 50)
+ if(error_code >= 50)
ss << "Hardware error: " << error_code;
else
ss << "Unknown error code " << error_code <<
@@ -109,11 +109,11 @@
return ss.str();
}
}
- else if (((cmd[0] == 'M' || cmd[0] == 'N') &&
+ else if(((cmd[0] == 'M' || cmd[0] == 'N') &&
(cmd[1] == 'D' || cmd[1] == 'E')) ||
(cmd[0] == 'M' && cmd[1] == 'S'))
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Starting step has non-numeric value";
@@ -130,14 +130,14 @@
case 7:
return "Number of scans is non-numeric";
default:
- if (error_code >= 21 && error_code <= 49)
+ if(error_code >= 21 && error_code <= 49)
{
return "Processing stopped to verify error. "
"This function is not yet supported by hokuyo_aist.";
}
- else if (error_code >= 50 && error_code <= 97)
+ else if(error_code >= 50 && error_code <= 97)
ss << "Hardware error: " << error_code;
- else if (error_code == 98)
+ else if(error_code == 98)
{
return "Resumption of processing after confirming normal "
"laser opteration. This function is not yet supported "
@@ -150,9 +150,9 @@
return ss.str();
}
}
- else if (cmd[0] == 'T' && cmd[1] == 'M')
+ else if(cmd[0] == 'T' && cmd[1] == 'M')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Invalid control code";
@@ -166,9 +166,9 @@
return "Adjust mode is off when requested time";
}
}
- else if (cmd[0] == 'S' && cmd[1] == 'S')
+ else if(cmd[0] == 'S' && cmd[1] == 'S')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Baud rate has non-numeric value";
@@ -180,9 +180,9 @@
return "Not compatible with the sensor model";
}
}
- else if (cmd[0] == 'C' && cmd[1] == 'R')
+ else if(cmd[0] == 'C' && cmd[1] == 'R')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Invalid speed";
@@ -194,9 +194,9 @@
return "Not compatible with the sensor model";
}
}
- else if (cmd[0] == 'H' && cmd[1] == 'S')
+ else if(cmd[0] == 'H' && cmd[1] == 'S')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Parameter error";
@@ -207,9 +207,9 @@
}
}
// No info in the manual for this.
-// else if (cmd[0] == 'R' && cmd[1] == 'S')
+// else if(cmd[0] == 'R' && cmd[1] == 'S')
// {
-// switch (error_code)
+// switch(error_code)
// {
// case :
// return "";
@@ -221,35 +221,35 @@
// }
// }
// No info in the manual for this.
-// else if (cmd[0] == 'V' && cmd[1] == 'V')
+// else if(cmd[0] == 'V' && cmd[1] == 'V')
// {
-// switch (error_code)
+// switch(error_code)
// {
// case :
// return "";
// }
// }
// No info in the manual for this.
-// else if (cmd[0] == 'P' && cmd[1] == 'P')
+// else if(cmd[0] == 'P' && cmd[1] == 'P')
// {
-// switch (error_code)
+// switch(error_code)
// {
// case :
// return "";
// }
// }
// No info in the manual for this.
-// else if (cmd[0] == 'I' && cmd[1] == 'I')
+// else if(cmd[0] == 'I' && cmd[1] == 'I')
// {
-// switch (error_code)
+// switch(error_code)
// {
// case :
// return "";
// }
// }
- else if (cmd[0] == 'D' && cmd[1] == 'B')
+ else if(cmd[0] == 'D' && cmd[1] == 'B')
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "Parameter error";
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -77,7 +77,7 @@
/** Formatted description of the error. */
std::stringstream ss;
- /** Static string representation of the error. */
+ /** String representation of the error. */
char _error_type[32];
}; //class BaseError
@@ -113,10 +113,6 @@
: BaseError(desc_code, error_type)
{}
virtual ~RuntimeError() throw() {};
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class RuntimeError
@@ -449,7 +445,7 @@
}; // class MissingFirmSpecError
-/// Bad response error
+/// Bad response error - may be sent in response to any command
class HOKUYO_AIST_EXPORT ResponseError: public ProtocolError
{
public:
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -42,7 +42,7 @@
: _error(error), _time(time), _model(model)
{
_length = length;
- if (_length == 0)
+ if(_length == 0)
{
_ranges = NULL;
_intensities = NULL;
@@ -53,7 +53,7 @@
{
_ranges = new uint32_t[_length];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
_length = 0;
throw;
@@ -65,13 +65,13 @@
}
-ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
+ScanData::ScanData(uint32_t* ranges, uint32_t* intensities,
unsigned int length, bool error, unsigned int time,
LaserModel model)
: _error(error), _time(time), _model(model)
{
_length = length;
- if (_length == 0)
+ if(_length == 0)
{
_ranges = NULL;
_intensities = NULL;
@@ -82,7 +82,7 @@
{
_ranges = new uint32_t[_length];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
_length = 0;
throw;
@@ -95,10 +95,10 @@
}
-ScanData::ScanData(const ScanData &rhs)
+ScanData::ScanData(ScanData const& rhs)
{
_length = rhs.length();
- if (_length == 0)
+ if(_length == 0)
_ranges = NULL;
else
{
@@ -106,14 +106,14 @@
{
_ranges = new uint32_t[_length];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
_length = 0;
throw;
}
memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
- if (rhs.intensities() != NULL)
+ if(rhs.intensities() != NULL)
{
_intensities = new uint32_t[_length];
memcpy(_intensities, rhs.intensities(),
@@ -135,9 +135,9 @@
std::string ScanData::error_code_to_string(uint32_t error_code)
{
- if (_model == MODEL_UTM30LX)
+ if(_model == MODEL_UTM30LX)
{
- switch (error_code)
+ switch(error_code)
{
case 1:
return "No object in the range.";
@@ -157,7 +157,7 @@
}
else
{
- switch (error_code)
+ switch(error_code)
{
case 0:
return "Detected object is possibly at 22m.";
@@ -210,15 +210,15 @@
}
-ScanData& ScanData::operator=(const ScanData &rhs)
+ScanData& ScanData::operator=(ScanData const& rhs)
{
- if (rhs.length() == 0)
+ if(rhs.length() == 0)
{
_length = 0;
- if (_ranges != NULL)
+ if(_ranges != NULL)
delete[] _ranges;
_ranges = NULL;
- if (_intensities != NULL)
+ if(_intensities != NULL)
delete[] _intensities;
_intensities = NULL;
_error = rhs.get_error_status();
@@ -228,37 +228,37 @@
else
{
unsigned int rhslength = rhs.length();
- uint32_t *newData;
- if (rhslength != _length)
+ uint32_t* newData;
+ if(rhslength != _length)
{
// Copy the data into a temporary variable pointing to new space
// (prevents dangling pointers on allocation error and prevents
// self-assignment making a mess).
newData = new uint32_t[rhslength];
memcpy(newData, rhs.ranges(), sizeof(uint32_t) * rhslength);
- if (_ranges != NULL)
+ if(_ranges != NULL)
delete[] _ranges;
_ranges = newData;
_length = rhs.length();
- if (rhs.intensities() != NULL)
+ if(rhs.intensities() != NULL)
{
try
{
newData = new uint32_t[rhslength];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
// We have to remove any old intensity data or the length
// won't match
- if (_intensities != NULL)
+ if(_intensities != NULL)
delete[] _intensities;
_intensities = NULL;
throw;
}
memcpy(newData, rhs.intensities(), sizeof(uint32_t) *
rhslength);
- if (_intensities != NULL)
+ if(_intensities != NULL)
delete[] _intensities;
_intensities = newData;
}
@@ -267,7 +267,7 @@
{
// If lengths are the same, no need to reallocate
memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
- if (rhs.intensities() != NULL)
+ if(rhs.intensities() != NULL)
memcpy(_intensities, rhs.intensities(), sizeof(uint32_t) *
_length);
}
@@ -283,7 +283,7 @@
uint32_t ScanData::operator[](unsigned int index)
{
- if (index >= _length)
+ if(index >= _length)
throw IndexError();
return _ranges[index];
}
@@ -297,19 +297,19 @@
ss << model_to_string(_model) << ":\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _ranges[ii] << "\t";
- if (_intensities != NULL)
+ if(_intensities != NULL)
{
ss << std::endl << _length << " intensities:\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _intensities[ii] << "\t";
}
ss << '\n';
- if (_error)
+ if(_error)
{
ss << "Detected data errors:\n";
for (unsigned int ii = 0; ii < _length; ii++)
{
- if (_ranges[ii] < 20)
+ if(_ranges[ii] < 20)
ss << ii << ": " <<
error_code_to_string(_ranges[ii]) << '\n';
}
@@ -324,10 +324,10 @@
void ScanData::clean_up()
{
- if (_ranges != NULL)
+ if(_ranges != NULL)
delete[] _ranges;
_ranges = NULL;
- if (_intensities != NULL)
+ if(_intensities != NULL)
delete[] _intensities;
_intensities = NULL;
_length = 0;
@@ -339,13 +339,13 @@
void ScanData::allocate_data(unsigned int length, bool include_intensities)
{
// If no data yet, allocate new
- if (_ranges == NULL)
+ if(_ranges == NULL)
{
try
{
_ranges = new uint32_t[length];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
_length = 0;
throw;
@@ -353,14 +353,14 @@
_length = length;
}
// If there is data, reallocate only if the length is different
- else if (length != _length)
+ else if(length != _length)
{
delete[] _ranges;
try
{
_ranges = new uint32_t[length];
}
- catch (std::bad_alloc &e)
+ catch(std::bad_alloc& e)
{
_length = 0;
throw;
@@ -369,22 +369,22 @@
}
// Else data is already allocated to the right length, so do nothing
- if (include_intensities)
+ if(include_intensities)
{
// If no data yet, allocate new
- if (_intensities == NULL)
+ if(_intensities == NULL)
{
_intensities = new uint32_t[length];
}
// If there is data, reallocate only if the length is different
- else if (length != _length)
+ else if(length != _length)
{
delete[] _intensities;
_intensities = new uint32_t[length];
}
// Else data is already allocated to the right length, so do nothing
}
- else if (_intensities != NULL)
+ else if(_intensities != NULL)
{
// If not told to allocate space for intensity data and it exists,
// remove it
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -66,7 +66,7 @@
unsigned int length, bool error, unsigned int time,
LaserModel model);
/// This copy constructor performs a deep copy of present data.
- ScanData(const ScanData &rhs);
+ ScanData(ScanData const& rhs);
~ScanData();
/** @brief Return a pointer to array of range readings in millimetres.
@@ -97,7 +97,7 @@
LaserModel model() const { return _model; }
/// @brief Assignment operator.
- ScanData& operator=(const ScanData &rhs);
+ ScanData& operator=(ScanData const& rhs);
/** @brief Subscript operator.
Provides direct access to an element of the range data. */
@@ -110,8 +110,8 @@
void clean_up();
protected:
- uint32_t *_ranges;
- uint32_t *_intensities;
+ uint32_t* _ranges;
+ uint32_t* _intensities;
unsigned int _length;
bool _error;
unsigned int _time;
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -27,6 +27,7 @@
#include <flexiport/port.h>
#include <flexiport/serialport.h>
+#include <cassert>
#include <cstring>
#include <stdarg.h>
#include <stdlib.h>
@@ -45,9 +46,9 @@
{
// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
+unsigned int const SCIP1_LINE_LENGTH = 66;
// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
+unsigned int const SCIP2_LINE_LENGTH = 67;
///////////////////////////////////////////////////////////////////////////////
// SCIP protocol version 1 notes
@@ -213,7 +214,7 @@
// Utility functions
///////////////////////////////////////////////////////////////////////////////
-unsigned int decode_2_byte_value(char *data)
+unsigned int decode_2_byte_value(char* data)
{
unsigned int byte1, byte2;
@@ -224,7 +225,7 @@
}
-unsigned int decode_3_byte_value(char *data)
+unsigned int decode_3_byte_value(char* data)
{
unsigned int byte1, byte2, byte3;
@@ -236,7 +237,7 @@
}
-unsigned int decode_4_byte_value(char *data)
+unsigned int decode_4_byte_value(char* data)
{
unsigned int byte1, byte2, byte3, byte4;
@@ -290,14 +291,14 @@
Sensor::~Sensor()
{
- if (_port != NULL)
+ if(_port != NULL)
delete _port;
}
void Sensor::open(std::string port_options)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Creating and opening port using options: " <<
@@ -306,7 +307,7 @@
_port = flexiport::CreatePort(port_options);
_port->Open();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Connected using " <<
_port->GetPortType() << " connection.\n";
@@ -324,7 +325,7 @@
unsigned int Sensor::open_with_probing(std::string port_options)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Creating and opening port using options: " << port_options <<
@@ -333,7 +334,7 @@
_port = flexiport::CreatePort(port_options);
_port->Open();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Connected using " <<
_port->GetPortType() << " connection.\n";
@@ -349,20 +350,20 @@
// Get some values we need for providing default ranges
_get_defaults();
}
- catch (BaseError)
+ catch(BaseError)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Failed to connect at the default baud rate.\n";
}
- if (_port->GetPortType() == "serial")
+ if(_port->GetPortType() == "serial")
{
// Failed at the default baud rate, so try again at the other
// rates. Note that a baud rate of 750000 or 250000 doesn't appear
// to be supported on any common OS.
- const unsigned int bauds[] = {500000, 115200, 57600, 38400, 19200};
- const unsigned int numBauds = 5;
+ unsigned int const bauds[] = {500000, 115200, 57600, 38400, 19200};
+ unsigned int const numBauds(5);
for (unsigned int ii = 0; ii < numBauds; ii++)
{
reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(bauds[ii]);
@@ -372,19 +373,19 @@
_get_defaults();
// If the above two functions succeed, break out of the
// loop and be happy
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Connected at " << bauds[ii] << '\n';
}
return bauds[ii];
}
- catch (BaseError)
+ catch(BaseError)
{
- if (ii == numBauds - 1)
+ if(ii == numBauds - 1)
{
// Last baud rate, give up and rethrow
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Failed to connect at any baud rate.\n";
@@ -397,7 +398,7 @@
}
else
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Port is not serial, cannot probe.\n";
@@ -406,7 +407,7 @@
}
}
- if (_port->GetPortType() == "serial")
+ if(_port->GetPortType() == "serial")
return reinterpret_cast<flexiport::SerialPort*>(_port)->GetBaudRate();
else
return 0;
@@ -415,9 +416,9 @@
void Sensor::close()
{
- if (!_port)
+ if(!_port)
throw CloseError();
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ << "() Closing connection.\n";
delete _port;
_port = NULL;
@@ -426,7 +427,7 @@
bool Sensor::is_open() const
{
- if (_port != NULL)
+ if(_port != NULL)
return _port->IsOpen();
return false;
}
@@ -434,36 +435,36 @@
void Sensor::set_power(bool on)
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
- if (on)
+ if(on)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Turning laser on.\n";
_send_command("L", "1", 1, NULL);
}
else
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Turning laser off.\n";
_send_command("L", "0", 1, NULL);
}
_skip_lines(1);
}
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (on)
+ if(on)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Turning laser on.\n";
_send_command("BM", NULL, 0, "02");
}
else
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Turning laser off.\n";
_send_command("QT", NULL, 0, "02");
@@ -479,22 +480,20 @@
// set to the same baud.
void Sensor::set_baud(unsigned int baud)
{
- if (_port->GetPortType() != "serial")
- {
+ if(_port->GetPortType() != "serial")
throw NotSerialError();
- }
char newBaud[13];
memset(newBaud, 0, sizeof(char) * 13);
- if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
+ if(baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
baud != 250000 && baud != 500000 && baud != 750000)
{
throw BaudrateError(baud);
}
number_to_string(baud, newBaud, 6);
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
// Send the command to change baud rate
_send_command("S", newBaud, 13, NULL);
@@ -502,7 +501,7 @@
// Change the port's baud rate
reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
}
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
// Send the command to change baud rate
_send_command("SS", newBaud, 6, "03");
@@ -532,7 +531,7 @@
'.' << subnet.fourth << ' ';
params << gateway.first << '.' << gateway.second << '.' << gateway.third <<
'.' << gateway.fourth;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Setting IP information to $I" << params.str() << '\n';
@@ -540,18 +539,18 @@
int status = _send_command(&command[0], params.str().c_str(), 48, NULL);
// Skip the extra line feed
_skip_lines(1);
- if (status != 0)
+ if(status != 0)
throw SetIPError();
}
void Sensor::reset()
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(7);
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Resetting laser.\n";
@@ -566,11 +565,11 @@
void Sensor::semi_reset()
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(35);
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Resetting laser.\n";
_send_command("RS", NULL, 0, NULL);
@@ -583,17 +582,17 @@
void Sensor::set_motor_speed(unsigned int speed)
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(8);
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
// Sanity check the value
- if (speed > 10 && speed != 99)
+ if(speed > 10 && speed != 99)
throw MotorSpeedError();
char buffer[3];
- if (speed == 0)
+ if(speed == 0)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Reseting motor speed to default.\n";
@@ -602,9 +601,9 @@
buffer[1] = '0';
buffer[2] = '\0';
}
- else if (speed == 99)
+ else if(speed == 99)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Reseting motor speed to default.\n";
@@ -615,7 +614,7 @@
}
else
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Setting motor speed to ratio " << speed << '\n';
@@ -632,20 +631,20 @@
void Sensor::set_high_sensitivity(bool on)
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(10);
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (on)
+ if(on)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Switching to high sensitivity.\n";
_send_command("HS", "1", 1, "02");
}
else
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Switching to normal sensitivity.\n";
@@ -659,14 +658,14 @@
}
-void Sensor::get_sensor_info(SensorInfo *info)
+void Sensor::get_sensor_info(SensorInfo* info)
{
- if (info == NULL)
+ if(info == NULL)
throw NoDestinationError();
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Getting sensor information using SCIP version 1.\n";
@@ -697,7 +696,7 @@
info->serial = &buffer[5];
// Get either the status line or the end of message
_read_line(buffer);
- if (buffer[0] != '\0')
+ if(buffer[0] != '\0')
{
// Got a status line
info->sensor_diagnostic = &buffer[5];
@@ -709,17 +708,17 @@
// eg: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
// Note that this example is right up against the maximum SCIP v1 line
// length of 64 bytes.
- if (atoi(info->firmware.c_str()) >= 3)
+ if(atoi(info->firmware.c_str()) >= 3)
{
- if (_verbose)
+ if(_verbose)
_err_output << "SCIP1 Firmware line for parsing: " <<
info->firmware << '\n';
// Now the fun part: parsing the line. It would be nice if we could
// use the POSIX regex functions, but since MS doesn't believe in
// POSIX we get to do it the hard way.
// Start by finding the first (
- const char *valueStart;
- if ((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
+ char const* valueStart;
+ if((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
{
// No bracket? Crud. Fail and use the hard-coded values from
// the manual.
@@ -731,13 +730,13 @@
"(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info->min_range,
&info->max_range, &aperture, &info->first_step,
&info->last_step, &info->speed);
- if (numFound != 6)
+ if(numFound != 6)
{
// Didn't get enough values out, assume unknown format and fall
// back on the defaults
info->set_defaults();
info->calculate_values();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Retrieved sensor info (hard-coded, not "
"enough values):\n";
@@ -759,7 +758,7 @@
info->max_angle = (info->last_step - info->front_step) *
info->resolution;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Retrieved sensor info (from FIRM line):\n";
_err_output << info->as_string();
@@ -771,16 +770,16 @@
// We're stuck with hard-coded defaults from the manual (already
// set earlier).
info->calculate_values();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Retrieved sensor info (hard-coded):\n";
_err_output << info->as_string();
}
}
}
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Getting sensor information using SCIP version 2.\n";
@@ -794,23 +793,23 @@
// We need to send three commands to get all the info we want: VV, PP
// and II
_send_command("VV", NULL, 0, NULL);
- while (_read_line_with_check(buffer, -1, true) != 0)
+ while(_read_line_with_check(buffer, -1, true) != 0)
_process_vv_line(buffer, info);
// Next up, PP
_send_command("PP", NULL, 0, NULL);
- while (_read_line_with_check(buffer, -1, true) != 0)
+ while(_read_line_with_check(buffer, -1, true) != 0)
_process_pp_line(buffer, info);
// Command II: Revenge of the Commands.
_send_command("II", NULL, 0, NULL);
- while (_read_line_with_check(buffer, -1, true) != 0)
+ while(_read_line_with_check(buffer, -1, true) != 0)
_process_ii_line(buffer, info);
_enable_checksum_workaround = false;
info->calculate_values();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Retrieved sensor info:\n";
_err_output << info->as_string();
@@ -823,11 +822,11 @@
unsigned int Sensor::get_time()
{
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(12);
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Retrieving time from laser.\n";
_send_command("TM", "0", 1, NULL);
@@ -846,22 +845,22 @@
}
-unsigned int Sensor::get_ranges(ScanData *data, int start_step,
+unsigned int Sensor::get_ranges(ScanData* data, int start_step,
int end_step, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
char buffer[11];
memset(buffer, 0, sizeof(char) * 11);
- if (start_step < 0)
+ if(start_step < 0)
start_step = _first_step;
- if (end_step < 0)
+ if(end_step < 0)
end_step = _last_step;
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges between " << start_step << " and " <<
@@ -869,7 +868,7 @@
'\n';
}
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
// Send the command to ask for the most recent range data from
// start_step to end_step
@@ -880,21 +879,21 @@
// In SCIP1 mode we're going to get back 2-byte data
_read_2_byte_range_data(data, num_steps);
}
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
// Send the command to ask for the most recent range data from
// start_step to end_step
number_to_string(start_step, buffer, 4);
number_to_string(end_step, &buffer[4], 4);
number_to_string(cluster_count, &buffer[8], 2);
- if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+ if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
_send_command("HD", buffer, 10, NULL);
else
_send_command("GD", buffer, 10, NULL);
// There will be a timestamp before the data (if there is data)
// Normally we would send 6 for the expected length, but we may get no
// timestamp back if there was no data.
- if (_read_line_with_check(buffer) == 0)
+ if(_read_line_with_check(buffer) == 0)
throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
@@ -908,10 +907,10 @@
}
-unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
+unsigned int Sensor::get_ranges_by_angle(ScanData* data, double start_angle,
double end_angle, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDataError();
// Calculate the given angles in steps, rounding towards _front_step
@@ -920,12 +919,12 @@
end_step = angle_to_step(end_angle);
// Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
+ if(start_step < _first_step || start_step > _last_step)
throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
+ if(end_step < _first_step || end_step > _last_step)
throw EndStepError();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
@@ -937,22 +936,22 @@
}
-unsigned int Sensor::get_ranges_intensities(ScanData *data, int start_step,
+unsigned int Sensor::get_ranges_intensities(ScanData* data, int start_step,
int end_step, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
char buffer[11];
memset(buffer, 0, sizeof(char) * 11);
- if (start_step < 0)
+ if(start_step < 0)
start_step = _first_step;
- if (end_step < 0)
+ if(end_step < 0)
end_step = _last_step;
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges between " << start_step << " and " <<
@@ -960,9 +959,9 @@
'\n';
}
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(36);
- else if (_scip_version != 2)
+ else if(_scip_version != 2)
throw UnknownScipVersionError();
// Send the command to ask for the most recent data from
@@ -970,14 +969,14 @@
number_to_string(start_step, buffer, 4);
number_to_string(end_step, &buffer[4], 4);
number_to_string(cluster_count, &buffer[8], 2);
- if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+ if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
_send_command("HE", buffer, 10, NULL);
else
_send_command("GE", buffer, 10, NULL);
// There will be a timestamp before the data (if there is data)
// Normally we would send 6 for the expected length, but we may get no
// timestamp back if there was no data.
- if (_read_line_with_check(buffer) == 0)
+ if(_read_line_with_check(buffer) == 0)
throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
@@ -988,10 +987,10 @@
}
-unsigned int Sensor::get_ranges_intensities_by_angle(ScanData *data,
+unsigned int Sensor::get_ranges_intensities_by_angle(ScanData* data,
double start_angle, double end_angle, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDataError();
// Calculate the given angles in steps, rounding towards _front_step
@@ -1000,12 +999,12 @@
end_step = angle_to_step(end_angle);
// Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
+ if(start_step < _first_step || start_step > _last_step)
throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
+ if(end_step < _first_step || end_step > _last_step)
throw EndStepError();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
@@ -1017,27 +1016,27 @@
}
-unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
+unsigned int Sensor::get_new_ranges(ScanData* data, int start_step,
int end_step, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(16);
- else if (_scip_version != 2)
+ else if(_scip_version != 2)
throw UnknownScipVersionError();
char buffer[14];
memset(buffer, 0, sizeof(char) * 14);
- if (start_step < 0)
+ if(start_step < 0)
start_step = _first_step;
- if (end_step < 0)
+ if(end_step < 0)
end_step = _last_step;
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " new ranges between " << start_step << " and " <<
@@ -1052,7 +1051,7 @@
number_to_string(1, &buffer[10], 1);
number_to_string(1, &buffer[11], 2);
char command[3];
- if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+ if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
command[0] = 'N';
else
command[0] = 'M';
@@ -1066,22 +1065,22 @@
_skip_lines(1); // End of the command echo message
_read_line(response, 16); // Size is command(2)+params(13)+new line(1)
// Check the echo is correct
- if (response[0] != command[0] || response[1] != command[1])
+ if(response[0] != command[0] || response[1] != command[1])
throw CommandEchoError(command, response);
// Then compare the parameters
buffer[12] = '0'; // There will be zero scans remaining after this one
- if (memcmp(&response[2], buffer, 13) != 0)
+ if(memcmp(&response[2], buffer, 13) != 0)
throw ParamEchoError(command);
// The next line should be the status line
_read_line_with_check(response, 4);
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() " << command << " data prefix status: " << response[0] <<
response[1] << '\n';
}
// Check the status code is OK - should only get 99 here
- if (response[0] != '9' || response[1] != '9')
+ if(response[0] != '9' || response[1] != '9')
{
// There is an extra line feed after an error status (signalling
// end of message)
@@ -1093,7 +1092,7 @@
// There will be a timestamp before the data (if there is data)
// Normally we would send 6 for the expected length, but we may get no
// timestamp back if there was no data.
- if (_read_line_with_check(buffer) == 0)
+ if(_read_line_with_check(buffer) == 0)
throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
@@ -1104,12 +1103,12 @@
}
-unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
+unsigned int Sensor::get_new_ranges_by_angle(ScanData* data,
double start_angle, double end_angle, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(16);
// Calculate the given angles in steps, rounding towards _front_step
@@ -1118,12 +1117,12 @@
end_step = angle_to_step(end_angle);
// Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
+ if(start_step < _first_step || start_step > _last_step)
throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
+ if(end_step < _first_step || end_step > _last_step)
throw EndStepError();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
@@ -1135,27 +1134,27 @@
}
-unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
+unsigned int Sensor::get_new_ranges_intensities(ScanData* data,
int start_step, int end_step, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(17);
- else if (_scip_version != 2)
+ else if(_scip_version != 2)
throw UnknownScipVersionError();
char buffer[14];
memset(buffer, 0, sizeof(char) * 14);
- if (start_step < 0)
+ if(start_step < 0)
start_step = _first_step;
- if (end_step < 0)
+ if(end_step < 0)
end_step = _last_step;
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " new ranges and intensities between " <<
@@ -1171,7 +1170,7 @@
number_to_string(1, &buffer[10], 1);
number_to_string(1, &buffer[11], 2);
char command[3];
- if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+ if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
command[0] = 'N';
else
command[0] = 'M';
@@ -1185,21 +1184,22 @@
_skip_lines(1); // End of the command echo message
_read_line(response, 16); // Size is command(2)+params(13)+new line(1)
// Check the echo is correct
- if (response[0] != command[0] || response[1] != command[1])
+ if(response[0] != command[0] || response[1] != command[1])
throw CommandEchoError(command, response);
// Then compare the parameters
- if (memcmp(&response[2], buffer, 13) != 0)
+ buffer[12] = '0'; // There will be zero scans remaining after this one
+ if(memcmp(&response[2], buffer, 13) != 0)
throw ParamEchoError(command);
// The next line should be the status line
_read_line_with_check(response, 4);
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() " << command << " data prefix status: " << response[0] <<
response[1] << '\n';
}
// Check the status code is OK - should only get 99 here
- if (response[0] != '9' || response[1] != '9')
+ if(response[0] != '9' || response[1] != '9')
{
// There is an extra line feed after an error status (signalling
// end of message)
@@ -1211,7 +1211,7 @@
// There will be a timestamp before the data (if there is data)
// Normally we would send 6 for the expected length, but we may get no
// timestamp back if there was no data.
- if (_read_line_with_check(buffer) == 0)
+ if(_read_line_with_check(buffer) == 0)
throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
@@ -1222,12 +1222,12 @@
}
-unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
+unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData* data,
double start_angle, double end_angle, unsigned int cluster_count)
{
- if (data == NULL)
+ if(data == NULL)
throw NoDestinationError();
- if (_scip_version == 1)
+ if(_scip_version == 1)
throw UnsupportedError(17);
// Calculate the given angles in steps, rounding towards _front_step
@@ -1236,12 +1236,12 @@
end_step = angle_to_step(end_angle);
// Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
+ if(start_step < _first_step || start_step > _last_step)
throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
+ if(end_step < _first_step || end_step > _last_step)
throw EndStepError();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
@@ -1269,7 +1269,7 @@
(static_cast<double>(angle) / static_cast<double>(_resolution));
// Round towards _front_step so that the step values are always inside the
// angles given
- if (resultF < _front_step)
+ if(resultF < _front_step)
result = static_cast<int>(ceil(resultF));
else
result = static_cast<int>(floor(resultF));
@@ -1290,7 +1290,7 @@
// timeout (which may be infinite).
void Sensor::_clear_read_buffer()
{
- while (_port->BytesAvailableWait() > 0)
+ while(_port->BytesAvailableWait() > 0)
_port->Flush();
}
@@ -1303,46 +1303,46 @@
// The line feed that terminates a line will be replaced with a NULL.
// The return value is the number of bytes received, not including the NULL
// byte or the line feed.
-int Sensor::_read_line(char *buffer, int expected_length)
+int Sensor::_read_line(char* buffer, int expected_length)
{
int linelength = 0;
- if (expected_length == -1)
+ if(expected_length == -1)
{
int maxlength = (_scip_version == 1) ?
SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading up to " <<
maxlength << " bytes.\n";
}
// We need to get at least 1 byte in a line: the line feed.
- if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
+ if((linelength = _port->ReadLine(buffer, maxlength)) < 0)
throw ReadError(0);
- else if (linelength == 0)
+ else if(linelength == 0)
throw ReadError(1);
// Replace the line feed with a NULL
buffer[linelength - 1] = '\0';
}
else
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Reading exactly " << expected_length << " bytes.\n";
}
// expected_length+1 for the NULL
- if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
+ if((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
throw ReadError(0);
- else if (linelength == 0)
+ else if(linelength == 0)
throw ReadError(1);
- else if (linelength < expected_length)
+ else if(linelength < expected_length)
throw LineLengthError(linelength, expected_length);
// Replace the line feed with a NULL
buffer[linelength - 1] = '\0';
}
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Read " << linelength <<
" bytes.\n";
@@ -1370,11 +1370,11 @@
// _model is MODEL_UTM30LX. In this case, if the checksum fails normally, it
// scans the line for "<-" and recalculates the checksum on the bytes up to
// that point. This only happens in SCIP v2.
-int Sensor::_read_line_with_check(char *buffer, int expected_length,
+int Sensor::_read_line_with_check(char* buffer, int expected_length,
bool has_semicolon)
{
int linelength = _read_line(buffer, expected_length);
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
// No checksums in SCIP version 1
return linelength;
@@ -1382,20 +1382,20 @@
// If the line is empty, assume it was a line-feed message terminator, in
// which case there is no checksum to check.
- if (linelength == 0)
+ if(linelength == 0)
return 0;
// Ignore the checksum itself, and possibly a semicolon (_read_line has
// already chopped off the line feed for us).
int bytesToConsider = linelength - 1 - (has_semicolon ? 1 : 0);
int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Considering " <<
bytesToConsider << " bytes for checksum from a line length of " <<
linelength << " bytes.\n";
}
- if (bytesToConsider < 1)
+ if(bytesToConsider < 1)
throw InsufficientBytesError(bytesToConsider, linelength);
int checkSum = 0;
@@ -1404,23 +1404,23 @@
checkSum = _confirm_checksum(buffer, bytesToConsider,
static_cast<int>(buffer[checksumIndex]));
}
- catch (ProtocolError &e)
+ catch(ProtocolError& e)
{
- if (_model == MODEL_UTM30LX && _enable_checksum_workaround)
+ if(_model == MODEL_UTM30LX && _enable_checksum_workaround)
{
// Here comes the UTM-30LX workaround
char* hasComment = strstr(buffer, "<-");
- if (hasComment != NULL)
+ if(hasComment != NULL)
{
int newBytesToConsider = hasComment - buffer;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Performing UTM-30LX II response "
"checksum workaround: trying with " <<
newBytesToConsider << " bytes.\n";
}
- if (newBytesToConsider < 1)
+ if(newBytesToConsider < 1)
{
throw InsufficientBytesError(newBytesToConsider,
linelength);
@@ -1448,10 +1448,10 @@
// Reads lines until the number specified has passed.
void Sensor::_skip_lines(int count)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ << "() Skipping " << count <<
" lines.\n";
- if (_port->SkipUntil(0x0A, count) < 0)
+ if(_port->SkipUntil(0x0A, count) < 0)
throw ReadError(18);
}
@@ -1465,8 +1465,8 @@
// If param_length is 0, no parameters will be sent or expected in the reply.
// extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
// Return value is the status code returned for the command.
-int Sensor::_send_command(const char *cmd, const char *param,
- int param_length, const char *extra_ok)
+int Sensor::_send_command(char const* cmd, char const* param,
+ int param_length, char const* extra_ok)
{
int statusCode = -1;
char response[17];
@@ -1474,23 +1474,23 @@
// Flush first to clear out the dregs of any previous commands
_port->Flush();
- if (_scip_version == 1)
+ if(_scip_version == 1)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Writing in SCIP1 mode. Command is " << cmd[0] <<
", parameters length is " << param_length << '\n';
}
// Write the command
- if (_port->Write(cmd, 1) < 1)
+ if(_port->Write(cmd, 1) < 1)
throw WriteError(19);
- if (param_length > 0)
+ if(param_length > 0)
{
- if (_port->Write(param, param_length) < param_length)
+ if(_port->Write(param, param_length) < param_length)
throw WriteError(20);
}
- if (_port->Write("\n", 1) < 1)
+ if(_port->Write("\n", 1) < 1)
throw WriteError(21);
// Read back the response (should get at least 4 bytes , possibly up to
@@ -1500,7 +1500,7 @@
_read_line(response, 2 + param_length);
_read_line(&response[statusIndex], 2);
// First make sure that the echoed command matches
- if (response[0] != cmd[0])
+ if(response[0] != cmd[0])
{
char temp_cmd[2];
temp_cmd[0] = cmd[0];
@@ -1511,9 +1511,9 @@
throw CommandEchoError(temp_cmd, temp_echo);
}
// Then compare the parameters
- if (param_length > 0)
+ if(param_length > 0)
{
- if (memcmp(&response[1], param, param_length) != 0)
+ if(memcmp(&response[1], param, param_length) != 0)
{
char temp_cmd[2];
temp_cmd[0] = cmd[0];
@@ -1522,17 +1522,17 @@
}
}
// Next up, check the status byte
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Command response status: " << response[statusIndex] <<
'\n';
}
- if (response[statusIndex] != '0')
+ if(response[statusIndex] != '0')
{
- if (extra_ok != NULL)
+ if(extra_ok != NULL)
{
- if (response[statusIndex] != extra_ok[0])
+ if(response[statusIndex] != extra_ok[0])
{
// There is an extra line feed after an error status
// (signalling end of message)
@@ -1551,40 +1551,40 @@
statusCode = atoi(&response[statusIndex]);
// All OK, data starts at beginning of port's buffer
}
- else if (_scip_version == 2)
+ else if(_scip_version == 2)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Writing in SCIP2 mode. Command is " << cmd <<
", parameters length is " << param_length << '\n';
}
// Write the command
- if (_port->Write(cmd, 2) < 2)
+ if(_port->Write(cmd, 2) < 2)
throw WriteError(19);
- if (param_length > 0)
+ if(param_length > 0)
{
- if (_port->Write(param, param_length) < param_length)
+ if(_port->Write(param, param_length) < param_length)
throw WriteError(20);
}
- if (_port->Write("\n", 1) < 1)
+ if(_port->Write("\n", 1) < 1)
throw WriteError(21);
// Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
_read_line(response, 3 + param_length);
// Check the echo is correct
- if (response[0] != cmd[0] || response[1] != cmd[1])
+ if(response[0] != cmd[0] || response[1] != cmd[1])
throw CommandEchoError(cmd, response);
// Then compare the parameters
- if (param_length > 0)
+ if(param_length > 0)
{
- if (memcmp(&response[2], param, param_length) != 0)
+ if(memcmp(&response[2], param, param_length) != 0)
throw ParamEchoError(cmd);
}
// The next line should be the status line
_read_line_with_check(response, 4);
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Command response status: " << response[0] << response[1] <<
@@ -1592,12 +1592,12 @@
}
// Check the status code is OK
response[2] = '\0';
- if (!(response[0] == '0' && response[1] == '0') &&
+ if(!(response[0] == '0' && response[1] == '0') &&
!(response[0] == '9' && response[1] == '9'))
{
- if (extra_ok != NULL)
+ if(extra_ok != NULL)
{
- if (response[0] != extra_ok[0] || response[1] != extra_ok[1])
+ if(response[0] != extra_ok[0] || response[1] != extra_ok[1])
{
// There is an extra line feed after an error status
// (signalling end of message)
@@ -1625,20 +1625,20 @@
void Sensor::_find_model(char const* buffer)
{
- if (strstr(buffer, "URG-04LX") != NULL)
+ if(strstr(buffer, "URG-04LX") != NULL)
_model = MODEL_URG04LX;
- else if (strstr(buffer, "UBG-04LX") != NULL)
+ else if(strstr(buffer, "UBG-04LX") != NULL)
_model = MODEL_UBG04LXF01;
- else if (strstr(buffer, "UHG-08LX") != NULL)
+ else if(strstr(buffer, "UHG-08LX") != NULL)
_model = MODEL_UHG08LX;
- else if (strstr(buffer, "UTM-30LX") != NULL)
+ else if(strstr(buffer, "UTM-30LX") != NULL)
{
_model = MODEL_UTM30LX;
// Also enable the work around for a checksum problem in this
// model.
_enable_checksum_workaround = true;
}
- else if (strstr(buffer, "UXM-30LX") != NULL)
+ else if(strstr(buffer, "UXM-30LX") != NULL)
_model = MODEL_UXM30LXE;
else
_model = MODEL_UNKNOWN;
@@ -1650,7 +1650,7 @@
bool scip2Failed = false;
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Testing SCIP protocol version.\n";
// Try SCIP version 2 first by sending an info command
@@ -1658,16 +1658,16 @@
{
_send_command("VV", NULL, 0, NULL);
}
- catch (BaseError)
+ catch(BaseError)
{
// That didn't work too well...
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Initial SCIP version 2 test failed.\n";
scip2Failed = true;
}
- if (scip2Failed)
+ if(scip2Failed)
{
// Currently using SCIP version 1
// Get the firmware version and check if we can move to SCIP version 2
@@ -1678,7 +1678,7 @@
{
_send_command("V", NULL, 0, NULL);
}
- catch (BaseError)
+ catch(BaseError)
{
throw ScipVersionError();
}
@@ -1689,7 +1689,7 @@
memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
_read_line(buffer);
- if (strncmp(buffer, "FIRM:", 5) != 0)
+ if(strncmp(buffer, "FIRM:", 5) != 0)
throw MissingFirmSpecError();
// Pull out the major version number
// Note that although lasers such as the UTM-30LX appear to use a
@@ -1697,9 +1697,9 @@
// don't support SCIP v1 and so shouldn't get to this point anyway - if
// they do, it's an uncaught error.
int majorVer = strtol(&buffer[5], NULL, 10);
- if (errno == ERANGE)
+ if(errno == ERANGE)
throw FirmwareError();
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Firmware major version is " << majorVer <<
@@ -1711,9 +1711,9 @@
// If the firmware version is less than 3, we're stuck with SCIP
// version 1.
- if (majorVer < 3)
+ if(majorVer < 3)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Firmware does not support SCIP version 2; using SCIP "
"version 1.\n";
@@ -1732,9 +1732,9 @@
{
_send_command("S", "CIP2.0", 6, NULL);
}
- catch (BaseError)
+ catch(BaseError)
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Could not change to SCIP version 2; using SCIP "
"version 1.\n";
@@ -1744,7 +1744,7 @@
_skip_lines(1);
// Changed to SCIP version 2
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Using SCIP version 2.\n";
_scip_version = 2;
@@ -1758,7 +1758,7 @@
// Dump the rest of the result
_skip_lines(6);
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Using SCIP version 2.\n";
return;
@@ -1771,7 +1771,7 @@
void Sensor::_get_defaults()
{
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ <<
"() Getting default values.\n";
@@ -1786,7 +1786,7 @@
_last_step = info.last_step;
_front_step = info.front_step;
_max_range = info.max_range;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Got default values: " << _min_angle << " " << _max_angle <<
@@ -1796,79 +1796,79 @@
}
-void Sensor::_process_vv_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_vv_line(char const* buffer, SensorInfo* info)
{
- if (strncmp(buffer, "VEND", 4) == 0)
+ if(strncmp(buffer, "VEND", 4) == 0)
info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
- else if (strncmp(buffer, "PROD", 4) == 0)
+ else if(strncmp(buffer, "PROD", 4) == 0)
{
info->product = &buffer[5]; // Product info
// Find the product model
_find_model(&buffer[5]);
info->detected_model = _model;
}
- else if (strncmp(buffer, "FIRM", 4) == 0)
+ else if(strncmp(buffer, "FIRM", 4) == 0)
info->firmware = &buffer[5]; // Firmware version
- else if (strncmp(buffer, "PROT", 4) == 0)
+ else if(strncmp(buffer, "PROT", 4) == 0)
info->protocol = &buffer[5]; // Protocol version
- else if (strncmp(buffer, "SERI", 4) == 0)
+ else if(strncmp(buffer, "SERI", 4) == 0)
info->serial = &buffer[5]; // Serial number
- else if (!_ignore_unknowns)
+ else if(!_ignore_unknowns)
throw UnknownLineError(buffer);
}
-void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_pp_line(char const* buffer, SensorInfo* info)
{
- if (strncmp(buffer, "MODL", 4) == 0)
+ if(strncmp(buffer, "MODL", 4) == 0)
info->model = &buffer[5]; // Model
// On to the fun ones that require parsing
- else if (strncmp(buffer, "DMIN", 4) == 0)
+ else if(strncmp(buffer, "DMIN", 4) == 0)
info->min_range = atoi(&buffer[5]);
- else if (strncmp(buffer, "DMAX", 4) == 0)
+ else if(strncmp(buffer, "DMAX", 4) == 0)
info->max_range = atoi(&buffer[5]);
- else if (strncmp(buffer, "ARES", 4) == 0)
+ else if(strncmp(buffer, "ARES", 4) == 0)
info->steps = atoi(&buffer[5]);
- else if (strncmp(buffer, "AMIN", 4) == 0)
+ else if(strncmp(buffer, "AMIN", 4) == 0)
info->first_step = atoi(&buffer[5]);
- else if (strncmp(buffer, "AMAX", 4) == 0)
+ else if(strncmp(buffer, "AMAX", 4) == 0)
info->last_step = atoi(&buffer[5]);
- else if (strncmp(buffer, "AFRT", 4) == 0)
+ else if(strncmp(buffer, "AFRT", 4) == 0)
info->front_step = atoi(&buffer[5]);
- else if (strncmp(buffer, "SCAN", 4) == 0)
+ else if(strncmp(buffer, "SCAN", 4) == 0)
info->standard_speed = atoi(&buffer[5]);
/* No example in the manual and sensor with support for this has not
* arrived yet, so don't know what to look for.
- else if (strncmp(buffer, "", 4) == 0)
+ else if(strncmp(buffer, "", 4) == 0)
{
- if (strstr(buffer, "CCW") != NULL)
+ if(strstr(buffer, "CCW") != NULL)
info->rot_dir = COUNTERCLOCKWISE;
else
info->rot_dir = CLOCKWISE;
}*/
- else if (!_ignore_unknowns)
+ else if(!_ignore_unknowns)
throw UnknownLineError(buffer);
}
-void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_ii_line(char const* buffer, SensorInfo* info)
{
- if (strncmp(buffer, "MODL", 4) == 0)
+ if(strncmp(buffer, "MODL", 4) == 0)
// Do nothing here - we already know this value from PP
return;
- else if (strncmp(buffer, "LASR", 4) == 0)
+ else if(strncmp(buffer, "LASR", 4) == 0)
{
- if (strncmp(&buffer[5], "OFF", 3) == 0)
+ if(strncmp(&buffer[5], "OFF", 3) == 0)
info->power = false;
else
info->power = true;
}
- else if (strncmp(buffer, "SCSP", 4) == 0)
+ else if(strncmp(buffer, "SCSP", 4) == 0)
{
- if (strncmp(&buffer[5], "Initial", 7) == 0)
+ if(strncmp(&buffer[5], "Initial", 7) == 0)
{
// Unchanged motor speed
- if (sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
+ if(sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
{
throw ParseError(buffer, "Motor speed");
}
@@ -1878,42 +1878,76 @@
{
// Changed motor speed, format is:
// <level>%<ignored string>(<speed>[rpm])
- if (sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
+ if(sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
&info->speed) != 2)
{
throw ParseError(buffer, "Motor speed");
}
}
}
- else if (strncmp(buffer, "MESM", 4) == 0)
+ else if(strncmp(buffer, "MESM", 4) == 0)
info->measure_state = &buffer[5];
- else if (strncmp(buffer, "SBPS", 4) == 0)
+ else if(strncmp(buffer, "SBPS", 4) == 0)
{
- if (strncmp(&buffer[5], "USB only", 8) == 0 ||
+ if(strncmp(&buffer[5], "USB only", 8) == 0 ||
strncmp(&buffer[5], "USB Full Speed", 14) == 0)
{
// No baud rate for USB-only devices such as the UHG-08LX
info->baud = 0;
}
- else if (sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
+ else if(sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
throw ParseError(buffer, "Baud rate");
}
- else if (strncmp(buffer, "TIME", 4) == 0)
+ else if(strncmp(buffer, "TIME", 4) == 0)
{
- if (sscanf(buffer, "TIME:%x", &info->time) != 1)
+ if(sscanf(buffer, "TIME:%x", &info->time) != 1)
throw ParseError(buffer, "Timestamp");
}
- else if (strncmp(buffer, "STAT", 4) == 0)
+ else if(strncmp(buffer, "STAT", 4) == 0)
info->sensor_diagnostic = &buffer[5];
- else if (!_ignore_unknowns)
+ else if(!_ignore_unknowns)
throw UnknownLineError(buffer);
}
-void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
+/// Combines up to three values from an echo buffer into a single value based
+/// on the setting of _multiecho_mode.
+void Sensor::_process_echo_buffer(int const* buffer, int num_echos,
+ uint32_t* dest)
{
- if (_verbose)
+ uint32_t sum = 0;
+ switch(_multiecho_mode)
{
+ case ME_FRONT:
+ *dest = buffer[0];
+ break;
+ case ME_MIDDLE:
+ if(num_echos == 3)
+ *dest = buffer[1];
+ else
+ *dest = buffer[0];
+ break;
+ case ME_REAR:
+ *dest = buffer[num_echos - 1];
+ break;
+ case ME_AVERAGE:
+ for (int ii = 0; ii < num_echos; ii++)
+ sum += buffer[ii];
+ sum /= static_cast<float>(num_echos);
+ *dest = sum;
+ break;
+ case ME_OFF:
+ default:
+ *dest = buffer[0];
+ break;
+ }
+}
+
+
+void Sensor::_read_2_byte_range_data(ScanData* data, unsigned int num_steps)
+{
+ if(_verbose)
+ {
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges.\n";
}
@@ -1926,45 +1960,45 @@
// 2 byte data is easy since it fits neatly in a 64-byte block
char buffer[SCIP2_LINE_LENGTH];
- unsigned int currentStep = 0;
+ unsigned int current_step = 0;
int numBytesInLine = 0;
- while (true)
+ while(true)
{
// Read a line of data
numBytesInLine = _read_line_with_check(buffer);
// Check if we've reached the end of the data
- if (numBytesInLine == 0)
+ if(numBytesInLine == 0)
break;
// Process pairs of bytes until we encounter the end of the line
- for (int ii = 0; ii < numBytesInLine; ii += 2, currentStep++)
+ for (int ii = 0; ii < numBytesInLine; ii += 2, current_step++)
{
- if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+ if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a data block? Why?
throw MisplacedLineFeedError();
}
- data->_ranges[currentStep] = decode_2_byte_value(&buffer[ii]);
- if (data->_ranges[currentStep] < 20)
+ data->_ranges[current_step] = decode_2_byte_value(&buffer[ii]);
+ if(data->_ranges[current_step] < 20)
data->_error = true;
}
// End of this line. Go around again.
}
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ << "() Read " <<
- currentStep << " ranges.\n";
- if (currentStep != num_steps)
+ current_step << " ranges.\n";
+ if(current_step != num_steps)
throw DataCountError();
}
-void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
+void Sensor::_read_3_byte_range_data(ScanData* data, unsigned int num_steps)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges.\n";
- if (_multiecho_mode != ME_OFF)
+ if(_multiecho_mode != ME_OFF)
{
_err_output << "Sensor::" << __func__ <<
"() Multi-echo mode is set to " <<
@@ -1981,99 +2015,132 @@
// 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;
- int numBytesInLine = 0, splitCount = 0;
- char splitValue[3];
- while (true)
+ unsigned int current_step = 0;
+ int numBytesInLine = 0, split_count = 0;
+ char split_value[3];
+ int echo_buffer[3] = {-1, -1, -1};
+ int echo_buf_ind = 0;
+ while(true)
{
// Read a line of data
numBytesInLine = _read_line_with_check(buffer);
// Check if we've reached the end of the data
- if (numBytesInLine == 0)
+ if(numBytesInLine == 0)
+ {
+ if(echo_buffer[0] != -1)
+ {
+ // Not the first value, so deal with the previous
+ _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+ &data->_ranges[current_step]);
+ if(data->_ranges[current_step] > _max_range)
+ {
+ _err_output << "WARNING: Sensor::" << __func__ <<
+ "() Value at step " << current_step <<
+ " beyond maximum range: " <<
+ data->_ranges[current_step];
+ }
+ else if(data->_ranges[current_step] < 20)
+ data->_error = true;
+ current_step++;
+ }
break;
+ }
// Process triplets of bytes until we encounter or overrun the end of
// the line
for (int ii = 0; ii < numBytesInLine;)
{
- if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+ if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
throw MisplacedLineFeedError();
}
- if (ii == numBytesInLine - 2) // Short 1 byte
+ if(split_count == 0)
{
- splitValue[0] = buffer[ii];
- splitValue[1] = buffer[ii + 1];
+ // Start of a value. Decide where to store the next value, and
+ // if the previous is complete.
+ if(buffer[ii] == '&')
+ {
+ // Next echo
+ echo_buf_ind++;
+ ii++;
+ }
+ else if(echo_buffer[0] != -1)
+ {
+ // Not the first value, so deal with the previous
+ _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+ &data->_ranges[current_step]);
+ if(data->_ranges[current_step] > _max_range)
+ {
+ _err_output << "WARNING: Sensor::" << __func__ <<
+ "() Value at step " << current_step <<
+ " beyond maximum range: " <<
+ data->_ranges[current_step];
+ }
+ else if(data->_ranges[current_step] < 20)
+ data->_error = true;
+ current_step++;
+ echo_buf_ind = 0;
+ echo_buffer[0] = -1;
+ }
+ }
+ if(ii == numBytesInLine - 2) // Short 1 byte
+ {
+ split_value[0] = buffer[ii];
+ split_value[1] = buffer[ii + 1];
// Will be reset on the next iteration, after it's used
- splitCount = 1;
+ split_count = 1;
ii += 2;
}
- else if (ii == numBytesInLine - 1) // Short 2 bytes
+ else if(ii == numBytesInLine - 1) // Short 2 bytes
{
- splitValue[0] = buffer[ii];
+ split_value[0] = buffer[ii];
// Will be reset on the next iteration, after it's used
- splitCount = 2;
+ split_count = 2;
ii += 1;
}
else
{
- if (splitCount == 1)
+ if(split_count == 1)
{
- splitValue[2] = buffer[ii++];
- data->_ranges[currentStep] =
- decode_3_byte_value(splitValue);
+ split_value[2] = buffer[ii++];
+ echo_buffer[echo_buf_ind] =
+ decode_3_byte_value(split_value);
}
- else if (splitCount == 2)
+ else if(split_count == 2)
{
- splitValue[1] = buffer[ii++];
- splitValue[2] = buffer[ii++];
- data->_ranges[currentStep] =
- decode_3_byte_value(splitValue);
+ split_value[1] = buffer[ii++];
+ split_value[2] = buffer[ii++];
+ echo_buffer[echo_buf_ind] =
+ decode_3_byte_value(split_value);
}
else
{
- data->_ranges[currentStep] =
+ echo_buffer[echo_buf_ind] =
decode_3_byte_value(&buffer[ii]);
ii += 3;
}
- if (data->_ranges[currentStep] > _max_range)
- {
- _err_output << "WARNING: Sensor::" << __func__ <<
- "() Value at step " << currentStep <<
- " beyond maximum range: " <<
- data->_ranges[currentStep] << " (raw bytes: ";
- if (splitCount != 0)
- _err_output << splitValue[0] << splitValue[1] <<
- splitValue[2] << ")\n";
- else
- _err_output << buffer[0] << buffer[1] << buffer[2] <<
- ")\n";
- }
- else if (data->_ranges[currentStep] < 20)
- data->_error = true;
- currentStep++;
- splitCount = 0; // Reset this here now that it's been used
+ split_count = 0; // Reset this here now that it's been used
}
}
// End of this line. Go around again.
}
- if (_verbose)
+ if(_verbose)
_err_output << "Sensor::" << __func__ << "() Read " <<
- currentStep << " ranges.\n";
- if (currentStep != num_steps)
+ current_step << " ranges.\n";
+ if(current_step != num_steps)
throw DataCountError();
}
-void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
+void Sensor::_read_3_byte_range_and_intensity_data(ScanData* data,
unsigned int num_steps)
{
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges and intensities.\n";
- if (_multiecho_mode != ME_OFF)
+ if(_multiecho_mode != ME_OFF)
{
_err_output << "Sensor::" << __func__ <<
"() Multi-echo mode is set to " <<
@@ -2089,115 +2156,140 @@
// 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 currentRange = 0, currentIntensity = 0;
- int numBytesInLine = 0, splitCount = 0;
- char splitValue[3];
+ unsigned int current_range = 0, current_intensity = 0;
+ int numBytesInLine = 0, split_count = 0;
+ char split_value[3];
bool nextIsIntensity = false;
- while (true)
+ int echo_buffer[3] = {-1, -1, -1};
+ int echo_buf_ind = 0;
+ while(true)
{
// Read a line of data
numBytesInLine = _read_line_with_check(buffer);
// Check if we've reached the end of the data
- if (numBytesInLine == 0)
+ if(numBytesInLine == 0)
+ {
+ // The last piece should always be an intensity value (if it isn't
+ // then the data count won't add up and an error will be thrown
+ // below anyway).
+ if(echo_buffer[0] != -1)
+ {
+ assert(nextIsIntensity == true);
+ _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+ &data->_intensities[current_intensity]);
+ current_intensity++;
+ }
break;
+ }
// Process triplets of bytes until we encounter or overrun the end of
// the line
for (int ii = 0; ii < numBytesInLine;)
{
- if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+ if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
throw MisplacedLineFeedError();
}
- if (ii == numBytesInLine - 2) // Short 1 byte
+ if(split_count == 0)
{
- splitValue[0] = buffer[ii];
- splitValue[1] = buffer[ii + 1];
+ // Start of a value. Decide where to store the next value, and
+ // if the previous is complete.
+ if(buffer[ii] == '&')
+ {
+ // Next echo
+ echo_buf_ind++;
+ ii++;
+ }
+ else if(echo_buffer[0] != -1)
+ {
+ // Not the first value, so deal with the previous
+ uint32_t* dest;
+ if(nextIsIntensity)
+ dest = &data->_intensities[current_intensity];
+ else
+ dest = &data->_ranges[current_range];
+ _process_echo_buffer(echo_buffer, echo_buf_ind + 1, dest);
+ echo_buf_ind = 0;
+ echo_buffer[0] = -1;
+ if(data->_ranges[current_range] > _max_range &&
+ !nextIsIntensity)
+ {
+ _err_output << "WARNING: Sensor::" << __func__ <<
+ "() Value at step " << current_range <<
+ " beyond maximum range: " <<
+ data->_ranges[current_range] << " (raw bytes: ";
+ if(split_count != 0)
+ _err_output << split_value[0] << split_value[1] <<
+ split_value[2] << ")\n";
+ else
+ _err_output << buffer[0] << buffer[1] << buffer[2] <<
+ ")\n";
+ }
+ else if(data->_ranges[current_range] < 20)
+ data->_error = true;
+ if(nextIsIntensity)
+ current_intensity++;
+ else
+ current_range++;
+ // Alternate between range and intensity values
+ nextIsIntensity = !nextIsIntensity;
+ }
+ }
+ if(ii == numBytesInLine - 2) // Short 1 byte
+ {
+ split_value[0] = buffer[ii];
+ split_value[1] = buffer[ii + 1];
// Will be reset on the next iteration, after it's used
- splitCount = 1;
+ split_count = 1;
ii += 2;
}
- else if (ii == numBytesInLine - 1) // Short 2 bytes
+ else if(ii == numBytesInLine - 1) // Short 2 bytes
{
- splitValue[0] = buffer[ii];
+ split_value[0] = buffer[ii];
// Will be reset on the next iteration, after it's used
- splitCount = 2;
+ split_count = 2;
ii += 1;
}
else
{
- if (splitCount == 1)
+ if(split_count == 1)
{
- splitValue[2] = buffer[ii++];
- if (nextIsIntensity)
- data->_intensities[currentIntensity] =
- decode_3_byte_value(splitValue);
- else
- data->_ranges[currentRange] =
- decode_3_byte_value(splitValue);
+ split_value[2] = buffer[ii++];
+ echo_buffer[echo_buf_ind] =
+ decode_3_byte_value(split_value);
}
- else if (splitCount == 2)
+ else if(split_count == 2)
{
- splitValue[1] = buffer[ii++];
- splitValue[2] = buffer[ii++];
- if (nextIsIntensity)
- data->_intensities[currentIntensity] =
- decode_3_byte_value(splitValue);
- else
- data->_ranges[currentRange] =
- decode_3_byte_value(splitValue);
+ split_value[1] = buffer[ii++];
+ split_value[2] = buffer[ii++];
+ echo_buffer[echo_buf_ind] =
+ decode_3_byte_value(split_value);
}
else
{
- if (nextIsIntensity)
- data->_intensities[currentIntensity] =
- decode_3_byte_value(&buffer[ii]);
- else
- data->_ranges[currentRange] =
- decode_3_byte_value(&buffer[ii]);
+ echo_buffer[echo_buf_ind] =
+ decode_3_byte_value(&buffer[ii]);
ii += 3;
}
- if (data->_ranges[currentRange] > _max_range &&
- !nextIsIntensity)
- {
- _err_output << "WARNING: Sensor::" << __func__ <<
- "() Value at step " << currentRange <<
- " beyond maximum range: " <<
- data->_ranges[currentRange] << " (raw bytes: ";
- if (splitCount != 0)
- _err_output << splitValue[0] << splitValue[1] <<
- splitValue[2] << ")\n";
- else
- _err_output << buffer[0] << buffer[1] << buffer[2] <<
- ")\n";
- }
- else if (data->_ranges[currentRange] < 20)
- data->_error = true;
- if (nextIsIntensity)
- currentIntensity++;
- else
- currentRange++;
// Reset this here now that it's been used
- splitCount = 0;
- // Alternate between range and intensity values
- nextIsIntensity = !nextIsIntensity;
+ split_count = 0;
}
}
// End of this line. Go around again.
}
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ << "() Read " <<
- currentRange << " ranges and " << currentIntensity <<
+ current_range << " ranges and " << current_intensity <<
" intensities (expected " << num_steps << ").\n";
}
- if (currentRange != num_steps || currentIntensity != num_steps)
+ if(current_range != num_steps || current_intensity != num_steps)
throw DataCountError();
}
-int Sensor::_confirm_checksum(const char *buffer, int length,
+int Sensor::_confirm_checksum(char const* buffer, int length,
int expected_sum)
{
int checksum = 0;
@@ -2209,7 +2301,7 @@
// Add 0x30
checksum += 0x30;
- if (_verbose)
+ if(_verbose)
{
_err_output << "Sensor::" << __func__ <<
"() Calculated checksum = " << checksum << " (" <<
@@ -2217,7 +2309,7 @@
expected_sum << " (" << static_cast<char> (expected_sum) <<
")\n";
}
- if (checksum != expected_sum)
+ if(checksum != expected_sum)
throw ChecksumError(expected_sum, checksum);
return checksum;
Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
/*
* GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
* http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
*
* hokuyo_aist Hokuyo laser scanner driver.
*
@@ -50,11 +50,13 @@
/// The Tough-URG features multiecho detection capability. To use this, set
/// the sensor to use any mode other than ME_OFF.
/// The sensor can register up to three echos for a single reading. The
-/// multiecho mode determines how these are combined into a single value. If
-/// ME_FRONT is set, only the closest reading will be used. If ME_MIDDLE is
-/// set, only the middle reading will be used, or the closest reading if there
-/// are only two echos. If ME_REAR is set, the furthest reading will be used.
-/// Setting ME_AVERAGE will use the average of all two or three echos.
+/// multiecho mode determines how these are combined into a single value:
+/// - ME_FRONT: only the closest reading will be used.
+/// - ME_MIDDLE: the middle reading will be used, or the closest
+/// reading if there are only two echos.
+/// - ME_REAR: the furthest reading will be used.
+/// - ME_AVERAGE: the average of all two or three echos will be used.
+/// In all cases, if there is only one echo, then this setting has no effect.
enum MultiechoMode
{
ME_OFF,
@@ -187,7 +189,7 @@
/** @brief Get various information about the scanner.
Much of the information is not available with the SCIP v1 protocol. */
- void get_sensor_info(SensorInfo *info);
+ void get_sensor_info(SensorInfo* info);
/** @brief Get the value of the scanner's clock in milliseconds.
@@ -216,7 +218,7 @@
@param end_step The last step to get ranges from. Set to -1 for the last
scannable step.
@return The number of range readings read into @ref data. */
- unsigned int get_ranges(ScanData *data, int start_step = -1,
+ unsigned int get_ranges(ScanData* data, int start_step = -1,
int end_step = -1, unsigned int cluster_count = 1);
/** @brief Get the latest scan data from the scanner.
@@ -233,7 +235,7 @@
single reading. The minimum value from a cluster is returned as the
range for that cluster.
@return The number of range readings read into @ref data. */
- unsigned int get_ranges_by_angle(ScanData *data, double start_angle,
+ unsigned int get_ranges_by_angle(ScanData* data, double start_angle,
double end_angle, unsigned int cluster_count = 1);
/** @brief Get the latest scan data from the scanner with intensities.
@@ -258,7 +260,7 @@
@param end_step The last step to get ranges from. Set to -1 for the last
scannable step.
@return The number of range readings read into @ref data. */
- unsigned int get_ranges_intensities(ScanData *data,
+ unsigned int get_ranges_intensities(ScanData* data,
int start_step = -1, int end_step = -1,
unsigned int cluster_count = 1);
@@ -276,7 +278,7 @@
single reading. The minimum value from a cluster is returned as the
range for that cluster.
@return The number of range readings read into @ref data. */
- unsigned int get_ranges_intensities_by_angle(ScanData *data,
+ unsigned int get_ranges_intensities_by_angle(ScanData* data,
double start_angle, double end_angle,
unsigned int cluster_count = 1);
@@ -308,7 +310,7 @@
@param end_step The last step to get ranges from. Set to -1 for the last
scannable step.
@return The number of range readings read into @ref data. */
- unsigned int get_new_ranges(ScanData *data, int start_step = -1,
+ unsigned int get_new_ranges(ScanData* data, int start_step = -1,
int end_step = -1, unsigned int cluster_count = 1);
/** @brief Get a new scan from the scanner.
@@ -327,7 +329,7 @@
single reading. The minimum value from a cluster is returned as the
range for that cluster.
@return The number of range readings read into @ref data. */
- unsigned int get_new_ranges_by_angle(ScanData *data,
+ unsigned int get_new_ranges_by_angle(ScanData* data,
double start_angle, double end_angle,
unsigned int cluster_count = 1);
@@ -358,7 +360,7 @@
@param end_step The last step to get ranges from. Set to -1 for the last
scannable step.
@return The number of range readings read into @ref data. */
- unsigned int get_new_ranges_intensities(ScanData *data,
+ unsigned int get_new_ranges_intensities(ScanData* data,
int start_step = -1, int end_step = -1,
unsigned int cluster_count = 1);
@@ -378,7 +380,7 @@
single reading. The minimum value from a cluster is returned as the
range for that cluster.
@return The number of range readings read into @ref data. */
- unsigned int get_new_ranges_intensities_by_angle(ScanData *data,
+ unsigned int get_new_ranges_intensities_by_angle(ScanData* data,
double start_angle, double end_angle,
unsigned int cluster_count = 1);
@@ -403,7 +405,7 @@
unsigned int angle_to_step(double angle);
private:
- flexiport::Port *_port;
+ flexiport::Port* _port;
std::ostream& _err_output;
uint8_t _scip_version;
@@ -416,25 +418,27 @@
unsigned int _max_range;
void _clear_read_buffer();
- int _read_line(char *buffer, int expected_length = -1);
- int _read_line_with_check(char *buffer, int expected_length = -1,
+ int _read_line(char* buffer, int expected_length = -1);
+ int _read_line_with_check(char* buffer, int expected_length = -1,
bool has_semicolon = false);
void _skip_lines(int count);
- int _send_command(const char *cmd, const char *param, int param_length,
- const char *extra_ok);
+ int _send_command(char const* cmd, char const* param, int param_length,
+ char const* extra_ok);
@@ Diff output truncated at 100000 characters. @@
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|