Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[507] gearbox/trunk/src/hokuyo_aist
|
From: <gb...@us...> - 2010-08-05 01:26:53
|
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 work...
[truncated message content] |