Menu
▾
▴
gearbox-commit — Commit messages to the Gearbox SVN repository.
You can subscribe to this list here.
| 2008 |
Jan
|
Feb
(58) |
Mar
(15) |
Apr
(23) |
May
(8) |
Jun
(92) |
Jul
(66) |
Aug
(6) |
Sep
(9) |
Oct
(44) |
Nov
(8) |
Dec
(1) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(10) |
Feb
(8) |
Mar
(2) |
Apr
(8) |
May
(19) |
Jun
(11) |
Jul
(8) |
Aug
(6) |
Sep
(5) |
Oct
(4) |
Nov
(26) |
Dec
(4) |
| 2010 |
Jan
(5) |
Feb
(3) |
Mar
(4) |
Apr
(3) |
May
(4) |
Jun
|
Jul
(1) |
Aug
(16) |
Sep
(7) |
Oct
(3) |
Nov
(7) |
Dec
|
| 2011 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
(1) |
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
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] |
|
From: <gb...@us...> - 2010-08-04 10:42:51
|
Revision: 506
http://gearbox.svn.sourceforge.net/gearbox/?rev=506&view=rev
Author: gbiggs
Date: 2010-08-04 10:42:44 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Added GE, RT, $IP commands, find sensor model
Modified Paths:
--------------
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/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
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 10:42:44 UTC (rev 506)
@@ -81,7 +81,8 @@
// return ss.str ();
// }
// }
- else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+ else if (((cmd[0] == 'G' || cmd[0] == 'H') &&
+ (cmd[1] == 'D' || cmd[1] == 'E')) ||
(cmd[0] == 'G' && cmd[1] == 'S'))
{
switch (error_code)
@@ -108,7 +109,8 @@
return ss.str();
}
}
- else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+ else if (((cmd[0] == 'M' || cmd[0] == 'N') &&
+ (cmd[1] == 'D' || cmd[1] == 'E')) ||
(cmd[0] == 'M' && cmd[1] == 'S'))
{
switch (error_code)
@@ -245,6 +247,22 @@
// return "";
// }
// }
+ else if (cmd[0] == 'D' && cmd[1] == 'B')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Parameter error";
+ case 2:
+ return "Already running in the set mode";
+ case 3:
+ return "Already returned to normal mode";
+ case 4:
+ return "Selected mode does not match the SCIP version in use";
+ case 5:
+ return "Sensor has a physical malfunction";
+ }
+ }
else
{
ss << "Unknown command: " << cmd[0] << cmd[1];
@@ -261,41 +279,44 @@
std::string desc_code_to_string(unsigned int code)
{
static char const* const descriptions[] = {
- "Timed out trying to read a line",
- "No data received when trying to read a line.",
- "Invalid data index.",
- "Port is not open.",
- "Unknown SCIP version.",
- "Cannot change baud rate of non-serial connection.",
- "Bad baud rate: ",
- "SCIP version 1 does not support the reset command.",
- "SCIP version 1 does not support the set motor speed command.",
- "Invalid motor speed.",
- "SCIP version 1 does not support the high sensitivity command.",
- "No info object provided.",
- "SCIP version 1 does not support the get time command.",
- "No data received. Check data error code.",
- "Start step is out of range.",
- "End step is out of range.",
- "SCIP version 1 does not support the get new ranges command.",
- "SCIP version 1 does not support the get new ranges and intensities command.",
- "Timed out while skipping.",
- "Failed to write command byte.",
- "Failed to write command parameters.",
- "Failed to write termination character.",
- "SCIP versions 1 and 2 failed.",
- "Out-of-range firmware version.",
- "Invalid checksum: ",
- "Read a different number of range or intensity readings than were asked for.",
- "Found line feed in a data block.",
- "Unknown line: ",
- "Parse error: ",
- "'FIRM:' was not found when checking firmware version.",
- "Bad response.",
- "Incorrect command echo.",
- "Incorrect parameters echo for command.",
- "Not enough bytes to calculate checksum.",
- "Incorrect line length received.",
+/* 0 */ "Timed out trying to read a line",
+/* 1 */ "No data received when trying to read a line.",
+/* 2 */ "Invalid data index.",
+/* 3 */ "Port is not open.",
+/* 4 */ "Unknown SCIP version.",
+/* 5 */ "Cannot change baud rate of non-serial connection.",
+/* 6 */ "Bad baud rate: ",
+/* 7 */ "SCIP version 1 does not support the reset command.",
+/* 8 */ "SCIP version 1 does not support the set motor speed command.",
+/* 9 */ "Invalid motor speed.",
+/* 10 */ "SCIP version 1 does not support the high sensitivity command.",
+/* 11 */ "No info object provided.",
+/* 12 */ "SCIP version 1 does not support the get time command.",
+/* 13 */ "No data received. Check data error code.",
+/* 14 */ "Start step is out of range.",
+/* 15 */ "End step is out of range.",
+/* 16 */ "SCIP version 1 does not support the get new ranges command.",
+/* 17 */ "SCIP version 1 does not support the get new ranges and intensities command.",
+/* 18 */ "Timed out while skipping.",
+/* 19 */ "Failed to write command byte.",
+/* 20 */ "Failed to write command parameters.",
+/* 21 */ "Failed to write termination character.",
+/* 22 */ "SCIP versions 1 and 2 failed.",
+/* 23 */ "Out-of-range firmware version.",
+/* 24 */ "Invalid checksum: ",
+/* 25 */ "Read a different number of range or intensity readings than were asked for.",
+/* 26 */ "Found line feed in a data block.",
+/* 27 */ "Unknown line: ",
+/* 28 */ "Parse error: ",
+/* 29 */ "'FIRM:' was not found when checking firmware version.",
+/* 30 */ "Bad response.",
+/* 31 */ "Incorrect command echo.",
+/* 32 */ "Incorrect parameters echo for command.",
+/* 33 */ "Not enough bytes to calculate checksum.",
+/* 34 */ "Incorrect line length received.",
+/* 35 */ "SCIP version 1 does not support the semi-reset command.",
+/* 36 */ "SCIP version 1 does not support the get ranges and intensities command.",
+/* 37 */ "Error configuring IP address.",
};
return std::string(descriptions[code]);
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 10:42:44 UTC (rev 506)
@@ -281,6 +281,16 @@
}; // class IndexError
+/// Set IP error class
+class HOKUYO_AIST_EXPORT SetIPError: public RuntimeError
+{
+ public:
+ SetIPError()
+ : RuntimeError(37, "SetIPError")
+ {}
+}; // class SetIPError
+
+
/// Invalid motor speed error class
class HOKUYO_AIST_EXPORT MotorSpeedError: public ArgError
{
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 10:42:44 UTC (rev 506)
@@ -36,6 +36,7 @@
{
}
+
ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
unsigned int time, LaserModel model)
: _error(error), _time(time), _model(model)
@@ -63,6 +64,7 @@
}
}
+
ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
unsigned int length, bool error, unsigned int time,
LaserModel model)
@@ -92,6 +94,7 @@
}
}
+
ScanData::ScanData(const ScanData &rhs)
{
_length = rhs.length();
@@ -122,12 +125,14 @@
_model = rhs.model();
}
+
ScanData::~ScanData()
{
if(_ranges != NULL)
delete[] _ranges;
}
+
std::string ScanData::error_code_to_string(uint32_t error_code)
{
if (_model == MODEL_UTM30LX)
@@ -204,6 +209,7 @@
}
}
+
ScanData& ScanData::operator=(const ScanData &rhs)
{
if (rhs.length() == 0)
@@ -274,6 +280,7 @@
return *this;
}
+
uint32_t ScanData::operator[](unsigned int index)
{
if (index >= _length)
@@ -281,6 +288,7 @@
return _ranges[index];
}
+
std::string ScanData::as_string()
{
std::stringstream ss;
@@ -313,6 +321,7 @@
return ss.str();
}
+
void ScanData::clean_up()
{
if (_ranges != NULL)
@@ -326,6 +335,7 @@
_time = 0;
}
+
void ScanData::allocate_data(unsigned int length, bool include_intensities)
{
// If no data yet, allocate new
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 10:42:44 UTC (rev 506)
@@ -35,6 +35,7 @@
#include <math.h>
#include <sstream>
#include <iostream>
+#include <iomanip>
#if defined(WIN32)
#define __func__ __FUNCTION__
@@ -80,7 +81,6 @@
2 byte command block
*/
-///////////////////////////////////////////////////////////////////////////////
// SCIP protocol version 2 notes
///////////////////////////////////////////////////////////////////////////////
@@ -96,7 +96,7 @@
and a line feed.
- Status codes 00 and 99 are OK, anything else is an error.
- Checksum is calculated by... well, see the code.
- - BIG NOTE: The URG-30LX has a probable bug in its response to the II code
+ - BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
whereby it does not include anything after and including "<-" in the
checksum calculation.
@@ -107,8 +107,8 @@
3 byte command block
PP Specification info
P|P|LF
- P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|
- MinRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
+ P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MinRange...|;|Sum|LF|
+ MaxRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF
3 byte command block
II Status info
@@ -131,41 +131,82 @@
9 byte command block
MDMS Get new data
M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|
- LF|Data...|LF|LF
+ M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+ Status(2)|Sum|LF|Data...|LF|LF
16 byte command block
ME Get new data, including intensity data
M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|
- Data...|LF|LF
+ M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+ Status(2)|Sum|LF|Data...|LF|LF
16 byte command block
- Featured only on UTM-30LX for getting intensity data.
GDGS Get latest data
G|D/S|Start(4)|End(4)|Cluster(2)|LF
G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
12 byte command block
+GE Get latest data, including intensity data
+ G|E|Start(4)|End(4)|Cluster(2)|LF
+ G|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ 12 byte command block
CR Set motor speed
C|R|Speed(2)|LF
C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
5 byte command block
TM Get sensor time
T|M|Code|LF
- T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
+ T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF
4 byte command block
Optional part only comes back for control code 1.
RS Reset
R|S|LF
R|S|LF|Status(2)|Sum|LF|LF
3 byte command block
-GE
-ND
-NE
-HD
-HE
-HS
-$IP
-RT
-DB
+RT Same as reset, but does not stop the motor or alter serial speed
+ R|T|LF
+ R|T|LF|Status(2)|Sum|LF|LF
+ND Multi-echo version of MD
+ N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
+ N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+ Status(2)|Sum|LF|Data...|LF|LF
+ 16 byte command block
+ Returned scan data may include echo data separated by '&'.
+NE Multi-echo version of ME
+ N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
+ N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+ Status(2)|Sum|LF|Data...|LF|LF
+ 16 byte command block
+ Returned scan data may include echo data separated by '&'.
+HD Multi-echo version of GD
+ H|D|Start(4)|End(4)|Cluster(2)|LF
+ H|D|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ 12 byte command block
+ Returned scan data may include echo data separated by '&'.
+HE Multi-echo version of GE
+ H|E|Start(4)|End(4)|Cluster(2)|LF
+ H|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ 12 byte command block
+ Returned scan data may include echo data separated by '&'.
+HS Set high sensitivity mode
+ H|S|On/Off(1)|LF
+ H|S|On/Off(1)|LF|Status(2)|Sum|LF|LF
+ 3 byte command block
+ Use '0' for on, '1' for off.
+$IP Change IP address settings
+ $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF
+ $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF|Status(2)|
+ Sum|LF|LF
+ Only available on models supporting ethernet connections.
+ Does not conform to standard command format.
+ Laser will require a restart after use (wait for response first).
+DB Simulate sensor failure modes
+ D|B|Command(2)|LF
+ D|B|Command(2)|LF|Status(2)|Sum|LF|LF
+ Where command is one of:
+ "02" Enter failure state.
+ "03" While sending data, go through normal -> malfunction -> normal.
+ "04" While sending data, go through normal -> malfunction -> failure.
+ "05" While sending data, go through normal -> failure.
+ "10" Return to normal operation.
+ 4 byte command block
*/
///////////////////////////////////////////////////////////////////////////////
@@ -182,6 +223,7 @@
return (byte1 << 6) + (byte2);
}
+
unsigned int decode_3_byte_value(char *data)
{
unsigned int byte1, byte2, byte3;
@@ -193,6 +235,7 @@
return (byte1 << 12) + (byte2 << 6) + (byte3);
}
+
unsigned int decode_4_byte_value(char *data)
{
unsigned int byte1, byte2, byte3, byte4;
@@ -205,7 +248,8 @@
return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
}
-void number_to_string(unsigned int num, char *dest, int length)
+
+void number_to_string(unsigned int num, char* dest, int length)
{
#if defined(WIN32)
_snprintf(dest, length + 1, "%*d", length, num);
@@ -217,6 +261,7 @@
dest[ii] = '0';
}
+
///////////////////////////////////////////////////////////////////////////////
// Sensor class
///////////////////////////////////////////////////////////////////////////////
@@ -225,27 +270,31 @@
///////////////////////////////////////////////////////////////////////////////
Sensor::Sensor()
- : _port(NULL), _err_output(std::cerr), _scip_version(2),
- _verbose(false), _enable_checksum_workaround(false),
- _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+ : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
+ _enable_checksum_workaround(false), _ignore_unknowns(false),
+ _multiecho_mode(ME_OFF), _min_angle(0.0), _max_angle(0.0),
_resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
}
+
Sensor::Sensor(std::ostream& err_output)
: _port(NULL), _err_output(err_output), _scip_version(2),
_verbose(false), _enable_checksum_workaround(false),
- _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
- _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
+ _ignore_unknowns(false), _multiecho_mode(ME_OFF), _min_angle(0.0),
+ _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
+ _front_step(0)
{
}
+
Sensor::~Sensor()
{
if (_port != NULL)
delete _port;
}
+
void Sensor::open(std::string port_options)
{
if (_verbose)
@@ -272,6 +321,7 @@
_get_defaults();
}
+
unsigned int Sensor::open_with_probing(std::string port_options)
{
if (_verbose)
@@ -362,6 +412,7 @@
return 0;
}
+
void Sensor::close()
{
if (!_port)
@@ -372,6 +423,7 @@
_port = NULL;
}
+
bool Sensor::is_open() const
{
if (_port != NULL)
@@ -379,6 +431,7 @@
return false;
}
+
void Sensor::set_power(bool on)
{
if (_scip_version == 1)
@@ -421,6 +474,7 @@
throw UnknownScipVersionError();
}
+
// This function assumes that both the port and the laser scanner are already
// set to the same baud.
void Sensor::set_baud(unsigned int baud)
@@ -460,6 +514,37 @@
throw UnknownScipVersionError();
}
+
+void Sensor::set_ip(IPAddr const& addr, IPAddr const& subnet,
+ IPAddr const& gateway)
+{
+ // Command is "$IP"(3) + IP(15) + ' ' + subnet(15) + ' ' + gateway(15),
+ // +1 for the null byte, but split two bytes off the front for sending.
+ // Hijack the _send_command function, treating the rest of the command as
+ // parameters.
+ char const command[3] = "$I";
+ std::stringstream params;
+ params << 'P';
+ params << std::setw(3) << std::setfill('0');
+ params << addr.first << '.' << addr.second << '.' << addr.third << '.' <<
+ addr.fourth << ' ';
+ params << subnet.first << '.' << subnet.second << '.' << subnet.third <<
+ '.' << subnet.fourth << ' ';
+ params << gateway.first << '.' << gateway.second << '.' << gateway.third <<
+ '.' << gateway.fourth;
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ <<
+ "() Setting IP information to $I" << params.str() << '\n';
+ }
+ int status = _send_command(&command[0], params.str().c_str(), 48, NULL);
+ // Skip the extra line feed
+ _skip_lines(1);
+ if (status != 0)
+ throw SetIPError();
+}
+
+
void Sensor::reset()
{
if (_scip_version == 1)
@@ -467,8 +552,10 @@
else if (_scip_version == 2)
{
if (_verbose)
+ {
_err_output << "Sensor::" << __func__ <<
"() Resetting laser.\n";
+ }
_send_command("RS", NULL, 0, NULL);
_skip_lines(1);
}
@@ -476,6 +563,24 @@
throw UnknownScipVersionError();
}
+
+void Sensor::semi_reset()
+{
+ if (_scip_version == 1)
+ throw UnsupportedError(35);
+ else if (_scip_version == 2)
+ {
+ if (_verbose)
+ _err_output << "Sensor::" << __func__ <<
+ "() Resetting laser.\n";
+ _send_command("RS", NULL, 0, NULL);
+ _skip_lines(1);
+ }
+ else
+ throw UnknownScipVersionError();
+}
+
+
void Sensor::set_motor_speed(unsigned int speed)
{
if (_scip_version == 1)
@@ -524,6 +629,7 @@
throw UnknownScipVersionError();
}
+
void Sensor::set_high_sensitivity(bool on)
{
if (_scip_version == 1)
@@ -552,6 +658,7 @@
throw UnknownScipVersionError();
}
+
void Sensor::get_sensor_info(SensorInfo *info)
{
if (info == NULL)
@@ -713,6 +820,7 @@
throw UnknownScipVersionError();
}
+
unsigned int Sensor::get_time()
{
if (_scip_version == 1)
@@ -737,6 +845,7 @@
return 0;
}
+
unsigned int Sensor::get_ranges(ScanData *data, int start_step,
int end_step, unsigned int cluster_count)
{
@@ -778,7 +887,10 @@
number_to_string(start_step, buffer, 4);
number_to_string(end_step, &buffer[4], 4);
number_to_string(cluster_count, &buffer[8], 2);
- _send_command("GD", buffer, 10, NULL);
+ 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.
@@ -795,6 +907,7 @@
return data->_length;
}
+
unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
double end_angle, unsigned int cluster_count)
{
@@ -823,6 +936,87 @@
return get_ranges(data, start_step, end_step, cluster_count);
}
+
+unsigned int Sensor::get_ranges_intensities(ScanData *data, int start_step,
+ int end_step, unsigned int cluster_count)
+{
+ if (data == NULL)
+ throw NoDestinationError();
+
+ char buffer[11];
+ memset(buffer, 0, sizeof(char) * 11);
+
+ if (start_step < 0)
+ start_step = _first_step;
+ if (end_step < 0)
+ end_step = _last_step;
+
+ unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " ranges between " << start_step << " and " <<
+ end_step << " with a cluster count of " << cluster_count <<
+ '\n';
+ }
+
+ if (_scip_version == 1)
+ throw UnsupportedError(36);
+ else if (_scip_version != 2)
+ throw UnknownScipVersionError();
+
+ // Send the command to ask for the most recent 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)
+ _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)
+ throw NoDataError();
+ data->_time = decode_4_byte_value(buffer);
+ // In SCIP2 mode we're going to get back 3-byte data because we're
+ // sending the GE command
+ _read_3_byte_range_data(data, num_steps);
+
+ return data->_length;
+}
+
+
+unsigned int Sensor::get_ranges_intensities_by_angle(ScanData *data,
+ double start_angle, double end_angle, unsigned int cluster_count)
+{
+ if (data == NULL)
+ throw NoDataError();
+
+ // Calculate the given angles in steps, rounding towards _front_step
+ int start_step, end_step;
+ start_step = angle_to_step(start_angle);
+ end_step = angle_to_step(end_angle);
+
+ // Check the steps are within the allowable range
+ if (start_step < _first_step || start_step > _last_step)
+ throw StartStepError();
+ if (end_step < _first_step || end_step > _last_step)
+ throw EndStepError();
+
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ << "() Start angle " <<
+ start_angle << " is step " << start_step << ", end angle " <<
+ end_angle << " is step " << end_step << '\n';
+ }
+
+ // Get the data
+ return get_ranges_intensities(data, start_step, end_step, cluster_count);
+}
+
+
unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
int end_step, unsigned int cluster_count)
{
@@ -831,79 +1025,85 @@
if (_scip_version == 1)
throw UnsupportedError(16);
- else if (_scip_version == 2)
- {
- char buffer[14];
- memset(buffer, 0, sizeof(char) * 14);
+ else if (_scip_version != 2)
+ throw UnknownScipVersionError();
- if (start_step < 0)
- start_step = _first_step;
- if (end_step < 0)
- end_step = _last_step;
+ char buffer[14];
+ memset(buffer, 0, sizeof(char) * 14);
- unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " new ranges between " << start_step << " and " <<
- end_step << " with a cluster count of " << cluster_count <<
- '\n';
- }
+ if (start_step < 0)
+ start_step = _first_step;
+ if (end_step < 0)
+ end_step = _last_step;
- // 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);
- number_to_string(1, &buffer[10], 1);
- number_to_string(1, &buffer[11], 2);
- _send_command("MD", buffer, 13, NULL);
- // Mx commands will perform a scan, then send the data prefixed with
- // another command echo.
- // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
- char response[17];
- _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] != 'M' || response[1] != 'D')
- throw CommandEchoError("MD", response);
- // Then compare the parameters
- buffer[12] = '0'; // There will be zero scans remaining after this one
- if (memcmp(&response[2], buffer, 13) != 0)
- throw ParamEchoError("MD");
- // The next line should be the status line
- _read_line_with_check(response, 4);
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() MD 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')
- {
- // There is an extra line feed after an error status (signalling
- // end of message)
- _skip_lines(1);
- throw ResponseError(response, "MD");
- }
+ unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " new ranges between " << start_step << " and " <<
+ end_step << " with a cluster count of " << cluster_count <<
+ '\n';
+ }
- // Now the actual data will arrive
- // 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)
- throw NoDataError();
- data->_time = decode_4_byte_value(buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're
- // sending the MD command
- _read_3_byte_range_data(data, num_steps);
+ // 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);
+ 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)
+ command[0] = 'N';
+ else
+ command[0] = 'M';
+ command[1] = 'D';
+ command[2] = '\0';
+ _send_command(command, buffer, 13, NULL);
+ // Mx commands will perform a scan, then send the data prefixed with
+ // another command echo.
+ // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
+ char response[17];
+ _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])
+ 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)
+ throw ParamEchoError(command);
+ // The next line should be the status line
+ _read_line_with_check(response, 4);
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ <<
+ "() " << command << " data prefix status: " << response[0] <<
+ response[1] << '\n';
}
- else
- throw UnknownScipVersionError();
+ // Check the status code is OK - should only get 99 here
+ if (response[0] != '9' || response[1] != '9')
+ {
+ // There is an extra line feed after an error status (signalling
+ // end of message)
+ _skip_lines(1);
+ throw ResponseError(response, command);
+ }
+ // Now the actual data will arrive
+ // 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)
+ throw NoDataError();
+ data->_time = decode_4_byte_value(buffer);
+ // In SCIP2 mode we're going to get back 3-byte data because we're
+ // sending the MD command
+ _read_3_byte_range_data(data, num_steps);
+
return data->_length;
}
+
unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
double start_angle, double end_angle, unsigned int cluster_count)
{
@@ -934,6 +1134,7 @@
return get_new_ranges(data, start_step, end_step, cluster_count);
}
+
unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
int start_step, int end_step, unsigned int cluster_count)
{
@@ -942,79 +1143,85 @@
if (_scip_version == 1)
throw UnsupportedError(17);
- else if (_scip_version == 2)
- {
- char buffer[14];
- memset(buffer, 0, sizeof(char) * 14);
+ else if (_scip_version != 2)
+ throw UnknownScipVersionError();
- if (start_step < 0)
- start_step = _first_step;
- if (end_step < 0)
- end_step = _last_step;
+ char buffer[14];
+ memset(buffer, 0, sizeof(char) * 14);
- unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " new ranges between " << start_step << " and " <<
- end_step << " with a cluster count of " << cluster_count <<
- '\n';
- }
+ if (start_step < 0)
+ start_step = _first_step;
+ if (end_step < 0)
+ end_step = _last_step;
- // Send the command to ask for the most recent range data with
- // intensity 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);
- number_to_string(1, &buffer[10], 1);
- number_to_string(1, &buffer[11], 2);
- _send_command("ME", buffer, 13, NULL);
- // Mx commands will perform a scan, then send the data prefixed with
- // another command echo.
- // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
- char response[17];
- _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] != 'M' || response[1] != 'E')
- throw CommandEchoError("ME", response);
- // Then compare the parameters
- if (memcmp(&response[2], buffer, 13) != 0)
- throw ParamEchoError("ME");
- // The next line should be the status line
- _read_line_with_check(response, 4);
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() ME 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')
- {
- // There is an extra line feed after an error status (signalling
- // end of message)
- _skip_lines(1);
- throw ResponseError(response, "ME");
- }
+ unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+ if (_verbose)
+ {
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " new ranges and intensities between " <<
+ start_step << " and " << end_step <<
+ " with a cluster count of " << cluster_count << '\n';
+ }
- // Now the actual data will arrive
- // 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)
- throw NoDataError();
- data->_time = decode_4_byte_value(buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're
- // sending the ME command
- _read_3_byte_range_and_intensity_data(data, num_steps);
+ // Send the command to ask for the most recent range data with
+ // intensity 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);
+ 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)
+ command[0] = 'N';
+ else
+ command[0] = 'M';
+ command[1] = 'E';
+ command[2] = '\0';
+ _send_command(command, buffer, 13, NULL);
+ // Mx commands will perform a scan, then send the data prefixed with
+ // another command echo.
+ // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
+ char response[17];
+ _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])
+ throw CommandEchoError(command, response);
+ // Then compare the parameters
+ 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)
+ {
+ _err_output << "Sensor::" << __func__ <<
+ "() " << command << " data prefix status: " << response[0] <<
+ response[1] << '\n';
}
- else
- throw UnknownScipVersionError();
+ // Check the status code is OK - should only get 99 here
+ if (response[0] != '9' || response[1] != '9')
+ {
+ // There is an extra line feed after an error status (signalling
+ // end of message)
+ _skip_lines(1);
+ throw ResponseError(response, command);
+ }
+ // Now the actual data will arrive
+ // 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)
+ throw NoDataError();
+ data->_time = decode_4_byte_value(buffer);
+ // In SCIP2 mode we're going to get back 3-byte data because we're
+ // sending the ME command
+ _read_3_byte_range_and_intensity_data(data, num_steps);
+
return data->_length;
}
+
unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
double start_angle, double end_angle, unsigned int cluster_count)
{
@@ -1046,12 +1253,14 @@
cluster_count);
}
+
double Sensor::step_to_angle(unsigned int step)
{
return (static_cast<int>(step) - static_cast<int>(_front_step)) *
_resolution;
}
+
unsigned int Sensor::angle_to_step(double angle)
{
unsigned int result;
@@ -1068,6 +1277,7 @@
return result;
}
+
// Private functions
///////////////////////////////////////////////////////////////////////////////
@@ -1084,6 +1294,7 @@
_port->Flush();
}
+
// If expected_length is not -1, it should include the terminating line feed but
// not the NULL (although the buffer still has to include this).
// If expected_length is -1, this function expects buffer to be a certain length
@@ -1141,6 +1352,7 @@
return linelength - 1; // Line feed not included
}
+
// This function will read a line and then calculate its checksum, comparing it
// with the checksum at the end of the line. The checksum will be removed
// (along with the semi-colon, if present). buffer and expected_length args are
@@ -1232,6 +1444,7 @@
return bytesToConsider;
}
+
// Reads lines until the number specified has passed.
void Sensor::_skip_lines(int count)
{
@@ -1242,6 +1455,7 @@
throw ReadError(18);
}
+
// Sends a command with optional parameters and checks that the echo of the
// command and parameters sent are correct, and that the returned status code
// is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two
@@ -1413,6 +1627,10 @@
{
if (strstr(buffer, "URG-04LX") != NULL)
_model = MODEL_URG04LX;
+ else if (strstr(buffer, "UBG-04LX") != NULL)
+ _model = MODEL_UBG04LXF01;
+ else if (strstr(buffer, "UHG-08LX") != NULL)
+ _model = MODEL_UHG08LX;
else if (strstr(buffer, "UTM-30LX") != NULL)
{
_model = MODEL_UTM30LX;
@@ -1420,6 +1638,8 @@
// model.
_enable_checksum_workaround = true;
}
+ else if (strstr(buffer, "UXM-30LX") != NULL)
+ _model = MODEL_UXM30LXE;
else
_model = MODEL_UNKNOWN;
}
@@ -1548,6 +1768,7 @@
throw ScipVersionError();
}
+
void Sensor::_get_defaults()
{
if (_verbose)
@@ -1574,6 +1795,7 @@
}
}
+
void Sensor::_process_vv_line(const char *buffer, SensorInfo *info)
{
if (strncmp(buffer, "VEND", 4) == 0)
@@ -1583,6 +1805,7 @@
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)
info->firmware = &buffer[5]; // Firmware version
@@ -1594,6 +1817,7 @@
throw UnknownLineError(buffer);
}
+
void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
{
if (strncmp(buffer, "MODL", 4) == 0)
@@ -1613,10 +1837,20 @@
info->front_step = atoi(&buffer[5]);
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)
+ {
+ if (strstr(buffer, "CCW") != NULL)
+ info->rot_dir = COUNTERCLOCKWISE;
+ else
+ info->rot_dir = CLOCKWISE;
+ }*/
else if (!_ignore_unknowns)
throw UnknownLineError(buffer);
}
+
void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
{
if (strncmp(buffer, "MODL", 4) == 0)
@@ -1675,11 +1909,14 @@
throw UnknownLineError(buffer);
}
+
void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
{
if (_verbose)
+ {
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges.\n";
+ }
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
@@ -1720,11 +1957,20 @@
throw DataCountError();
}
+
void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
{
if (_verbose)
+ {
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges.\n";
+ if (_multiecho_mode != ME_OFF)
+ {
+ _err_output << "Sensor::" << __func__ <<
+ "() Multi-echo mode is set to " <<
+ multiecho_mode_to_string(_multiecho_mode) << '\n';
+ }
+ }
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
@@ -1819,6 +2065,7 @@
throw DataCountError();
}
+
void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
unsigned int num_steps)
{
@@ -1826,6 +2073,12 @@
{
_err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges and intensities.\n";
+ if (_multiecho_mode != ME_OFF)
+ {
+ _err_output << "Sensor::" << __func__ <<
+ "() Multi-echo mode is set to " <<
+ multiecho_mode_to_string(_multiecho_mode) << '\n';
+ }
}
// This will automatically take care of whether it actually needs to
@@ -1943,6 +2196,7 @@
throw DataCountError();
}
+
int Sensor::_confirm_checksum(const char *buffer, int length,
int expected_sum)
{
Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 10:42:44 UTC (rev 506)
@@ -45,6 +45,60 @@
namespace hokuyo_aist
{
+/// @brief Possible values of the multiecho mode setting.
+///
+/// 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.
+enum MultiechoMode
+{
+ ME_OFF,
+ ME_FRONT,
+ ME_MIDDLE,
+ ME_REAR,
+ ME_AVERAGE
+};
+
+
+HOKUYO_AIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode)
+{
+ switch(mode)
+ {
+ case ME_OFF:
+ return "Off";
+ case ME_FRONT:
+ return "Front";
+ case ME_MIDDLE:
+ return "Middle";
+ case ME_REAR:
+ return "Rear";
+ case ME_AVERAGE:
+ return "Average";
+ default:
+ return "Unknown";
+ }
+}
+
+
+/// Structure to store an IP address.
+typedef struct IPAddr
+{
+ /// First byte
+ unsigned int first;
+ /// Second byte
+ unsigned int second;
+ /// Third byte
+ unsigned int third;
+ /// Fourth byte
+ unsigned int fourth;
+} IPAddr;
+
+
/** @brief Hokuyo laser scanner class.
Provides an interface for interacting with a Hokuyo laser scanner using SCIP
@@ -95,11 +149,25 @@
500.0Kbps, 750.0Kbps (dependent on those available in FlexiPort). */
void set_baud(unsigned int baud);
+ /** @brief Change the IP address information.
+
+ This function only works on devices providing an ethernet connection.
+
+ Once this function has successfully completed, the laser must be
+ restarted for it to take effect. */
+ void set_ip(IPAddr const& addr, IPAddr const& subnet,
+ IPAddr const& gateway);
+
/** @brief Reset the laser scanner to its default settings.
Not available with the SCIP v1 protocol. */
void reset();
+ /** @brief Reset everything except motor and serial speed.
+
+ Requires SCIP v2.1 or higher. */
+ void semi_reset();
+
/** @brief Set the speed at which the scanner's sensor spins.
Set the speed to 0 to have it reset to the default value, and 99 to
@@ -168,6 +236,50 @@
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.
+
+ This function requires a pointer to a @ref ScanData object. It will
+ allocate space in this object as necessary for storing range data. If
+ the passed-in @ref ScanData object already has the correct quantity
+ of space to store the range data, it will not be re-allocated. If it
+ does not have any space, it will be allocated. If it has space, but it
+ is the wrong size, it will be re-allocated. This means you can
+ repeatedly send the same @ref ScanData object without having to worry
+ about allocating its data, whether it will change or not, while also
+ avoiding excessive allocations.
+
+ @param data Pointer to a @ref ScanData object to store the range
+ readings in.
+ @param cluster_count The number of readings to cluster together into a
+ single reading. The minimum value from a cluster is returned as the
+ range for that cluster.
+ @param start_step The first step to get ranges from. Set to -1 for the
+ first scannable step.
+ @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,
+ int start_step = -1, int end_step = -1,
+ unsigned int cluster_count = 1);
+
+ /** @brief Get the latest scan data from the scanner with intensities.
+
+ @param data Pointer to a @ref ScanData object to store the range
+ readings in.
+ @param start_angle The angle to get range readings from. Exclusive; if
+ this falls between two steps the step inside the angle will be
+ returned, but the step outside won't.
+ @param end_angle The angle to get range readings to. Exclusive; if this
+ falls between two steps the step inside the angle will be returned, but
+ the step outside won't.
+ @param cluster_count The number of readings to cluster together into a
+ 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,
+ double start_angle, double end_angle,
+ unsigned int cluster_count = 1);
+
/** @brief Get a new scan from the scanner.
Unlike @ref get_ranges, which returns the most recent scan the scanner
@@ -281,6 +393,9 @@
messages. Default is off. */
void ignore_unknowns(bool ignore) { _ignore_unknowns = ignore; }
+ /** @brief Set the multi-echo mode to use. Default is ME_OFF. */
+ void set_multiecho_mode(MultiechoMode mode) { _multiecho_mode = mode; }
+
/// @brief A convenience function to convert a step index to an angle.
double step_to_angle(unsigned int step);
/** @brief A convenience function to convert an angle to a step
@@ -295,6 +410,7 @@
LaserModel _model;
bool _verbose, _enable_checksum_workaround,
_ignore_unknowns;
+ MultiechoMode _multiecho_mode;
double _min_angle, _max_angle, _resolution;
int _first_step, _last_step, _front_step;
unsigned int _max_range;
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 10:42:44 UTC (rev 506)
@@ -30,23 +30,79 @@
SensorInfo::SensorInfo()
: min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
- front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
- baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
- scanable_steps(0), detected_model(MODEL_UNKNOWN)
+ front_step(0), standard_speed(0), rot_dir(COUNTERCLOCKWISE), power(false),
+ speed(0), speed_level(0), baud(0), time(0), min_angle(0.0), max_angle(0.0),
+ resolution(0.0), scanable_steps(0), max_step(0),
+ detected_model(MODEL_UNKNOWN)
{
}
+
// Set various known values based on what the manual says
void SensorInfo::set_defaults()
{
- min_range = 20;
- max_range = 4095;
- steps = 1024;
- first_step = 44;
- last_step = 725;
- front_step = 384;
+ switch (detected_model)
+ {
+ case MODEL_URG04LX:
+ min_range = 20;
+ max_range = 4095;
+ steps = 1024;
+ first_step = 44;
+ last_step = 725;
+ front_step = 384;
+ max_step = 768;
+ break;
+ case MODEL_UBG04LXF01:
+ min_range = 20;
+ max_range = 4095;
+ steps = 1024;
+ first_step = 44;
+ last_step = 725;
+ front_step = 384;
+ max_step = 768;
+ break;
+ case MODEL_UHG08LX:
+ min_range = 20;
+ max_range = 4095;
+ steps = 1024;
+ first_step = 0;
+ last_step = 768;
+ front_step = 384;
+ max_step = 768;
+ break;
+ case MODEL_UTM30LX:
+ min_range = 20;
+ max_range = 4095;
+ steps = 1440;
+ first_step = 0;
+ last_step = 1080;
+ front_step = 540;
+ max_step = 1080;
+ break;
+ case MODEL_UXM30LXE:
+ min_range = 20;
+ max_range = 4095;
+ steps = 1440;
+ first_step = 0;
+ last_step = 760;
+ front_step = 380;
+ max_step = 760;
+ break;
+ case MODEL_UNKNOWN:
+ default:
+ // Use the URG-04LX settings
+ min_range = 20;
+ max_range = 4095;
+ steps = 1024;
+ first_step = 44;
+ last_step = 725;
+ front_step = 384;
+ max_step = 768;
+ break;
+ }
}
+
void SensorInfo::calculate_values()
{
resolution = DTOR(360.0) / steps;
@@ -59,12 +115,14 @@
scanable_steps = last_step - first_step + 1;
}
+
std::string SensorInfo::as_string()
{
std::stringstream ss;
ss << "Vendor: " << vendor << '\n';
ss << "Product: " << product << '\n';
+ ss << "Identified as: " << model_to_string(detected_model) << '\n';
ss << "Firmware: " << firmware << '\n';
ss << "Protocol: " << protocol << '\n';
ss << "Serial: " << serial << '\n';
@@ -75,11 +133,12 @@
ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
scanable_steps << '\n';
ss << "First step: " << first_step << "\tFront step: " << front_step <<
- "\tLast step: " << last_step << '\n';
+ "\tLast step: " << last_step << "\tMax step: " << max_step << '\n';
ss << "Resolution: " << resolution << " radians/step\n";
ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
max_angle << " radians\n";
ss << "Standard motor speed: " << standard_speed << "rpm\n";
+ ss << "Rotation direction: " << rot_dir_to_string(rot_dir) << '\n';
ss << "Power status: " << (power ? "On" : "Off") <<
"\tMeasurement state: " << measure_state << '\n';
@@ -91,4 +150,3 @@
return ss.str();
}
-
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 10:42:44 UTC (rev 506)
@@ -36,6 +36,7 @@
#endif
#include <string>
+#include <cstring>
/** @ingroup gbx_library_hokuyo_aist
@{
@@ -45,27 +46,80 @@
{
/// Laser models
-enum LaserModel {
- MODEL_URG04LX,
- MODEL_UTM30LX,
+enum LaserModel
+{
+ MODEL_URG04LX, // Classic-URG
+ MODEL_UBG04LXF01, // Rapid-URG
+ MODEL_UHG08LX, // Hi-URG
+ MODEL_UTM30LX, // Top-URG
+ MODEL_UXM30LXE, // Tough-URG
MODEL_UNKNOWN
};
+
HOKUYO_AIST_EXPORT inline char const* model_to_string(LaserModel model)
{
switch(model)
{
case MODEL_URG04LX:
return "URG-04LX";
+ case MODEL_UBG04LXF01:
+ return "UBG-04LX-F01";
+ case MODEL_UHG08LX:
+ return "UHG-08LX";
case MODEL_UTM30LX:
return "UTM-30LX";
+ case MODEL_UXM30LXE:
+ return "UXM-30LX-E";
default:
return "Unknown model";
}
}
+
+HOKUYO_AIST_EXPORT inline LaserModel string_to_model(char const* model)
+{
+ if (strncmp(model, "URG-04LX", 8) == 0)
+ return MODEL_URG04LX;
+ else if (strncmp(model, "UBG-04LX-F01", 8) == 0)
+ return MODEL_UBG04LXF01;
+ else if (strncmp(model, "UHG-08LX", 8) == 0)
+ return MODEL_UHG08LX;
+ else if (strncmp(model, "UTM-30LX", 8) == 0)
+ return MODEL_UTM30LX;
+ else if (strncmp(model, "UXM-30LX-E", 8) == 0)
+ return MODEL_UXM30LXE;
+ else
+ return MODEL_UNKNOWN;
+}
+
+
+/// Sensor direction of rotation
+enum RotationDirection
+{
+ CLOCKWISE,
+ COUNTERCLOCKWISE
+};
+
+
+HOKUYO_AIST_EXPORT inline char const* rot_dir_to_string(RotationDirection dir)
+{
+ switch(dir)
+ {
+ case CLOCKWISE:
+ return "Clockwise";
+ case COUNTERCLOCKWISE:
+ return "Counter-clockwise";
+ default:
+ return "Unknown";
+ }
+}
+
+
+// Forward declaration
class Sensor;
+
/** @brief Sensor information.
Returned from a call to @get_sensor_info. Contains various information about the
@@ -114,6 +168,8 @@
unsigned int front_step;
/// Standard motor speed (rpm).
unsigned int standard_speed;
+ /// Rotation direction.
+ RotationDirection rot_dir;
// Status details.
/// Operational status - illuminated or not.
@@ -142,6 +198,8 @@
double resolution;
/// Total number of steps in a full scan (lastStep - firstStep).
unsigned int scanable_steps;
+ /// Absolute maximum commandable step.
+ unsigned int max_step;
/// Detected model of the laser.
LaserModel detected_model;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-08-04 05:30:30
|
Revision: 505
http://gearbox.svn.sourceforge.net/gearbox/?rev=505&view=rev
Author: gbiggs
Date: 2010-08-04 05:30:21 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Identify laser models
Modified Paths:
--------------
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
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -32,13 +32,13 @@
ScanData::ScanData()
: _ranges(NULL), _intensities(NULL), _length(0),
- _error(false), _time(0)
+ _error(false), _time(0), _model(MODEL_UNKNOWN)
{
}
ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
- unsigned int time)
- : _error(error), _time(time)
+ unsigned int time, LaserModel model)
+ : _error(error), _time(time), _model(model)
{
_length = length;
if (_length == 0)
@@ -64,8 +64,9 @@
}
ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
- unsigned int length, bool error, unsigned int time)
- : _error(error), _time(time)
+ unsigned int length, bool error, unsigned int time,
+ LaserModel model)
+ : _error(error), _time(time), _model(model)
{
_length = length;
if (_length == 0)
@@ -118,6 +119,7 @@
}
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
ScanData::~ScanData()
@@ -128,7 +130,7 @@
std::string ScanData::error_code_to_string(uint32_t error_code)
{
- if (_sensorIsUTM30LX)
+ if (_model == MODEL_UTM30LX)
{
switch (error_code)
{
@@ -215,6 +217,7 @@
_intensities = NULL;
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
else
{
@@ -265,6 +268,7 @@
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
return *this;
@@ -281,7 +285,8 @@
{
std::stringstream ss;
- ss << _length << " readings:\n";
+ ss << _length << " readings from model ";
+ ss << model_to_string(_model) << ":\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _ranges[ii] << "\t";
if (_intensities != NULL)
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -35,6 +35,8 @@
#define HOKUYO_AIST_EXPORT
#endif
+#include "sensor_info.h"
+
#include <string>
/** @ingroup gbx_library_hokuyo_aist
@@ -57,11 +59,12 @@
ScanData();
/// This constructor performs a deep copy of existing range data.
ScanData(uint32_t* ranges, unsigned int length, bool error,
- unsigned int time);
+ unsigned int time, LaserModel model);
/// This constructor performs a deep copy of existing range and
/// intensity data.
ScanData(uint32_t* ranges, uint32_t* intensities,
- unsigned int length, bool error, unsigned int time);
+ 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();
@@ -90,6 +93,8 @@
This value is only available using SCIP version 2). */
unsigned int time_stamp() const { return _time; }
+ /// Get the model of the laser that produced this scan.
+ LaserModel model() const { return _model; }
/// @brief Assignment operator.
ScanData& operator=(const ScanData &rhs);
@@ -110,7 +115,7 @@
unsigned int _length;
bool _error;
unsigned int _time;
- bool _sensorIsUTM30LX;
+ LaserModel _model;
void allocate_data(unsigned int length,
bool include_intensities = false);
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -225,16 +225,16 @@
///////////////////////////////////////////////////////////////////////////////
Sensor::Sensor()
- : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ : _port(NULL), _err_output(std::cerr), _scip_version(2),
+ _verbose(false), _enable_checksum_workaround(false),
_ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
_resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
}
Sensor::Sensor(std::ostream& err_output)
- : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ : _port(NULL), _err_output(err_output), _scip_version(2),
+ _verbose(false), _enable_checksum_workaround(false),
_ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
_resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
@@ -577,6 +577,8 @@
// Get the product info line
_read_line(buffer);
info->product = &buffer[5];
+ // Only the URG-04LX supports SCIP1
+ _model = MODEL_URG04LX;
// Get the firmware line
_read_line(buffer);
info->firmware = &buffer[5];
@@ -1153,7 +1155,7 @@
// BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
// whereby it does not include anything after and including "<-" in the
// checksum calculation. A workaround is enabled in this function when
-// _sensorIsUTM30LX is true. In this case, if the checksum fails normally, it
+// _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,
@@ -1192,7 +1194,7 @@
}
catch (ProtocolError &e)
{
- if (_sensorIsUTM30LX && _enable_checksum_workaround)
+ if (_model == MODEL_UTM30LX && _enable_checksum_workaround)
{
// Here comes the UTM-30LX workaround
char* hasComment = strstr(buffer, "<-");
@@ -1406,6 +1408,23 @@
return statusCode;
}
+
+void Sensor::_find_model(char const* buffer)
+{
+ if (strstr(buffer, "URG-04LX") != NULL)
+ _model = MODEL_URG04LX;
+ 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
+ _model = MODEL_UNKNOWN;
+}
+
+
void Sensor::_get_and_set_scip_version()
{
bool scip2Failed = false;
@@ -1562,16 +1581,8 @@
else if (strncmp(buffer, "PROD", 4) == 0)
{
info->product = &buffer[5]; // Product info
- // Also do a check to see if this is a UTM-30LX, because we need to
- // work around a checksum problem and the error messages for data
- // change if this is the case
- if (strstr(buffer, "UTM-30LX") != NULL)
- {
- _sensorIsUTM30LX = true;
- _enable_checksum_workaround = true;
- }
- else
- _sensorIsUTM30LX = false;
+ // Find the product model
+ _find_model(&buffer[5]);
}
else if (strncmp(buffer, "FIRM", 4) == 0)
info->firmware = &buffer[5]; // Firmware version
@@ -1673,7 +1684,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
data->_error = false;
// 2 byte data is easy since it fits neatly in a 64-byte block
@@ -1718,7 +1729,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
data->_error = false;
// 3 byte data is a pain because it crosses the line boundary, it may
@@ -1820,7 +1831,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps, true);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
// 3 byte data is a pain because it crosses the line boundary, it may
// overlap by 0, 1 or 2 bytes
Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -292,7 +292,8 @@
std::ostream& _err_output;
uint8_t _scip_version;
- bool _verbose, _sensorIsUTM30LX, _enable_checksum_workaround,
+ LaserModel _model;
+ bool _verbose, _enable_checksum_workaround,
_ignore_unknowns;
double _min_angle, _max_angle, _resolution;
int _first_step, _last_step, _front_step;
@@ -306,6 +307,7 @@
int _send_command(const char *cmd, const char *param, int param_length,
const char *extra_ok);
+ void _find_model(char const* buffer);
void _get_and_set_scip_version();
void _get_defaults();
void _process_vv_line(const char *buffer, SensorInfo *info);
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -32,7 +32,7 @@
: min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
- scanable_steps(0)
+ scanable_steps(0), detected_model(MODEL_UNKNOWN)
{
}
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -44,6 +44,26 @@
namespace hokuyo_aist
{
+/// Laser models
+enum LaserModel {
+ MODEL_URG04LX,
+ MODEL_UTM30LX,
+ MODEL_UNKNOWN
+};
+
+HOKUYO_AIST_EXPORT inline char const* model_to_string(LaserModel model)
+{
+ switch(model)
+ {
+ case MODEL_URG04LX:
+ return "URG-04LX";
+ case MODEL_UTM30LX:
+ return "UTM-30LX";
+ default:
+ return "Unknown model";
+ }
+}
+
class Sensor;
/** @brief Sensor information.
@@ -122,13 +142,15 @@
double resolution;
/// Total number of steps in a full scan (lastStep - firstStep).
unsigned int scanable_steps;
+ /// Detected model of the laser.
+ LaserModel detected_model;
private:
void set_defaults();
void calculate_values();
}; // class SensorInfo
-} // namespace hokuyo_aist
+}; // namespace hokuyo_aist
/** @} */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-08-04 04:48:07
|
Revision: 504
http://gearbox.svn.sourceforge.net/gearbox/?rev=504&view=rev
Author: gbiggs
Date: 2010-08-04 04:48:00 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Fixed linker errors
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/sensor.cpp
gearbox/trunk/src/hokuyo_aist/utils.h
Removed Paths:
-------------
gearbox/trunk/src/hokuyo_aist/utils.cpp
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-04 04:48:00 UTC (rev 504)
@@ -21,8 +21,7 @@
set (srcs hokuyo_errors.cpp
sensor_info.cpp
scan_data.cpp
- sensor.cpp
- utils.cpp)
+ sensor.cpp)
if (WIN32)
if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 04:48:00 UTC (rev 504)
@@ -36,8 +36,6 @@
#include <sstream>
#include <iostream>
-using namespace flexiport;
-
#if defined(WIN32)
#define __func__ __FUNCTION__
#endif
@@ -171,6 +169,55 @@
*/
///////////////////////////////////////////////////////////////////////////////
+// Utility functions
+///////////////////////////////////////////////////////////////////////////////
+
+unsigned int decode_2_byte_value(char *data)
+{
+ unsigned int byte1, byte2;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+
+ return (byte1 << 6) + (byte2);
+}
+
+unsigned int decode_3_byte_value(char *data)
+{
+ unsigned int byte1, byte2, byte3;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
+
+ return (byte1 << 12) + (byte2 << 6) + (byte3);
+}
+
+unsigned int decode_4_byte_value(char *data)
+{
+ unsigned int byte1, byte2, byte3, byte4;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
+ byte4 = data[3] - 0x30;
+
+ return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
+}
+
+void number_to_string(unsigned int num, char *dest, int length)
+{
+#if defined(WIN32)
+ _snprintf(dest, length + 1, "%*d", length, num);
+#else
+ snprintf(dest, length + 1, "%*d", length, num);
+#endif
+ // Replace all leading spaces with '0'
+ for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
+ dest[ii] = '0';
+}
+
+///////////////////////////////////////////////////////////////////////////////
// Sensor class
///////////////////////////////////////////////////////////////////////////////
@@ -268,7 +315,7 @@
const unsigned int numBauds = 5;
for (unsigned int ii = 0; ii < numBauds; ii++)
{
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(bauds[ii]);
+ reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(bauds[ii]);
try
{
_get_and_set_scip_version();
@@ -310,7 +357,7 @@
}
if (_port->GetPortType() == "serial")
- return reinterpret_cast<SerialPort*>(_port)->GetBaudRate();
+ return reinterpret_cast<flexiport::SerialPort*>(_port)->GetBaudRate();
else
return 0;
}
@@ -399,7 +446,7 @@
_send_command("S", newBaud, 13, NULL);
_skip_lines(1);
// Change the port's baud rate
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
+ reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
}
else if (_scip_version == 2)
{
@@ -407,7 +454,7 @@
_send_command("SS", newBaud, 6, "03");
_skip_lines(1);
// Change the port's baud rate
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
+ reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
}
else
throw UnknownScipVersionError();
Deleted: gearbox/trunk/src/hokuyo_aist/utils.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/utils.cpp 2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/utils.cpp 2010-08-04 04:48:00 UTC (rev 504)
@@ -1,70 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
- *
- * hokuyo_aist Hokuyo laser scanner driver.
- *
- * 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 file is part of hokuyo_aist.
- *
- * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
- * http://www.opensource.org/licenses/eclipse-1.0.txt
- */
-
-#include "utils.h"
-
-#include <cstdio>
-
-using namespace hokuyo_aist;
-
-unsigned int decode_2_byte_value(char *data)
-{
- unsigned int byte1, byte2;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
-
- return (byte1 << 6) + (byte2);
-}
-
-unsigned int decode_3_byte_value(char *data)
-{
- unsigned int byte1, byte2, byte3;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
-
- return (byte1 << 12) + (byte2 << 6) + (byte3);
-}
-
-unsigned int decode_4_byte_value(char *data)
-{
- unsigned int byte1, byte2, byte3, byte4;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
- byte4 = data[3] - 0x30;
-
- return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
-}
-
-void number_to_string(unsigned int num, char *dest, int length)
-{
-#if defined(WIN32)
- _snprintf(dest, length + 1, "%*d", length, num);
-#else
- snprintf(dest, length + 1, "%*d", length, num);
-#endif
- // Replace all leading spaces with '0'
- for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
- dest[ii] = '0';
-}
-
Modified: gearbox/trunk/src/hokuyo_aist/utils.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/utils.h 2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/utils.h 2010-08-04 04:48:00 UTC (rev 504)
@@ -63,15 +63,6 @@
}
#endif
-/// Decode a 2 byte encoded value
-unsigned int decode_2_byte_value(char *data);
-/// Decode a 3 byte encoded value
-unsigned int decode_3_byte_value(char *data);
-/// Decode a 4 byte encoded value
-unsigned int decode_4_byte_value(char *data);
-/// Convert a number to a string
-void number_to_string(unsigned int num, char *dest, int length);
-
} // namespace hokuyo_aist
/** @} */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-08-04 04:25:21
|
Revision: 503
http://gearbox.svn.sourceforge.net/gearbox/?rev=503&view=rev
Author: gbiggs
Date: 2010-08-04 04:25:13 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Reorganising code
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
Added Paths:
-----------
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/utils.cpp
gearbox/trunk/src/hokuyo_aist/utils.h
Removed Paths:
-------------
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-04 03:20:45 UTC (rev 502)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-04 04:25:13 UTC (rev 503)
@@ -10,31 +10,40 @@
GBX_REQUIRE_LIBS (build LIB ${libName} ${reqLibs})
if (build)
- include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
+ include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
- set (hdrs hokuyo_aist.h hokuyo_errors.h)
- set (srcs hokuyo_aist.cpp hokuyo_errors.cpp)
+ set (hdrs hokuyo_aist.h
+ hokuyo_errors.h
+ sensor_info.h
+ scan_data.h
+ sensor.h
+ utils.h)
+ set (srcs hokuyo_errors.cpp
+ sensor_info.cpp
+ scan_data.cpp
+ sensor.cpp
+ utils.cpp)
- if (WIN32)
- if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
- add_definitions (-DHOKUYO_AIST_EXPORTS)
- else (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
- add_definitions (-DHOKUYO_AIST_STATIC -DFLEXIPORT_STATIC)
- endif (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
- endif (WIN32)
- GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
- target_link_libraries (${libName} ${reqLibs})
- GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
+ if (WIN32)
+ if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+ add_definitions (-DHOKUYO_AIST_EXPORTS)
+ else (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+ add_definitions (-DHOKUYO_AIST_STATIC -DFLEXIPORT_STATIC)
+ endif (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+ endif (WIN32)
+ GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
+ target_link_libraries (${libName} ${reqLibs})
+ GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
- GBX_ADD_HEADERS (${libName} ${hdrs})
+ GBX_ADD_HEADERS (${libName} ${hdrs})
- add_subdirectory (getid)
- if (GBX_BUILD_TESTS)
- add_subdirectory (test)
- endif (GBX_BUILD_TESTS)
+ add_subdirectory (getid)
+ if (GBX_BUILD_TESTS)
+ add_subdirectory (test)
+ endif (GBX_BUILD_TESTS)
- option (HOKUYO_AIST_BUILD_BINDINGS "Build the Python bindings for Hokuyo_aist" ON)
- if (HOKUYO_AIST_BUILD_BINDINGS)
- add_subdirectory (python)
- endif (HOKUYO_AIST_BUILD_BINDINGS)
+ option (HOKUYO_AIST_BUILD_BINDINGS "Build the Python bindings for Hokuyo_aist" ON)
+ if (HOKUYO_AIST_BUILD_BINDINGS)
+ add_subdirectory (python)
+ endif (HOKUYO_AIST_BUILD_BINDINGS)
endif (build)
Deleted: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-04 03:20:45 UTC (rev 502)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-04 04:25:13 UTC (rev 503)
@@ -1,2399 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
- *
- * hokuyo_aist Hokuyo laser scanner driver.
- *
- * 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 file is part of hokuyo_aist.
- *
- * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
- * http://www.opensource.org/licenses/eclipse-1.0.txt
- */
-
-#include "hokuyo_aist.h"
-#include "hokuyo_errors.h"
-using namespace hokuyo_aist;
-
-#include <flexiport/flexiport.h>
-#include <flexiport/port.h>
-#include <flexiport/serialport.h>
-
-#include <cstring>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <errno.h>
-#include <math.h>
-#include <sstream>
-#include <iostream>
-
-using namespace flexiport;
-
-#if defined(WIN32)
- #define __func__ __FUNCTION__
-#endif
-
-namespace hokuyo_aist
-{
-
-#ifndef M_PI
- const double M_PI = 3.14159265358979323846;
-#endif
-// Convert radians to degrees
-#ifndef RTOD
- inline double RTOD(double rad)
- {
- return rad * 180.0 / M_PI;
- }
-#endif
-// Convert degrees to radians
-#ifndef DTOR
- inline double DTOR(double deg)
- {
- return deg * M_PI / 180.0;
- }
-#endif
-
-// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
-// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
-
-///////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 1 notes
-///////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
-(x) = x byte block
- - No checksum
- - Host to sensor: Command | Parameters... | LF
- - Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF
- - Where a block of data would take more than 64 bytes, a line feed is inserted
- every 64 bytes.
- - Status 0 is OK, anything else is an error.
-
-L Power
- L|Control code|LF
- L|Control code|LF|Status|LF|LF
- 3 byte command block
-G Get data
- G|Start(3)|End(3)|Cluster(2)|LF
- G|Start(3)|End(3)|Cluster(2)|LF|Status|LF|Data...|LF|LF
- 10 byte command block
-S Set baud rate
- S|Baud rate(6)|Reserved(7)|LF
- S|Baud rate(6)|Reserved(7)|Status|LF|LF|
- 16 byte command block
-V Version info
- V|LF
- V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|
- Serial...|LF|LF
- 2 byte command block
-*/
-
-///////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 2 notes
-///////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
-(x) = x byte block
- - We don't use the string block (which can be up to 16 bytes) so it's marked
- as size 0 and ignored in the command definitions below.
- - Host to sensor: Command(2) | Parameters... | String(0) | LF
- - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) |
- Sum | LF
- - Each data row: Data (max 64) | Sum | LF
- - Data rows are broken after a maximum of 64 bytes, each one having a checksum
- and a line feed.
- - Status codes 00 and 99 are OK, anything else is an error.
- - Checksum is calculated by... well, see the code.
- - BIG NOTE: The URG-30LX has a probable bug in its response to the II code
- whereby it does not include anything after and including "<-" in the
- checksum calculation.
-
-VV Version info
- V|V|LF
- V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|
- Firmware...|;|Sum|LF|Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
- 3 byte command block
-PP Specification info
- P|P|LF
- P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|
- MinRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
- LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF
- 3 byte command block
-II Status info
- I|I|LF
- I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|
- MotorSpeed...|;|Sum|LF|Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|
- LF|Diagnostic...|;|Sum|LF|LF
- 3 byte command block
-BM Power on
- B|M|LF
- B|M|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-QT Power off
- Q|T|LF
- Q|T|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-SS Set baud rate
- S|S|Baud(6)|LF
- S|S|Baud(6)|LF|Status(2)|Sum|LF|LF
- 9 byte command block
-MDMS Get new data
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|
- LF|Data...|LF|LF
- 16 byte command block
-ME Get new data, including intensity data
- M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|
- Data...|LF|LF
- 16 byte command block
- Featured only on UTM-30LX for getting intensity data.
-GDGS Get latest data
- G|D/S|Start(4)|End(4)|Cluster(2)|LF
- G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
- 12 byte command block
-CR Set motor speed
- C|R|Speed(2)|LF
- C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
- 5 byte command block
-TM Get sensor time
- T|M|Code|LF
- T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
- 4 byte command block
- Optional part only comes back for control code 1.
-RS Reset
- R|S|LF
- R|S|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-GE
-ND
-NE
-HD
-HE
-HS
-$IP
-RT
-DB
-*/
-
-///////////////////////////////////////////////////////////////////////////////
-// Utility functions
-///////////////////////////////////////////////////////////////////////////////
-
-unsigned int decode_2_byte_value(char *data)
-{
- unsigned int byte1, byte2;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
-
- return (byte1 << 6) + (byte2);
-}
-
-unsigned int decode_3_byte_value(char *data)
-{
- unsigned int byte1, byte2, byte3;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
-
- return (byte1 << 12) + (byte2 << 6) + (byte3);
-}
-
-unsigned int decode_4_byte_value(char *data)
-{
- unsigned int byte1, byte2, byte3, byte4;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
- byte4 = data[3] - 0x30;
-
- return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
-}
-
-void number_to_string(unsigned int num, char *dest, int length)
-{
-#if defined(WIN32)
- _snprintf(dest, length + 1, "%*d", length, num);
-#else
- snprintf(dest, length + 1, "%*d", length, num);
-#endif
- // Replace all leading spaces with '0'
- for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
- dest[ii] = '0';
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// SensorInfo class
-///////////////////////////////////////////////////////////////////////////////
-
-SensorInfo::SensorInfo()
- : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
- front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
- baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
- scanable_steps(0)
-{
-}
-
-// Set various known values based on what the manual says
-void SensorInfo::set_defaults()
-{
- min_range = 20;
- max_range = 4095;
- steps = 1024;
- first_step = 44;
- last_step = 725;
- front_step = 384;
-}
-
-void SensorInfo::calculate_values()
-{
- resolution = DTOR(360.0) / steps;
- // If any of the steps are beyond INT_MAX, we have problems.
- // We also have an incredibly high-resolution sensor.
- min_angle = (static_cast<int>(first_step) - static_cast<int>(front_step)) *
- resolution;
- max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) *
- resolution;
- scanable_steps = last_step - first_step + 1;
-}
-
-std::string SensorInfo::as_string()
-{
- std::stringstream ss;
-
- ss << "Vendor: " << vendor << '\n';
- ss << "Product: " << product << '\n';
- ss << "Firmware: " << firmware << '\n';
- ss << "Protocol: " << protocol << '\n';
- ss << "Serial: " << serial << '\n';
- ss << "Model: " << model << '\n';
-
- ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range <<
- "mm\n";
- ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
- scanable_steps << '\n';
- ss << "First step: " << first_step << "\tFront step: " << front_step <<
- "\tLast step: " << last_step << '\n';
- ss << "Resolution: " << resolution << " radians/step\n";
- ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
- max_angle << " radians\n";
- ss << "Standard motor speed: " << standard_speed << "rpm\n";
-
- ss << "Power status: " << (power ? "On" : "Off") <<
- "\tMeasurement state: " << measure_state << '\n';
- ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
- ")\tBaud rate: " << baud << "bps\n";
- ss << "Time stamp: " << time << "ms\n";
- ss << "Sensor diagnostic: " << sensor_diagnostic << '\n';
-
- return ss.str();
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// ScanData class
-///////////////////////////////////////////////////////////////////////////////
-
-ScanData::ScanData()
- : _ranges(NULL), _intensities(NULL), _length(0),
- _error(false), _time(0)
-{
-}
-
-ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
- unsigned int time)
- : _error(error), _time(time)
-{
- _length = length;
- if (_length == 0)
- {
- _ranges = NULL;
- _intensities = NULL;
- }
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
- _intensities = NULL; // No intensity data to copy
- }
-}
-
-ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
- unsigned int length, bool error, unsigned int time)
- : _error(error), _time(time)
-{
- _length = length;
- if (_length == 0)
- {
- _ranges = NULL;
- _intensities = NULL;
- }
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
- _intensities = new uint32_t[_length];
- memcpy(_intensities, intensities, sizeof(uint32_t) * _length);
- }
-}
-
-ScanData::ScanData(const ScanData &rhs)
-{
- _length = rhs.length();
- if (_length == 0)
- _ranges = NULL;
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
-
- if (rhs.intensities() != NULL)
- {
- _intensities = new uint32_t[_length];
- memcpy(_intensities, rhs.intensities(),
- sizeof(uint32_t) * _length);
- }
- }
- _error = rhs.get_error_status();
- _time = rhs.time_stamp();
-}
-
-ScanData::~ScanData()
-{
- if(_ranges != NULL)
- delete[] _ranges;
-}
-
-std::string ScanData::error_code_to_string(uint32_t error_code)
-{
- if (_sensorIsUTM30LX)
- {
- switch (error_code)
- {
- case 1:
- return "No object in the range.";
- case 2:
- return "Object is too near (internal error).";
- case 3:
- return "Measurement error (may be due to interference).";
- case 4:
- return "Object out of range (at the near end).";
- case 5:
- return "Other error.";
- default:
- std::stringstream ss;
- ss << "Unknown error code: " << error_code;
- return ss.str();
- }
- }
- else
- {
- switch (error_code)
- {
- case 0:
- return "Detected object is possibly at 22m.";
- case 1:
- return "Reflected light has low intensity.";
- case 2:
- return "Reflected light has low intensity.";
- case 3:
- return "Reflected light has low intensity.";
- case 4:
- return "Reflected light has low intensity.";
- case 5:
- return "Reflected light has low intensity.";
- case 6:
- return "Possibility of detected object is at 5.7m.";
- case 7:
- return "Distance data on the preceding and succeeding steps "
- "have errors.";
- case 8:
- return "Others.";
- case 9:
- return "The same step had error in the last two scan.";
- case 10:
- return "Others.";
- case 11:
- return "Others.";
- case 12:
- return "Others.";
- case 13:
- return "Others.";
- case 14:
- return "Others.";
- case 15:
- return "Others.";
- case 16:
- return "Possibility of detected object is in the range "
- "4096mm.";
- case 17:
- return "Others.";
- case 18:
- return "Unspecified.";
- case 19:
- return "Non-measurable distance.";
- default:
- std::stringstream ss;
- ss << "Unknown error code: " << error_code;
- return ss.str();
- }
- }
-}
-
-ScanData& ScanData::operator=(const ScanData &rhs)
-{
- if (rhs.length() == 0)
- {
- _length = 0;
- if (_ranges != NULL)
- delete[] _ranges;
- _ranges = NULL;
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- _error = rhs.get_error_status();
- _time = rhs.time_stamp();
- }
- else
- {
- unsigned int rhslength = rhs.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)
- delete[] _ranges;
- _ranges = newData;
- _length = rhs.length();
-
- if (rhs.intensities() != NULL)
- {
- try
- {
- newData = new uint32_t[rhslength];
- }
- catch (std::bad_alloc &e)
- {
- // We have to remove any old intensity data or the length
- // won't match
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- throw;
- }
- memcpy(newData, rhs.intensities(), sizeof(uint32_t) *
- rhslength);
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = newData;
- }
- }
- else
- {
- // If lengths are the same, no need to reallocate
- memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
- if (rhs.intensities() != NULL)
- memcpy(_intensities, rhs.intensities(), sizeof(uint32_t) *
- _length);
- }
-
- _error = rhs.get_error_status();
- _time = rhs.time_stamp();
- }
-
- return *this;
-}
-
-uint32_t ScanData::operator[](unsigned int index)
-{
- if (index >= _length)
- throw IndexError();
- return _ranges[index];
-}
-
-std::string ScanData::as_string()
-{
- std::stringstream ss;
-
- ss << _length << " readings:\n";
- for (unsigned int ii = 0; ii < _length; ii++)
- ss << _ranges[ii] << "\t";
- 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)
- {
- ss << "Detected data errors:\n";
- for (unsigned int ii = 0; ii < _length; ii++)
- {
- if (_ranges[ii] < 20)
- ss << ii << ": " <<
- error_code_to_string(_ranges[ii]) << '\n';
- }
- }
- else
- ss << "No data errors.\n";
- ss << "Time stamp: " << _time << '\n';
-
- return ss.str();
-}
-
-void ScanData::clean_up()
-{
- if (_ranges != NULL)
- delete[] _ranges;
- _ranges = NULL;
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- _length = 0;
- _error = false;
- _time = 0;
-}
-
-void ScanData::allocate_data(unsigned int length, bool include_intensities)
-{
- // If no data yet, allocate new
- if (_ranges == NULL)
- {
- try
- {
- _ranges = new uint32_t[length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- _length = length;
- }
- // If there is data, reallocate only if the length is different
- else if (length != _length)
- {
- delete[] _ranges;
- try
- {
- _ranges = new uint32_t[length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- _length = length;
- }
- // Else data is already allocated to the right length, so do nothing
-
- if (include_intensities)
- {
- // If no data yet, allocate new
- if (_intensities == NULL)
- {
- _intensities = new uint32_t[length];
- }
- // If there is data, reallocate only if the length is different
- 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)
- {
- // If not told to allocate space for intensity data and it exists,
- // remove it
- delete[] _intensities;
- _intensities = NULL;
- }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Sensor class
-///////////////////////////////////////////////////////////////////////////////
-
-// Public API
-///////////////////////////////////////////////////////////////////////////////
-
-Sensor::Sensor()
- : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
- _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
- _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
-{
-}
-
-Sensor::Sensor(std::ostream& err_output)
- : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
- _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
- _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
-{
-}
-
-Sensor::~Sensor()
-{
- if (_port != NULL)
- delete _port;
-}
-
-void Sensor::open(std::string port_options)
-{
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Creating and opening port using options: " <<
- port_options << '\n';
- }
- _port = flexiport::CreatePort(port_options);
- _port->Open();
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Connected using " <<
- _port->GetPortType() << " connection.\n";
- _err_output << _port->GetStatus();
- }
- _port->Flush();
-
- // Figure out the SCIP version currently in use and switch to a higher one
- // if possible
- _get_and_set_scip_version();
- // Get some values we need for providing default ranges
- _get_defaults();
-}
-
-unsigned int Sensor::open_with_probing(std::string port_options)
-{
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Creating and opening port using options: " << port_options <<
- '\n';
- }
- _port = flexiport::CreatePort(port_options);
- _port->Open();
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Connected using " <<
- _port->GetPortType() << " connection.\n";
- _err_output << _port->GetStatus();
- }
- _port->Flush();
-
- try
- {
- // Figure out the SCIP version currently in use and switch to a higher
- // one if possible
- _get_and_set_scip_version();
- // Get some values we need for providing default ranges
- _get_defaults();
- }
- catch (BaseError)
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Failed to connect at the default baud rate.\n";
- }
- 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;
- for (unsigned int ii = 0; ii < numBauds; ii++)
- {
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(bauds[ii]);
- try
- {
- _get_and_set_scip_version();
- _get_defaults();
- // If the above two functions succeed, break out of the
- // loop and be happy
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Connected at " << bauds[ii] << '\n';
- }
- return bauds[ii];
- }
- catch (BaseError)
- {
- if (ii == numBauds - 1)
- {
- // Last baud rate, give up and rethrow
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Failed to connect at any baud rate.\n";
- }
- throw;
- }
- // Otherwise go around again
- }
- }
- }
- else
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Port is not serial, cannot probe.\n";
- }
- throw;
- }
- }
-
- if (_port->GetPortType() == "serial")
- return reinterpret_cast<SerialPort*>(_port)->GetBaudRate();
- else
- return 0;
-}
-
-void Sensor::close()
-{
- if (!_port)
- throw CloseError();
- if (_verbose)
- _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
- delete _port;
- _port = NULL;
-}
-
-bool Sensor::is_open() const
-{
- if (_port != NULL)
- return _port->IsOpen();
- return false;
-}
-
-void Sensor::set_power(bool on)
-{
- if (_scip_version == 1)
- {
- if (on)
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Turning laser on.\n";
- _send_command("L", "1", 1, NULL);
- }
- else
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Turning laser off.\n";
- _send_command("L", "0", 1, NULL);
- }
- _skip_lines(1);
- }
- else if (_scip_version == 2)
- {
- if (on)
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Turning laser on.\n";
- _send_command("BM", NULL, 0, "02");
- }
- else
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Turning laser off.\n";
- _send_command("QT", NULL, 0, "02");
- }
- _skip_lines(1);
- }
- else
- throw UnknownScipVersionError();
-}
-
-// This function assumes that both the port and the laser scanner are already
-// set to the same baud.
-void Sensor::set_baud(unsigned int baud)
-{
- if (_port->GetPortType() != "serial")
- {
- throw NotSerialError();
- }
-
- char newBaud[13];
- memset(newBaud, 0, sizeof(char) * 13);
-
- 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)
- {
- // Send the command to change baud rate
- _send_command("S", newBaud, 13, NULL);
- _skip_lines(1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
- }
- else if (_scip_version == 2)
- {
- // Send the command to change baud rate
- _send_command("SS", newBaud, 6, "03");
- _skip_lines(1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
- }
- else
- throw UnknownScipVersionError();
-}
-
-void Sensor::reset()
-{
- if (_scip_version == 1)
- throw UnsupportedError(7);
- else if (_scip_version == 2)
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Resetting laser.\n";
- _send_command("RS", NULL, 0, NULL);
- _skip_lines(1);
- }
- else
- throw UnknownScipVersionError();
-}
-
-void Sensor::set_motor_speed(unsigned int speed)
-{
- if (_scip_version == 1)
- throw UnsupportedError(8);
- else if (_scip_version == 2)
- {
- // Sanity check the value
- if (speed > 10 && speed != 99)
- throw MotorSpeedError();
- char buffer[3];
- if (speed == 0)
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Reseting motor speed to default.\n";
- }
- buffer[0] = '0';
- buffer[1] = '0';
- buffer[2] = '\0';
- }
- else if (speed == 99)
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Reseting motor speed to default.\n";
- }
- buffer[0] = '9';
- buffer[1] = '9';
- buffer[2] = '\0';
- }
- else
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Setting motor speed to ratio " << speed << '\n';
- }
- number_to_string(speed, buffer, 2);
- }
- _send_command("CR", buffer, 2, "03");
- _skip_lines(1);
- }
- else
- throw UnknownScipVersionError();
-}
-
-void Sensor::set_high_sensitivity(bool on)
-{
- if (_scip_version == 1)
- throw UnsupportedError(10);
- else if (_scip_version == 2)
- {
- if (on)
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Switching to high sensitivity.\n";
- _send_command("HS", "1", 1, "02");
- }
- else
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Switching to normal sensitivity.\n";
- }
- _send_command("HS", "0", 1, "02");
- }
- _skip_lines(1);
- }
- else
- throw UnknownScipVersionError();
-}
-
-void Sensor::get_sensor_info(SensorInfo *info)
-{
- if (info == NULL)
- throw NoDestinationError();
-
- if (_scip_version == 1)
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Getting sensor information using SCIP version 1.\n";
- }
-
- info->set_defaults();
-
- char buffer[SCIP1_LINE_LENGTH];
- memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
-
- _send_command("V", NULL, 0, NULL);
- // Get the vendor info line
- _read_line(buffer);
- info->vendor = &buffer[5]; // Chop off the "VEND:" tag
- // Get the product info line
- _read_line(buffer);
- info->product = &buffer[5];
- // Get the firmware line
- _read_line(buffer);
- info->firmware = &buffer[5];
- // Get the protocol version line
- _read_line(buffer);
- info->protocol = &buffer[5];
- // Get the serial number
- _read_line(buffer);
- info->serial = &buffer[5];
- // Get either the status line or the end of message
- _read_line(buffer);
- if (buffer[0] != '\0')
- {
- // Got a status line
- info->sensor_diagnostic = &buffer[5];
- _skip_lines(1);
- }
-
- // Check the firmware version major number. If it's >=3 there is
- // probably some extra info in the firmware line.
- // 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 (_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)
- {
- // No bracket? Crud. Fail and use the hard-coded values from
- // the manual.
- info->calculate_values();
- }
- // Now put it through sscanf and hope...
- int aperture;
- int numFound = sscanf(valueStart,
- "(%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)
- {
- // Didn't get enough values out, assume unknown format and fall
- // back on the defaults
- info->set_defaults();
- info->calculate_values();
- if (_verbose)
- {
- _err_output << "Retrieved sensor info (hard-coded, not "
- "enough values):\n";
- _err_output << info->as_string();
- }
- }
- else
- {
- // Need to calculate stuff differently since it gave us an
- // aperture value
- info->resolution = DTOR(static_cast<double>(aperture)) /
- static_cast<double>(info->last_step - info->first_step);
- // Assume that the range is evenly spread
- info->scanable_steps = info->last_step - info->first_step + 1;
- info->front_step = info->scanable_steps / 2 +
- info->first_step - 1;
- info->min_angle = (static_cast<int>(info->first_step) -
- static_cast<int>(info->front_step)) * info->resolution;
- info->max_angle = (info->last_step - info->front_step) *
- info->resolution;
-
- if (_verbose)
- {
- _err_output << "Retrieved sensor info (from FIRM line):\n";
- _err_output << info->as_string();
- }
- }
- }
- else
- {
- // We're stuck with hard-coded defaults from the manual (already
- // set earlier).
- info->calculate_values();
- if (_verbose)
- {
- _err_output << "Retrieved sensor info (hard-coded):\n";
- _err_output << info->as_string();
- }
- }
- }
- else if (_scip_version == 2)
- {
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Getting sensor information using SCIP version 2.\n";
- }
-
- info->set_defaults();
-
- char buffer[SCIP2_LINE_LENGTH];
- memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
-
- // 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)
- _process_vv_line(buffer, info);
-
- // Next up, PP
- _send_command("PP", NULL, 0, NULL);
- 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)
- _process_ii_line(buffer, info);
-
- _enable_checksum_workaround = false;
-
- info->calculate_values();
- if (_verbose)
- {
- _err_output << "Retrieved sensor info:\n";
- _err_output << info->as_string();
- }
- }
- else
- throw UnknownScipVersionError();
-}
-
-unsigned int Sensor::get_time()
-{
- if (_scip_version == 1)
- throw UnsupportedError(12);
- else if (_scip_version == 2)
- {
- if (_verbose)
- _err_output << "Sensor::" << __func__ <<
- "() Retrieving time from laser.\n";
- _send_command("TM", "0", 1, NULL);
- _send_command("TM", "1", 1, NULL);
- char buffer[7];
- _read_line_with_check(buffer, 6);
- _send_command("TM", "2", 1, NULL);
- _skip_lines(1);
- // We need to decode the time value that's in the buffer
- return decode_4_byte_value(buffer);
- }
- else
- throw UnknownScipVersionError();
-
- return 0;
-}
-
-unsigned int Sensor::get_ranges(ScanData *data, int start_step,
- int end_step, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDestinationError();
-
- char buffer[11];
- memset(buffer, 0, sizeof(char) * 11);
-
- if (start_step < 0)
- start_step = _first_step;
- if (end_step < 0)
- end_step = _last_step;
-
- unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " ranges between " << start_step << " and " <<
- end_step << " with a cluster count of " << cluster_count <<
- '\n';
- }
-
- if (_scip_version == 1)
- {
- // Send the command to ask for the most recent range data from
- // start_step to end_step
- number_to_string(start_step, buffer, 3);
- number_to_string(end_step, &buffer[3], 3);
- number_to_string(cluster_count, &buffer[6], 2);
- _send_command("G", buffer, 8, NULL);
- // 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)
- {
- // 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);
- _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)
- throw NoDataError();
- data->_time = decode_4_byte_value(buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're
- // sending the GD command
- _read_3_byte_range_data(data, num_steps);
- }
- else
- throw UnknownScipVersionError();
-
- return data->_length;
-}
-
-unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
- double end_angle, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDataError();
-
- // Calculate the given angles in steps, rounding towards _front_step
- int start_step, end_step;
- start_step = angle_to_step(start_angle);
- end_step = angle_to_step(end_angle);
-
- // Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
- throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
- throw EndStepError();
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Start angle " <<
- start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << '\n';
- }
-
- // Get the data
- return get_ranges(data, start_step, end_step, cluster_count);
-}
-
-unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
- int end_step, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDestinationError();
-
- if (_scip_version == 1)
- throw UnsupportedError(16);
- else if (_scip_version == 2)
- {
- char buffer[14];
- memset(buffer, 0, sizeof(char) * 14);
-
- if (start_step < 0)
- start_step = _first_step;
- if (end_step < 0)
- end_step = _last_step;
-
- unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " new ranges between " << start_step << " and " <<
- end_step << " with a cluster count of " << cluster_count <<
- '\n';
- }
-
- // 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);
- number_to_string(1, &buffer[10], 1);
- number_to_string(1, &buffer[11], 2);
- _send_command("MD", buffer, 13, NULL);
- // Mx commands will perform a scan, then send the data prefixed with
- // another command echo.
- // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
- char response[17];
- _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] != 'M' || response[1] != 'D')
- throw CommandEchoError("MD", response);
- // Then compare the parameters
- buffer[12] = '0'; // There will be zero scans remaining after this one
- if (memcmp(&response[2], buffer, 13) != 0)
- throw ParamEchoError("MD");
- // The next line should be the status line
- _read_line_with_check(response, 4);
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() MD 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')
- {
- // There is an extra line feed after an error status (signalling
- // end of message)
- _skip_lines(1);
- throw ResponseError(response, "MD");
- }
-
- // Now the actual data will arrive
- // 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)
- throw NoDataError();
- data->_time = decode_4_byte_value(buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're
- // sending the MD command
- _read_3_byte_range_data(data, num_steps);
- }
- else
- throw UnknownScipVersionError();
-
- return data->_length;
-}
-
-unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
- double start_angle, double end_angle, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDestinationError();
- if (_scip_version == 1)
- throw UnsupportedError(16);
-
- // Calculate the given angles in steps, rounding towards _front_step
- int start_step, end_step;
- start_step = angle_to_step(start_angle);
- end_step = angle_to_step(end_angle);
-
- // Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
- throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
- throw EndStepError();
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Start angle " <<
- start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << '\n';
- }
-
- // Get the data
- return get_new_ranges(data, start_step, end_step, cluster_count);
-}
-
-unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
- int start_step, int end_step, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDestinationError();
-
- if (_scip_version == 1)
- throw UnsupportedError(17);
- else if (_scip_version == 2)
- {
- char buffer[14];
- memset(buffer, 0, sizeof(char) * 14);
-
- if (start_step < 0)
- start_step = _first_step;
- if (end_step < 0)
- end_step = _last_step;
-
- unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " new ranges between " << start_step << " and " <<
- end_step << " with a cluster count of " << cluster_count <<
- '\n';
- }
-
- // Send the command to ask for the most recent range data with
- // intensity 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);
- number_to_string(1, &buffer[10], 1);
- number_to_string(1, &buffer[11], 2);
- _send_command("ME", buffer, 13, NULL);
- // Mx commands will perform a scan, then send the data prefixed with
- // another command echo.
- // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
- char response[17];
- _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] != 'M' || response[1] != 'E')
- throw CommandEchoError("ME", response);
- // Then compare the parameters
- if (memcmp(&response[2], buffer, 13) != 0)
- throw ParamEchoError("ME");
- // The next line should be the status line
- _read_line_with_check(response, 4);
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() ME 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')
- {
- // There is an extra line feed after an error status (signalling
- // end of message)
- _skip_lines(1);
- throw ResponseError(response, "ME");
- }
-
- // Now the actual data will arrive
- // 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)
- throw NoDataError();
- data->_time = decode_4_byte_value(buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're
- // sending the ME command
- _read_3_byte_range_and_intensity_data(data, num_steps);
- }
- else
- throw UnknownScipVersionError();
-
- return data->_length;
-}
-
-unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
- double start_angle, double end_angle, unsigned int cluster_count)
-{
- if (data == NULL)
- throw NoDestinationError();
- if (_scip_version == 1)
- throw UnsupportedError(17);
-
- // Calculate the given angles in steps, rounding towards _front_step
- int start_step, end_step;
- start_step = angle_to_step(start_angle);
- end_step = angle_to_step(end_angle);
-
- // Check the steps are within the allowable range
- if (start_step < _first_step || start_step > _last_step)
- throw StartStepError();
- if (end_step < _first_step || end_step > _last_step)
- throw EndStepError();
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Start angle " <<
- start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << '\n';
- }
-
- // Get the data
- return get_new_ranges_intensities(data, start_step, end_step,
- cluster_count);
-}
-
-double Sensor::step_to_angle(unsigned int step)
-{
- return (static_cast<int>(step) - static_cast<int>(_front_step)) *
- _resolution;
-}
-
-unsigned int Sensor::angle_to_step(double angle)
-{
- unsigned int result;
- double resultF;
- resultF = _front_step +
- (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)
- result = static_cast<int>(ceil(resultF));
- else
- result = static_cast<int>(floor(resultF));
-
- return result;
-}
-
-// Private functions
-///////////////////////////////////////////////////////////////////////////////
-
-// Sometimes, just flushing isn't enough, as it appears the scanner will wait
-// when the buffer gets full, then continue sending data. If we flush, we get
-// rid of what was sent, and the scanner just sends more. This is a problem if
-// we're trying to clear the result of a previous command. To get around this,
-// keep flushing until the port reports there is no data left after a timeout.
-// This shouldn't be called too much, as it introduces a delay as big as the
-// timeout (which may be infinite).
-void Sensor::_clear_read_buffer()
-{
- while (_port->BytesAvailableWait() > 0)
- _port->Flush();
-}
-
-// If expected_length is not -1, it should include the terminating line feed but
-// not the NULL (although the buffer still has to include this).
-// If expected_length is -1, this function expects buffer to be a certain length
-// to allow up to the maximum line length to be read. See SCIP1_LINE_LENGTH and
-// SCIP2_LINE_LENGTH.
-// 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 linelength = 0;
-
- if (expected_length == -1)
- {
- int maxlength = (_scip_version == 1) ?
- SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
- 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)
- throw ReadError(0);
- else if (linelength == 0)
- throw ReadError(1);
- // Replace the line feed with a NULL
- buffer[linelength - 1] = '\0';
- }
- else
- {
- 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)
- throw ReadError(0);
- else if (linelength == 0)
- throw ReadError(1);
- else if (linelength < expected_length)
- throw LineLengthError(linelength, expected_length);
- // Replace the line feed with a NULL
- buffer[linelength - 1] = '\0';
- }
-
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
- " bytes.\n";
- _err_output << "Sensor::" << __func__ << "() Line is " << buffer <<
- '\n';
- }
- return linelength - 1; // Line feed not included
-}
-
-// This function will read a line and then calculate its checksum, comparing it
-// with the checksum at the end of the line. The checksum will be removed
-// (along with the semi-colon, if present). buffer and expected_length args are
-// as for _read_line().
-// If has_semicolon is true, the byte before the checksum is assumed to be the
-// semi-colon separator and so not a part of the checksum. If it's not a
-// semi-colon, an exception is thrown.
-// Empty lines (i.e. a line that is just the line feed, as at the end of the
-// message) will result in a return value of zero and no checksum check will be
-// performed. Otherwise the number of actual data bytes (i.e. excluding the
-// checksum and semicolon) will be returned.
-// BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
-// whereby it does not include anything after and including "<-" in the
-// checksum calculation. A workaround is enabled in this function when
-// _sensorIsUTM30LX is true. 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,
- bool has_semicolon)
-{
- int linelength = _read_line(buffer, expected_length);
- if (_scip_version == 1)
- {
- // No checksums in SCIP version 1
- return linelength;
- }
-
- // 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)
- 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)
- {
- _err_output << "Sensor::" << __func__ << "() Considering " <<
- bytesToConsider << " bytes for checksum from a line length of " <<
- linelength << " bytes.\n";
- }
- if (bytesToConsider < 1)
- throw InsufficientBytesError(bytesToConsider, linelength);
-
- int checkSum = 0;
- try
- {
- checkSum = _confirm_checksum(buffer, bytesToConsider,
- static_cast<int>(buffer[checksumIndex]));
- }
- catch (ProtocolError &e)
- {
- if (_sensorIsUTM30LX && _enable_checksum_workaround)
- {
- // Here comes the UTM-30LX workaround
- char* hasComment = strstr(buffer, "<-");
- if (hasComment != NULL)
- {
- int newBytesToConsider = hasComment - buffer;
- if (_verbose)
- {
- _err_output << "Sensor::" << __func__ <<
- "() Performing UTM-30LX II response "
- "checksum workaround: trying with " <<
- newBytesToConsider << " bytes.\n";
- }
- if (newBytesToConsider < 1)
- {
- throw InsufficientBytesError(newBytesToConsider,
- linelength);
- }
-
- checkSum = _confirm_checksum(buffer, newBytesToConsider,
- static_cast<int>(buffer[checksumIndex]));
- }
- else
- // Workaround is disabled - rethrow
- throw;
- }
- else
- // Not a workaround-compatible error - rethrow
- throw;
- }
-
- // Null out the semi-colon (if there) and checksum
- buffer[bytesToConsider] = '\0';
-
- return bytesToConsider;
-}
-
-// Reads lines until the number specified has passed.
-void Sensor::_skip_lines(int count)
-{
- if (_verbose)
- _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
- " lines.\n";
- if (_port->SkipUntil(0x0A, count) < 0)
- throw ReadError(18);
-}
-
-// Sends a command with optional parameters and checks that the echo of the
-// command and parameters sent are correct, and that the returned status code
-// is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two
-// bytes of extra_ok (for SCIP2).
-// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string
-// for SCIP2.
-// 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 statusCode = -1;
- char response[17];
-
- // Flush first to clear out the dregs of any previous commands
- _port->Flush();
-
- if (_scip_version == 1)
- {
- 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)
- throw WriteError(19);
- if (param_length > 0)
- {
- if (_port->Write(param, param_length) < param_length)
- throw WriteError(20);
- }
- if (_port->Write("\n", 1) < 1)
- throw WriteError(21);
-
- // Read back the response (should get at least 4 bytes , possibly up to
- // 16 including \n's depending on the parameters): cmd[0] params \n
- // status \n
- int statusIndex = 2 + param_length;
- _read_line(response, 2 + param_length);
- _read_line(&response[statusIndex], 2);
- // First make sure that the echoed command matches
- if (response[0] != cmd[0])
- {
- char temp_cmd[2];
- temp_cmd[0] = cmd[0];
- temp_...
[truncated message content] |
|
From: <gb...@us...> - 2010-08-04 03:20:51
|
Revision: 502
http://gearbox.svn.sourceforge.net/gearbox/?rev=502&view=rev
Author: gbiggs
Date: 2010-08-04 03:20:45 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Fixed polymorphism in exceptions
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 02:51:43 UTC (rev 501)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 03:20:45 UTC (rev 502)
@@ -302,6 +302,20 @@
}
+BaseError::BaseError(unsigned int desc_code, char const* error_type)
+ : _desc_code(desc_code)
+{
+ strncpy(_error_type, error_type, 32);
+}
+
+
+BaseError::BaseError(BaseError const& rhs)
+ : _desc_code(rhs.desc_code())
+{
+ strncpy(_error_type, rhs.error_type(), 32);
+}
+
+
const char* BaseError::what() throw()
{
ss << _error_type << " (" << _desc_code << "): " <<
@@ -327,12 +341,19 @@
UnknownLineError::UnknownLineError(char const* const line)
- : ProtocolError(27)
+ : ProtocolError(27, "UnknownLineError")
{
strncpy(_line, line, 128);
}
+UnknownLineError::UnknownLineError(UnknownLineError const& rhs)
+ : ProtocolError(rhs)
+{
+ strncpy(_line, rhs.line(), 128);
+}
+
+
const char* UnknownLineError::what() throw()
{
ProtocolError::what();
@@ -342,13 +363,21 @@
ParseError::ParseError(char const* const line, char const* const type)
- : ProtocolError(28)
+ : ProtocolError(28, "ParseError")
{
strncpy(_line, line, 128);
strncpy(_type, type, 16);
}
+ParseError::ParseError(ParseError const& rhs)
+ : ProtocolError(rhs)
+{
+ strncpy(_line, rhs.line(), 128);
+ strncpy(_type, rhs.type(), 16);
+}
+
+
const char* ParseError::what() throw()
{
ProtocolError::what();
@@ -410,41 +439,5 @@
return ss.str().c_str();
}
-
-// Static member initialisation
-
-char const* BaseError::_error_type = "BaseError";
-char const* LogicError::_error_type = "LogicError";
-char const* RuntimeError::_error_type = "RuntimeError";
-char const* ReadError::_error_type = "ReadError";
-char const* WriteError::_error_type = "WriteError";
-char const* BaudrateError::_error_type = "BaudrateError";
-char const* CloseError::_error_type = "CloseError";
-char const* NoDestinationError::_error_type = "NoDestinationError";
-char const* FirmwareError::_error_type = "FirmwareError";
-char const* ScipVersionError::_error_type = "ScipVersionError";
-char const* UnknownScipVersionError::_error_type = "UnknownScipVersionError";
-char const* UnsupportedError::_error_type = "UnsupportedError";
-char const* ArgError::_error_type = "ArgError";
-char const* NoDataError::_error_type = "NoDataError";
-char const* NotSerialError::_error_type = "NotSerialError";
-char const* IndexError::_error_type = "IndexError";
-char const* MotorSpeedError::_error_type = "MotorSpeedError";
-char const* StartStepError::_error_type = "StartStepError";
-char const* EndStepError::_error_type = "EndStepError";
-char const* ProtocolError::_error_type = "ProtocolError";
-char const* ChecksumError::_error_type = "ChecksumError";
-char const* DataCountError::_error_type = "DataCountError";
-char const* MisplacedLineFeedError::_error_type = "MisplacedLineFeedError";
-char const* UnknownLineError::_error_type = "UnknownLineError";
-char const* ParseError::_error_type = "ParseError";
-char const* MissingFirmSpecError::_error_type = "MissingFirmSpecError";
-char const* ResponseError::_error_type = "ResponseError";
-char const* Scip1ResponseError::_error_type = "Scip1ResponseError";
-char const* CommandEchoError::_error_type = "CommandEchoError";
-char const* ParamEchoError::_error_type = "ParamEchoError";
-char const* InsufficientBytesError::_error_type = "InsufficientBytesError";
-char const* LineLengthError::_error_type = "LineLengthError";
-
}; // namespace hokuyo_aist
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 02:51:43 UTC (rev 501)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 03:20:45 UTC (rev 502)
@@ -59,17 +59,16 @@
/** @brief Hokuyo error constructor.
@param desc_code Index into the error descriptions string table. */
- BaseError(unsigned int desc_code)
- : _desc_code(desc_code)
- {}
- BaseError(BaseError const& rhs)
- : _desc_code(rhs.desc_code())
- {}
+ BaseError(unsigned int desc_code, char const* error_type);
+ BaseError(BaseError const& rhs);
virtual ~BaseError() throw() {};
virtual unsigned int desc_code() const throw()
{ return _desc_code; }
+ virtual char const* error_type() const throw()
+ { return _error_type; }
+
virtual const char* what() throw();
protected:
@@ -79,7 +78,7 @@
/** Formatted description of the error. */
std::stringstream ss;
/** Static string representation of the error. */
- static char const* _error_type;
+ char _error_type[32];
}; //class BaseError
@@ -91,13 +90,12 @@
@param desc_code Index into the error descriptions string table. */
LogicError(unsigned int desc_code)
- : BaseError(desc_code)
+ : BaseError(desc_code, "LogicError")
{}
+ LogicError(unsigned int desc_code, char const* error_type)
+ : BaseError(desc_code, error_type)
+ {}
virtual ~LogicError() throw() {};
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class LogicError
@@ -109,8 +107,11 @@
@param desc_code Index into the error descriptions string table. */
RuntimeError(unsigned int desc_code)
- : BaseError(desc_code)
+ : BaseError(desc_code, "RuntimeError")
{}
+ RuntimeError(unsigned int desc_code, char const* error_type)
+ : BaseError(desc_code, error_type)
+ {}
virtual ~RuntimeError() throw() {};
protected:
@@ -127,12 +128,8 @@
@param desc_code Index into the error descriptions string table. */
ReadError(unsigned int desc_code)
- : RuntimeError(desc_code)
+ : RuntimeError(desc_code, "ReadError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ReadError
@@ -144,12 +141,8 @@
@param desc_code Index into the error descriptions string table. */
WriteError(unsigned int desc_code)
- : RuntimeError(desc_code)
+ : RuntimeError(desc_code, "WriteError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class WriteError
@@ -161,8 +154,11 @@
@param baud The bad baud rate. */
BaudrateError(unsigned int baud)
- : RuntimeError(6), _baud(baud)
+ : RuntimeError(6, "BaudrateError"), _baud(baud)
{}
+ BaudrateError(BaudrateError const& rhs)
+ : RuntimeError(rhs), _baud(rhs.baud())
+ {}
unsigned int baud() const throw()
{ return _baud; }
@@ -172,8 +168,6 @@
protected:
/** Baud rate that caused the error. */
unsigned int _baud;
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class BaudrateError
@@ -182,12 +176,8 @@
{
public:
CloseError()
- : RuntimeError(3)
+ : RuntimeError(3, "CloseError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class CloseError
@@ -196,12 +186,8 @@
{
public:
NoDestinationError()
- : RuntimeError(11)
+ : RuntimeError(11, "NoDestinationError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class NoDestinationError
@@ -210,12 +196,8 @@
{
public:
FirmwareError()
- : RuntimeError(23)
+ : RuntimeError(23, "FirmwareError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class FirmwareError
@@ -224,12 +206,8 @@
{
public:
ScipVersionError()
- : RuntimeError(22)
+ : RuntimeError(22, "ScipVersionError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ScipVersionError
@@ -238,12 +216,8 @@
{
public:
UnknownScipVersionError()
- : RuntimeError(4)
+ : RuntimeError(4, "UnknownScipVersionError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class UnknownScipVersionError
@@ -255,12 +229,8 @@
@param desc_code Index into the error descriptions string table. */
UnsupportedError(unsigned int desc_code)
- : RuntimeError(desc_code)
+ : RuntimeError(desc_code, "UnsupportedError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class UnsupportedError
@@ -272,13 +242,12 @@
@param desc_code Index into the error descriptions string table. */
ArgError(unsigned int desc_code)
- : RuntimeError(desc_code)
+ : RuntimeError(desc_code, "ArgError")
{}
+ ArgError(unsigned int desc_code, char const* error_type)
+ : RuntimeError(desc_code, error_type)
+ {}
virtual ~ArgError() throw() {};
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ArgError
@@ -287,12 +256,8 @@
{
public:
NoDataError()
- : RuntimeError(13)
+ : RuntimeError(13, "NoDataError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class NoDataError
@@ -301,12 +266,8 @@
{
public:
NotSerialError()
- : RuntimeError(5)
+ : RuntimeError(5, "NotSerialError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class NotSerialError
@@ -315,12 +276,8 @@
{
public:
IndexError()
- : RuntimeError(2)
+ : RuntimeError(2, "IndexError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class IndexError
@@ -329,12 +286,8 @@
{
public:
MotorSpeedError()
- : ArgError(9)
+ : ArgError(9, "MotorSpeedError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class MotorSpeedError
@@ -343,12 +296,8 @@
{
public:
StartStepError()
- : ArgError(14)
+ : ArgError(14, "StartStepError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class StartStepError
@@ -357,12 +306,8 @@
{
public:
EndStepError()
- : ArgError(15)
+ : ArgError(15, "EndStepError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class EndStepError
@@ -374,12 +319,12 @@
@param desc_code Index into the error descriptions string table. */
ProtocolError(unsigned int desc_code)
- : RuntimeError(desc_code)
+ : RuntimeError(desc_code, "ProtocolError")
{}
+ ProtocolError(unsigned int desc_code, char const* error_type)
+ : RuntimeError(desc_code, error_type)
+ {}
virtual ~ProtocolError() throw() {}
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ProtocolError
@@ -392,8 +337,13 @@
@param expected The expected checksum.
@param calculated The calculated checksum. */
ChecksumError(int expected, int calculated)
- : ProtocolError(24), _expected(expected), _calculated(calculated)
+ : ProtocolError(24, "ChecksumError"), _expected(expected),
+ _calculated(calculated)
{}
+ ChecksumError(ChecksumError const& rhs)
+ : ProtocolError(rhs), _expected(rhs.expected()),
+ _calculated(rhs.calculated())
+ {}
virtual int expected() const throw()
{ return _expected; }
@@ -408,8 +358,6 @@
int _expected;
/** Calculated checksum value. */
int _calculated;
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ProtocolError
@@ -418,12 +366,8 @@
{
public:
DataCountError()
- : ProtocolError(25)
+ : ProtocolError(25, "DataCountError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class DataCountError
@@ -432,12 +376,8 @@
{
public:
MisplacedLineFeedError()
- : ProtocolError(26)
+ : ProtocolError(26, "MisplacedLineFeedError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class MisplacedLineFeedError
@@ -449,6 +389,7 @@
@param line The mystery line that was not understood. */
UnknownLineError(char const* const line);
+ UnknownLineError(UnknownLineError const& rhs);
virtual char const* const line() const throw()
{ return _line; }
@@ -458,8 +399,6 @@
protected:
/** The mystery line. */
char _line[128];
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class UnknownLineError
@@ -472,6 +411,7 @@
@param line The line that could not be parsed.
@param type The type of line that was expected. */
ParseError(char const* const line, char const* const type);
+ ParseError(ParseError const& rhs);
virtual char const* const line() const throw()
{ return _line; }
@@ -486,8 +426,6 @@
char _line[128];
/** The type of line. */
char _type[16];
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ParseError
@@ -496,12 +434,8 @@
{
public:
MissingFirmSpecError()
- : ProtocolError(29)
+ : ProtocolError(29, "MissingFirmSpecError")
{}
-
- protected:
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class MissingFirmSpecError
@@ -514,11 +448,19 @@
@param line The two-byte error code received.
@param type The command that caused the error. */
ResponseError(char const* const error, char const* const cmd)
- : ProtocolError(30)
+ : ProtocolError(30, "ResponseError")
{
_error[0] = error[0]; _error[1] = error[1];
_cmd[0] = cmd[0]; _cmd[1] = cmd[1];
}
+ ResponseError(ResponseError const& rhs)
+ : ProtocolError(rhs)
+ {
+ _error[0] = rhs.error_code()[0];
+ _error[1] = rhs.error_code()[1];
+ _cmd[0] = rhs.cmd_code()[0];
+ _cmd[1] = rhs.cmd_code()[1];
+ }
/// Get the two-byte error code as a non-null-terminated array.
virtual char const* const error_code() const throw()
@@ -535,8 +477,6 @@
char _error[2];
/** Command that triggered the error, from SCIP2 (two bytes). */
char _cmd[2];
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ResponseError
@@ -549,8 +489,13 @@
@param line The two-byte error code received.
@param type The command that caused the error. */
Scip1ResponseError(char error, char cmd)
- : ProtocolError(30), _error(error), _cmd(cmd)
+ : ProtocolError(30, "Scip1ResponseError"),
+ _error(error), _cmd(cmd)
{}
+ Scip1ResponseError(Scip1ResponseError const& rhs)
+ : ProtocolError(rhs), _error(rhs.error_code()),
+ _cmd(rhs.cmd_code())
+ {}
/// Get the one-byte error code.
virtual char error_code() const throw()
@@ -567,8 +512,6 @@
char _error;
/** Command that triggered the error, from SCIP2 (two bytes). */
char _cmd;
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class Scip1ResponseError
@@ -581,11 +524,19 @@
@param line The two-byte command code expected.
@param type The two-byte command echo received. */
CommandEchoError(char const* const cmd, char const* const echo)
- : ProtocolError(31)
+ : ProtocolError(31, "CommandEchoError")
{
_cmd[0] = cmd[0]; _cmd[1] = cmd[1];
_echo[0] = echo[0]; _echo[1] = echo[1];
}
+ CommandEchoError(CommandEchoError const& rhs)
+ : ProtocolError(rhs)
+ {
+ _cmd[0] = rhs.cmd_code()[0];
+ _cmd[1] = rhs.cmd_code()[1];
+ _echo[0] = rhs.cmd_echo()[0];
+ _echo[1] = rhs.cmd_echo()[1];
+ }
/// Get the two-byte command code as a non-null-terminated array.
virtual char const* const cmd_code() const throw()
@@ -602,8 +553,6 @@
char _cmd[2];
/** Received echo. */
char _echo[2];
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class CommandEchoError
@@ -615,10 +564,16 @@
@param type The two-byte command code sent. */
ParamEchoError(char const* const cmd)
- : ProtocolError(32)
+ : ProtocolError(32, "ParamEchoError")
{
_cmd[0] = cmd[0]; _cmd[1] = cmd[1];
}
+ ParamEchoError(ParamEchoError const& rhs)
+ : ProtocolError(rhs)
+ {
+ _cmd[0] = rhs.cmd_code()[0];
+ _cmd[1] = rhs.cmd_code()[1];
+ }
/// Get the two-byte command code as a non-null-terminated array.
virtual char const* const cmd_code() const throw()
@@ -629,8 +584,6 @@
protected:
/** Command that triggered the error, from SCIP2 (two bytes). */
char _cmd[2];
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class ParamEchoError
@@ -643,8 +596,13 @@
@param num The number of bytes received.
@param line_length The length of the line. */
InsufficientBytesError(int num, int line_length)
- : ProtocolError(33), _num(num), _line_length(line_length)
+ : ProtocolError(33, "InsufficientBytesError"),
+ _num(num), _line_length(line_length)
{}
+ InsufficientBytesError(InsufficientBytesError const& rhs)
+ : ProtocolError(rhs), _num(rhs.num()),
+ _line_length(rhs.line_length())
+ {}
virtual int num() const throw()
{ return _num; }
@@ -659,8 +617,6 @@
int _num;
/** Length of the line. */
int _line_length;
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class InsufficientBytesError
@@ -673,8 +629,13 @@
@param num The number of bytes received.
@param line_length The length of the line. */
LineLengthError(int length, int expected)
- : ProtocolError(34), _length(length), _expected(expected)
+ : ProtocolError(34, "LineLengthError"),
+ _length(length), _expected(expected)
{}
+ LineLengthError(LineLengthError const& rhs)
+ : ProtocolError(rhs), _length(rhs.length()),
+ _expected(rhs.expected())
+ {}
virtual int length() const throw()
{ return _length; }
@@ -689,8 +650,6 @@
int _length;
/** The expected line length. */
int _expected;
- /** Static string representation of the error. */
- static char const* _error_type;
}; // class LineLengthError
}; // namespace hokuyo_aist
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-08-04 02:51:50
|
Revision: 501
http://gearbox.svn.sourceforge.net/gearbox/?rev=501&view=rev
Author: gbiggs
Date: 2010-08-04 02:51:43 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Added ability to redirect error output
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-04 00:21:24 UTC (rev 500)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-04 02:51:43 UTC (rev 501)
@@ -274,30 +274,30 @@
{
std::stringstream ss;
- ss << "Vendor: " << vendor << std::endl;
- ss << "Product: " << product << std::endl;
- ss << "Firmware: " << firmware << std::endl;
- ss << "Protocol: " << protocol << std::endl;
- ss << "Serial: " << serial << std::endl;
- ss << "Model: " << model << std::endl;
+ ss << "Vendor: " << vendor << '\n';
+ ss << "Product: " << product << '\n';
+ ss << "Firmware: " << firmware << '\n';
+ ss << "Protocol: " << protocol << '\n';
+ ss << "Serial: " << serial << '\n';
+ ss << "Model: " << model << '\n';
ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range <<
- "mm" << std::endl;
+ "mm\n";
ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
- scanable_steps << std::endl;
+ scanable_steps << '\n';
ss << "First step: " << first_step << "\tFront step: " << front_step <<
- "\tLast step: " << last_step << std::endl;
- ss << "Resolution: " << resolution << " radians/step" << std::endl;
+ "\tLast step: " << last_step << '\n';
+ ss << "Resolution: " << resolution << " radians/step\n";
ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
- max_angle << " radians" << std::endl;
- ss << "Standard motor speed: " << standard_speed << "rpm" << std::endl;
+ max_angle << " radians\n";
+ ss << "Standard motor speed: " << standard_speed << "rpm\n";
ss << "Power status: " << (power ? "On" : "Off") <<
- "\tMeasurement state: " << measure_state << std::endl;
+ "\tMeasurement state: " << measure_state << '\n';
ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
- ")\tBaud rate: " << baud << "bps" << std::endl;
- ss << "Time stamp: " << time << "ms" << std::endl;
- ss << "Sensor diagnostic: " << sensor_diagnostic << std::endl;
+ ")\tBaud rate: " << baud << "bps\n";
+ ss << "Time stamp: " << time << "ms\n";
+ ss << "Sensor diagnostic: " << sensor_diagnostic << '\n';
return ss.str();
}
@@ -557,29 +557,29 @@
{
std::stringstream ss;
- ss << _length << " readings:" << std::endl;
+ ss << _length << " readings:\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _ranges[ii] << "\t";
if (_intensities != NULL)
{
- ss << std::endl << _length << " intensities:" << std::endl;
+ ss << std::endl << _length << " intensities:\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _intensities[ii] << "\t";
}
- ss << std::endl;
+ ss << '\n';
if (_error)
{
- ss << "Detected data errors:" << std::endl;
+ ss << "Detected data errors:\n";
for (unsigned int ii = 0; ii < _length; ii++)
{
if (_ranges[ii] < 20)
- ss << ii << ": " << error_code_to_string(_ranges[ii]) <<
- std::endl;
+ ss << ii << ": " <<
+ error_code_to_string(_ranges[ii]) << '\n';
}
}
else
- ss << "No data errors." << std::endl;
- ss << "Time stamp: " << _time << std::endl;
+ ss << "No data errors.\n";
+ ss << "Time stamp: " << _time << '\n';
return ss.str();
}
@@ -662,13 +662,21 @@
///////////////////////////////////////////////////////////////////////////////
Sensor::Sensor()
- : _port(NULL), _scip_version(2), _verbose(false), _sensorIsUTM30LX(false),
- _enable_checksum_workaround(false), _ignore_unknowns(false), _min_angle(0.0),
- _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
- _front_step(0)
+ : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
+ _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+ _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
}
+Sensor::Sensor(std::ostream& err_output)
+ : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
+ _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+ _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
+{
+}
+
Sensor::~Sensor()
{
if (_port != NULL)
@@ -679,18 +687,18 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Creating and opening port using options: " << port_options <<
- std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Creating and opening port using options: " <<
+ port_options << '\n';
}
_port = flexiport::CreatePort(port_options);
_port->Open();
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Connected using " <<
- _port->GetPortType() << " connection." << std::endl;
- std::cerr << _port->GetStatus();
+ _err_output << "Sensor::" << __func__ << "() Connected using " <<
+ _port->GetPortType() << " connection.\n";
+ _err_output << _port->GetStatus();
}
_port->Flush();
@@ -705,18 +713,18 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Creating and opening port using options: " << port_options <<
- std::endl;
+ '\n';
}
_port = flexiport::CreatePort(port_options);
_port->Open();
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Connected using " <<
- _port->GetPortType() << " connection." << std::endl;
- std::cerr << _port->GetStatus();
+ _err_output << "Sensor::" << __func__ << "() Connected using " <<
+ _port->GetPortType() << " connection.\n";
+ _err_output << _port->GetStatus();
}
_port->Flush();
@@ -732,8 +740,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Failed to connect at the default baud rate." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Failed to connect at the default baud rate.\n";
}
if (_port->GetPortType() == "serial")
{
@@ -753,8 +761,8 @@
// loop and be happy
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Connected at " << bauds[ii] << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Connected at " << bauds[ii] << '\n';
}
return bauds[ii];
}
@@ -765,9 +773,8 @@
// Last baud rate, give up and rethrow
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Failed to connect at any baud rate." <<
- std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Failed to connect at any baud rate.\n";
}
throw;
}
@@ -779,8 +786,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Port is not serial, cannot probe." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Port is not serial, cannot probe.\n";
}
throw;
}
@@ -797,8 +804,7 @@
if (!_port)
throw CloseError();
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Closing connection." <<
- std::endl;
+ _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
delete _port;
_port = NULL;
}
@@ -817,15 +823,15 @@
if (on)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Turning laser on." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Turning laser on.\n";
_send_command("L", "1", 1, NULL);
}
else
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Turning laser off." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Turning laser off.\n";
_send_command("L", "0", 1, NULL);
}
_skip_lines(1);
@@ -835,15 +841,15 @@
if (on)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Turning laser on." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Turning laser on.\n";
_send_command("BM", NULL, 0, "02");
}
else
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Turning laser off." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Turning laser off.\n";
_send_command("QT", NULL, 0, "02");
}
_skip_lines(1);
@@ -898,8 +904,8 @@
else if (_scip_version == 2)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Resetting laser." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Resetting laser.\n";
_send_command("RS", NULL, 0, NULL);
_skip_lines(1);
}
@@ -921,8 +927,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Reseting motor speed to default." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Reseting motor speed to default.\n";
}
buffer[0] = '0';
buffer[1] = '0';
@@ -932,8 +938,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Reseting motor speed to default." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Reseting motor speed to default.\n";
}
buffer[0] = '9';
buffer[1] = '9';
@@ -943,8 +949,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Setting motor speed to ratio " << speed << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Setting motor speed to ratio " << speed << '\n';
}
number_to_string(speed, buffer, 2);
}
@@ -964,16 +970,16 @@
if (on)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Switching to high sensitivity." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Switching to high sensitivity.\n";
_send_command("HS", "1", 1, "02");
}
else
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Switching to normal sensitivity." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Switching to normal sensitivity.\n";
}
_send_command("HS", "0", 1, "02");
}
@@ -992,9 +998,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Getting sensor information using SCIP version 1." <<
- std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Getting sensor information using SCIP version 1.\n";
}
info->set_defaults();
@@ -1035,8 +1040,8 @@
if (atoi(info->firmware.c_str()) >= 3)
{
if (_verbose)
- std::cerr << "SCIP1 Firmware line for parsing: " <<
- info->firmware << std::endl;
+ _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.
@@ -1062,9 +1067,9 @@
info->calculate_values();
if (_verbose)
{
- std::cerr << "Retrieved sensor info (hard-coded, not "
- "enough values):" << std::endl;
- std::cerr << info->as_string();
+ _err_output << "Retrieved sensor info (hard-coded, not "
+ "enough values):\n";
+ _err_output << info->as_string();
}
}
else
@@ -1084,9 +1089,8 @@
if (_verbose)
{
- std::cerr << "Retrieved sensor info (from FIRM line):" <<
- std::endl;
- std::cerr << info->as_string();
+ _err_output << "Retrieved sensor info (from FIRM line):\n";
+ _err_output << info->as_string();
}
}
}
@@ -1097,9 +1101,8 @@
info->calculate_values();
if (_verbose)
{
- std::cerr << "Retrieved sensor info (hard-coded):" <<
- std::endl;
- std::cerr << info->as_string();
+ _err_output << "Retrieved sensor info (hard-coded):\n";
+ _err_output << info->as_string();
}
}
}
@@ -1107,9 +1110,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Getting sensor information using SCIP version 2." <<
- std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Getting sensor information using SCIP version 2.\n";
}
info->set_defaults();
@@ -1138,8 +1140,8 @@
info->calculate_values();
if (_verbose)
{
- std::cerr << "Retrieved sensor info:" << std::endl;
- std::cerr << info->as_string();
+ _err_output << "Retrieved sensor info:\n";
+ _err_output << info->as_string();
}
}
else
@@ -1153,8 +1155,8 @@
else if (_scip_version == 2)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Retrieving time from laser." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Retrieving time from laser.\n";
_send_command("TM", "0", 1, NULL);
_send_command("TM", "1", 1, NULL);
char buffer[7];
@@ -1187,10 +1189,10 @@
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " ranges between " << start_step << " and " <<
end_step << " with a cluster count of " << cluster_count <<
- std::endl;
+ '\n';
}
if (_scip_version == 1)
@@ -1247,9 +1249,9 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+ _err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << std::endl;
+ end_angle << " is step " << end_step << '\n';
}
// Get the data
@@ -1277,10 +1279,10 @@
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " new ranges between " << start_step << " and " <<
end_step << " with a cluster count of " << cluster_count <<
- std::endl;
+ '\n';
}
// Send the command to ask for the most recent range data from start_step to end_step
@@ -1307,9 +1309,9 @@
_read_line_with_check(response, 4);
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() MD data prefix status: " << response[0] << response[1] <<
- std::endl;
+ '\n';
}
// Check the status code is OK - should only get 99 here
if (response[0] != '9' || response[1] != '9')
@@ -1358,9 +1360,9 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+ _err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << std::endl;
+ end_angle << " is step " << end_step << '\n';
}
// Get the data
@@ -1388,10 +1390,10 @@
unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
num_steps << " new ranges between " << start_step << " and " <<
end_step << " with a cluster count of " << cluster_count <<
- std::endl;
+ '\n';
}
// Send the command to ask for the most recent range data with
@@ -1418,9 +1420,9 @@
_read_line_with_check(response, 4);
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() ME data prefix status: " << response[0] << response[1] <<
- std::endl;
+ '\n';
}
// Check the status code is OK - should only get 99 here
if (response[0] != '9' || response[1] != '9')
@@ -1469,9 +1471,9 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+ _err_output << "Sensor::" << __func__ << "() Start angle " <<
start_angle << " is step " << start_step << ", end angle " <<
- end_angle << " is step " << end_step << std::endl;
+ end_angle << " is step " << end_step << '\n';
}
// Get the data
@@ -1535,8 +1537,8 @@
SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Reading up to " <<
- maxlength << " bytes." << std::endl;
+ _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)
@@ -1550,9 +1552,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
- "() Reading exactly " << expected_length << " bytes." <<
- std::endl;
+ _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)
@@ -1567,10 +1568,10 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Read " << linelength <<
- " bytes." << std::endl;
- std::cerr << "Sensor::" << __func__ << "() Line is " << buffer <<
- std::endl;
+ _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
+ " bytes.\n";
+ _err_output << "Sensor::" << __func__ << "() Line is " << buffer <<
+ '\n';
}
return linelength - 1; // Line feed not included
}
@@ -1613,9 +1614,9 @@
int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Considering " <<
+ _err_output << "Sensor::" << __func__ << "() Considering " <<
bytesToConsider << " bytes for checksum from a line length of " <<
- linelength << " bytes." << std::endl;
+ linelength << " bytes.\n";
}
if (bytesToConsider < 1)
throw InsufficientBytesError(bytesToConsider, linelength);
@@ -1637,10 +1638,10 @@
int newBytesToConsider = hasComment - buffer;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Performing UTM-30LX II response "
"checksum workaround: trying with " <<
- newBytesToConsider << " bytes." << std::endl;
+ newBytesToConsider << " bytes.\n";
}
if (newBytesToConsider < 1)
{
@@ -1670,8 +1671,8 @@
void Sensor::_skip_lines(int count)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Skipping " << count <<
- " lines." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
+ " lines.\n";
if (_port->SkipUntil(0x0A, count) < 0)
throw ReadError(18);
}
@@ -1698,9 +1699,9 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Writing in SCIP1 mode. Command is " << cmd[0] <<
- ", parameters length is " << param_length << std::endl;
+ ", parameters length is " << param_length << '\n';
}
// Write the command
if (_port->Write(cmd, 1) < 1)
@@ -1744,9 +1745,9 @@
// Next up, check the status byte
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Command response status: " << response[statusIndex] <<
- std::endl;
+ '\n';
}
if (response[statusIndex] != '0')
{
@@ -1775,9 +1776,9 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Writing in SCIP2 mode. Command is " << cmd <<
- ", parameters length is " << param_length << std::endl;
+ ", parameters length is " << param_length << '\n';
}
// Write the command
if (_port->Write(cmd, 2) < 2)
@@ -1806,9 +1807,9 @@
_read_line_with_check(response, 4);
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Command response status: " << response[0] << response[1] <<
- std::endl;
+ '\n';
}
// Check the status code is OK
response[2] = '\0';
@@ -1848,8 +1849,8 @@
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Testing SCIP protocol version." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Testing SCIP protocol version.\n";
// Try SCIP version 2 first by sending an info command
try
{
@@ -1859,8 +1860,8 @@
{
// That didn't work too well...
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Initial SCIP version 2 test failed." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Initial SCIP version 2 test failed.\n";
scip2Failed = true;
}
@@ -1898,9 +1899,9 @@
throw FirmwareError();
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Firmware major version is " << majorVer <<
- std::endl;
+ '\n';
}
// Dump the rest of the V command result (one of these will be the
// empty last line)
@@ -1911,9 +1912,9 @@
if (majorVer < 3)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Firmware does not support SCIP version 2; using SCIP "
- "version 1." << std::endl;
+ "version 1.\n";
return;
}
// Otherwise we can try SCIP version 2
@@ -1932,9 +1933,9 @@
catch (BaseError)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Could not change to SCIP version 2; using SCIP "
- "version 1." << std::endl;
+ "version 1.\n";
return;
}
// There'll be a trailing line on the end
@@ -1942,8 +1943,8 @@
// Changed to SCIP version 2
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Using SCIP version 2." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Using SCIP version 2.\n";
_scip_version = 2;
return;
}
@@ -1956,8 +1957,8 @@
// Dump the rest of the result
_skip_lines(6);
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Using SCIP version 2." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Using SCIP version 2.\n";
return;
}
@@ -1968,8 +1969,8 @@
void Sensor::_get_defaults()
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ <<
- "() Getting default values." << std::endl;
+ _err_output << "Sensor::" << __func__ <<
+ "() Getting default values.\n";
// Get the laser's info
SensorInfo info;
@@ -1984,10 +1985,10 @@
_max_range = info.max_range;
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Got default values: " << _min_angle << " " << _max_angle <<
" " << _resolution << " " << _first_step << " " << _last_step <<
- " " << _front_step << " " << _max_range << std::endl;
+ " " << _front_step << " " << _max_range << '\n';
}
}
@@ -2103,8 +2104,8 @@
void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " ranges." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " ranges.\n";
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
@@ -2139,8 +2140,8 @@
}
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Read " <<
- currentStep << " ranges." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Read " <<
+ currentStep << " ranges.\n";
if (currentStep != num_steps)
throw DataCountError();
}
@@ -2148,8 +2149,8 @@
void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
{
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " ranges." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " ranges.\n";
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
@@ -2217,16 +2218,16 @@
}
if (data->_ranges[currentStep] > _max_range)
{
- std::cerr << "WARNING: Sensor::" << __func__ <<
+ _err_output << "WARNING: Sensor::" << __func__ <<
"() Value at step " << currentStep <<
" beyond maximum range: " <<
data->_ranges[currentStep] << " (raw bytes: ";
if (splitCount != 0)
- std::cerr << splitValue[0] << splitValue[1] <<
- splitValue[2] << ")" << std::endl;
+ _err_output << splitValue[0] << splitValue[1] <<
+ splitValue[2] << ")\n";
else
- std::cerr << buffer[0] << buffer[1] << buffer[2] <<
- ")" << std::endl;
+ _err_output << buffer[0] << buffer[1] << buffer[2] <<
+ ")\n";
}
else if (data->_ranges[currentStep] < 20)
data->_error = true;
@@ -2238,8 +2239,8 @@
}
if (_verbose)
- std::cerr << "Sensor::" << __func__ << "() Read " <<
- currentStep << " ranges." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Read " <<
+ currentStep << " ranges.\n";
if (currentStep != num_steps)
throw DataCountError();
}
@@ -2249,8 +2250,8 @@
{
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Reading " <<
- num_steps << " ranges and intensities." << std::endl;
+ _err_output << "Sensor::" << __func__ << "() Reading " <<
+ num_steps << " ranges and intensities.\n";
}
// This will automatically take care of whether it actually needs to
@@ -2332,16 +2333,16 @@
if (data->_ranges[currentRange] > _max_range &&
!nextIsIntensity)
{
- std::cerr << "WARNING: Sensor::" << __func__ <<
+ _err_output << "WARNING: Sensor::" << __func__ <<
"() Value at step " << currentRange <<
" beyond maximum range: " <<
data->_ranges[currentRange] << " (raw bytes: ";
if (splitCount != 0)
- std::cerr << splitValue[0] << splitValue[1] <<
- splitValue[2] << ")" << std::endl;
+ _err_output << splitValue[0] << splitValue[1] <<
+ splitValue[2] << ")\n";
else
- std::cerr << buffer[0] << buffer[1] << buffer[2] <<
- ")" << std::endl;
+ _err_output << buffer[0] << buffer[1] << buffer[2] <<
+ ")\n";
}
else if (data->_ranges[currentRange] < 20)
data->_error = true;
@@ -2360,9 +2361,9 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ << "() Read " <<
+ _err_output << "Sensor::" << __func__ << "() Read " <<
currentRange << " ranges and " << currentIntensity <<
- " intensities (expected " << num_steps << ")." << std::endl;
+ " intensities (expected " << num_steps << ").\n";
}
if (currentRange != num_steps || currentIntensity != num_steps)
throw DataCountError();
@@ -2382,11 +2383,11 @@
if (_verbose)
{
- std::cerr << "Sensor::" << __func__ <<
+ _err_output << "Sensor::" << __func__ <<
"() Calculated checksum = " << checksum << " (" <<
static_cast<char>(checksum) << "), given checksum = " <<
expected_sum << " (" << static_cast<char> (expected_sum) <<
- ")" << std::endl;
+ ")\n";
}
if (checksum != expected_sum)
throw ChecksumError(expected_sum, checksum);
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-04 00:21:24 UTC (rev 500)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-04 02:51:43 UTC (rev 501)
@@ -280,6 +280,7 @@
{
public:
Sensor();
+ Sensor(std::ostream& err_output);
~Sensor();
/// @brief Open the laser scanner and begin scanning.
@@ -506,6 +507,7 @@
private:
flexiport::Port *_port;
+ std::ostream& _err_output;
uint8_t _scip_version;
bool _verbose, _sensorIsUTM30LX, _enable_checksum_workaround,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-08-04 00:21:32
|
Revision: 500
http://gearbox.svn.sourceforge.net/gearbox/?rev=500&view=rev
Author: gbiggs
Date: 2010-08-04 00:21:24 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Re-worked error handling
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/src/hokuyo_aist/test/example.cpp
Added Paths:
-----------
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-08-04 00:21:24 UTC (rev 500)
@@ -12,8 +12,8 @@
if (build)
include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
- set (hdrs hokuyo_aist.h)
- set (srcs hokuyo_aist.cpp)
+ set (hdrs hokuyo_aist.h hokuyo_errors.h)
+ set (srcs hokuyo_aist.cpp hokuyo_errors.cpp)
if (WIN32)
if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
Modified: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-08-04 00:21:24 UTC (rev 500)
@@ -23,6 +23,7 @@
using namespace std;
#include <hokuyo_aist/hokuyo_aist.h>
+#include <hokuyo_aist/hokuyo_errors.h>
int main(int argc, char **argv)
{
@@ -76,9 +77,9 @@
// Close the laser
laser.close ();
}
- catch (hokuyo_aist::BaseError e)
+ catch (hokuyo_aist::BaseError &e)
{
- cerr << "Caught exception: (" << e.code () << ") " << e.what () << endl;
+ cerr << "Caught exception: " << e.what () << endl;
return 1;
}
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-04 00:21:24 UTC (rev 500)
@@ -18,6 +18,7 @@
*/
#include "hokuyo_aist.h"
+#include "hokuyo_errors.h"
using namespace hokuyo_aist;
#include <flexiport/flexiport.h>
@@ -189,242 +190,6 @@
// Utility functions
///////////////////////////////////////////////////////////////////////////////
-std::string scip1_error_to_string(char error, char cmd)
-{
- return std::string("No error descriptions available");
-}
-
-// error must be null-terminated
-std::string scip2_error_to_string(const char *error, const char *cmd)
-{
- std::stringstream ss;
-
- // Check for non-errors
- if (error[0] == '0' && error[1] == '0')
- return "Status OK - 00";
- else if (error[0] == '9' && error[1] == '9')
- return "Status OK - 99";
-
- // Check for universal errors
- 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')
- return "Buffer shortage or command repeated that is already processed";
- else if (error[0] == '0' && error[1] == 'C')
- return "Command with insufficient parameters 1";
- else if (error[0] == '0' && error[1] == 'D')
- return "Undefined command 1";
- else if (error[0] == '0' && error[1] == 'E')
- return "Undefined command 2";
- else if (error[0] == '0' && error[1] == 'F')
- return "Command with insufficient parameters 2";
- else if (error[0] == '0' && error[1] == 'G')
- return "String character in command exceeds 16 letters";
- else if (error[0] == '0' && error[1] == 'H')
- return "String character has invalid letters";
- 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')
- {
- switch (error_code)
- {
- case 1:
- return "Unable to control due to laser malfunction";
- case 2:
- return "Laser is already on";
- }
- }
-// No info in the manual for this.
-// else if (cmd[0] == 'Q' && cmd[1] == 'T')
-// {
-// switch (error_code)
-// {
-// default:
-// std::stringstream ss;
-// ss << "Unknown error code " << error_code <<
-// " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
- else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
- (cmd[0] == 'G' && cmd[1] == 'S'))
- {
- switch (error_code)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 10:
- return "Laser is off";
- default:
- if (error_code >= 50)
- ss << "Hardware error: " << error_code;
- else
- ss << "Unknown error code " << error_code <<
- " for command " << cmd[0] << cmd[1];
-
- return ss.str();
- }
- }
- else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
- (cmd[0] == 'M' && cmd[1] == 'S'))
- {
- switch (error_code)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 6:
- return "Scan interval has non-numeric value";
- case 7:
- return "Number of scans is non-numeric";
- default:
- 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)
- ss << "Hardware error: " << error_code;
- else if (error_code == 98)
- {
- return "Resumption of processing after confirming normal "
- "laser opteration. This function is not yet supported "
- "by hokuyo_aist.";
- }
- else
- ss << "Unknown error code " << error_code <<
- " for command " << cmd[0] << cmd[1];
-
- return ss.str();
- }
- }
- else if (cmd[0] == 'T' && cmd[1] == 'M')
- {
- switch (error_code)
- {
- case 1:
- return "Invalid control code";
- case 2:
- return "Adjust mode on command received when sensor's adjust "
- "mode is already on";
- case 3:
- return "Adjust mode off command received when sensor's adjust "
- "mode is already off";
- case 4:
- return "Adjust mode is off when requested time";
- }
- }
- else if (cmd[0] == 'S' && cmd[1] == 'S')
- {
- switch (error_code)
- {
- case 1:
- return "Baud rate has non-numeric value";
- case 2:
- return "Invalid baud rate";
- case 3:
- return "Sensor is already running at that baud rate";
- case 4:
- return "Not compatible with the sensor model";
- }
- }
- else if (cmd[0] == 'C' && cmd[1] == 'R')
- {
- switch (error_code)
- {
- case 1:
- return "Invalid speed";
- case 2:
- return "Speed is out of range";
- case 3:
- return "Motor is already running at that speed";
- case 4:
- return "Not compatible with the sensor model";
- }
- }
- else if (cmd[0] == 'H' && cmd[1] == 'S')
- {
- switch (error_code)
- {
- case 1:
- return "Parameter error";
- case 2:
- return "Already running in the set mode";
- case 3:
- return "Not compatible with the sensor model";
- }
- }
-// No info in the manual for this.
-// else if (cmd[0] == 'R' && cmd[1] == 'S')
-// {
-// switch (error_code)
-// {
-// case :
-// return "";
-// default:
-// std::stringstream ss;
-// ss << "Unknown error code " << error_code <<
-// " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
-// No info in the manual for this.
-// else if (cmd[0] == 'V' && cmd[1] == 'V')
-// {
-// switch (error_code)
-// {
-// case :
-// return "";
-// }
-// }
-// No info in the manual for this.
-// else if (cmd[0] == 'P' && cmd[1] == 'P')
-// {
-// switch (error_code)
-// {
-// case :
-// return "";
-// }
-// }
-// No info in the manual for this.
-// else if (cmd[0] == 'I' && cmd[1] == 'I')
-// {
-// switch (error_code)
-// {
-// case :
-// return "";
-// }
-// }
- else
- {
- ss << "Unknown command: " << cmd[0] << cmd[1];
- return ss.str();
- }
-
- // Known commands with unknown error codes fall through to here
- ss << "Unknown error code " << error_code << " for command " << cmd[0] <<
- cmd[1];
- return ss.str();
-}
-
unsigned int decode_2_byte_value(char *data)
{
unsigned int byte1, byte2;
@@ -471,46 +236,6 @@
}
///////////////////////////////////////////////////////////////////////////////
-// BaseError class
-///////////////////////////////////////////////////////////////////////////////
-
-std::string BaseError::as_string() const throw()
-{
- switch (_error_code)
- {
- case HOKUYO_ERR_READ:
- return "HOKUYO_ERR_READ";
- case HOKUYO_ERR_WRITE:
- return "HOKUYO_ERR_WRITE";
- case HOKUYO_ERR_PROTOCOL:
- return "HOKUYO_ERR_PROTOCOL";
- case HOKUYO_ERR_CHANGEBAUD:
- return "HOKUYO_ERR_CHANGEBAUD";
- case HOKUYO_ERR_CONNECT_FAILED:
- return "HOKUYO_ERR_CONNECT_FAILED";
- case HOKUYO_ERR_CLOSE_FAILED:
- return "HOKUYO_ERR_CLOSE_FAILED";
- case HOKUYO_ERR_NODESTINATION:
- return "HOKUYO_ERR_NODESTINATION";
- case HOKUYO_ERR_BADFIRMWARE:
- return "HOKUYO_ERR_BADFIRMWARE";
- case HOKUYO_ERR_SCIPVERSION:
- return "HOKUYO_ERR_SCIPVERSION";
- case HOKUYO_ERR_MEMORY:
- return "HOKUYO_ERR_MEMORY";
- case HOKUYO_ERR_UNSUPPORTED:
- return "HOKUYO_ERR_UNSUPPORTED";
- case HOKUYO_ERR_BADARG:
- return "HOKUYO_ERR_BADARG";
- case HOKUYO_ERR_NODATA:
- return "HOKUYO_ERR_NODATA";
- case HOKUYO_ERR_NOTSERIAL:
- return "HOKUYO_ERR_NOTSERIAL";
- }
- return "UNKNOWN_ERROR";
-}
-
-///////////////////////////////////////////////////////////////////////////////
// SensorInfo class
///////////////////////////////////////////////////////////////////////////////
@@ -824,7 +549,7 @@
uint32_t ScanData::operator[](unsigned int index)
{
if (index >= _length)
- throw BaseError(HOKUYO_ERR_BADARG, "Invalid data index.");
+ throw IndexError();
return _ranges[index];
}
@@ -1070,7 +795,7 @@
void Sensor::close()
{
if (!_port)
- throw BaseError(HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
+ throw CloseError();
if (_verbose)
std::cerr << "Sensor::" << __func__ << "() Closing connection." <<
std::endl;
@@ -1124,7 +849,7 @@
_skip_lines(1);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
// This function assumes that both the port and the laser scanner are already
@@ -1133,8 +858,7 @@
{
if (_port->GetPortType() != "serial")
{
- throw BaseError(HOKUYO_ERR_NOTSERIAL,
- "Cannot change baud rate of non-serial connection.");
+ throw NotSerialError();
}
char newBaud[13];
@@ -1143,9 +867,7 @@
if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
baud != 250000 && baud != 500000 && baud != 750000)
{
- std::stringstream ss;
- ss << "Bad baud rate: " << baud << std::endl;
- throw BaseError(HOKUYO_ERR_BADARG, ss.str());
+ throw BaudrateError(baud);
}
number_to_string(baud, newBaud, 6);
@@ -1166,16 +888,13 @@
reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
void Sensor::reset()
{
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the reset command.");
- }
+ throw UnsupportedError(7);
else if (_scip_version == 2)
{
if (_verbose)
@@ -1185,21 +904,18 @@
_skip_lines(1);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
void Sensor::set_motor_speed(unsigned int speed)
{
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set motor speed command.");
- }
+ throw UnsupportedError(8);
else if (_scip_version == 2)
{
// Sanity check the value
if (speed > 10 && speed != 99)
- throw BaseError(HOKUYO_ERR_BADARG, "Invalid motor speed.");
+ throw MotorSpeedError();
char buffer[3];
if (speed == 0)
{
@@ -1236,16 +952,13 @@
_skip_lines(1);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
void Sensor::set_high_sensitivity(bool on)
{
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the high sensitivity command.");
- }
+ throw UnsupportedError(10);
else if (_scip_version == 2)
{
if (on)
@@ -1267,14 +980,13 @@
_skip_lines(1);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
void Sensor::get_sensor_info(SensorInfo *info)
{
if (info == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No info object provided.");
+ throw NoDestinationError();
if (_scip_version == 1)
{
@@ -1431,16 +1143,13 @@
}
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
}
unsigned int Sensor::get_time()
{
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get time command.");
- }
+ throw UnsupportedError(12);
else if (_scip_version == 2)
{
if (_verbose)
@@ -1456,7 +1165,7 @@
return decode_4_byte_value(buffer);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
return 0;
}
@@ -1465,8 +1174,7 @@
int end_step, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDestinationError();
char buffer[11];
memset(buffer, 0, sizeof(char) * 11);
@@ -1508,15 +1216,14 @@
// 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)
- throw BaseError(HOKUYO_ERR_NODATA,
- "No data received. Check data error code.");
+ throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
// sending the GD command
_read_3_byte_range_data(data, num_steps);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
return data->_length;
}
@@ -1525,8 +1232,7 @@
double end_angle, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDataError();
// Calculate the given angles in steps, rounding towards _front_step
int start_step, end_step;
@@ -1535,9 +1241,9 @@
// Check the steps are within the allowable range
if (start_step < _first_step || start_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+ throw StartStepError();
if (end_step < _first_step || end_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+ throw EndStepError();
if (_verbose)
{
@@ -1554,14 +1260,10 @@
int end_step, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDestinationError();
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges command.");
- }
+ throw UnsupportedError(16);
else if (_scip_version == 2)
{
char buffer[14];
@@ -1596,19 +1298,11 @@
_read_line(response, 16); // Size is command(2)+params(13)+new line(1)
// Check the echo is correct
if (response[0] != 'M' || response[1] != 'D')
- {
- std::stringstream ss;
- ss << "Incorrect data prefix: MD" << " != " << response[0] <<
- response[1];
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw CommandEchoError("MD", response);
// Then compare the parameters
buffer[12] = '0'; // There will be zero scans remaining after this one
if (memcmp(&response[2], buffer, 13) != 0)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Incorrect paramaters prefix for MD data.");
- }
+ throw ParamEchoError("MD");
// The next line should be the status line
_read_line_with_check(response, 4);
if (_verbose)
@@ -1623,10 +1317,7 @@
// There is an extra line feed after an error status (signalling
// end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad status for MD data: " << response[0] << response[1] <<
- " " << scip2_error_to_string(response, "MD");
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ResponseError(response, "MD");
}
// Now the actual data will arrive
@@ -1634,15 +1325,14 @@
// 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)
- throw BaseError(HOKUYO_ERR_NODATA,
- "No data received. Check data error code.");
+ throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
// sending the MD command
_read_3_byte_range_data(data, num_steps);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
return data->_length;
}
@@ -1651,13 +1341,9 @@
double start_angle, double end_angle, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDestinationError();
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges command.");
- }
+ throw UnsupportedError(16);
// Calculate the given angles in steps, rounding towards _front_step
int start_step, end_step;
@@ -1666,9 +1352,9 @@
// Check the steps are within the allowable range
if (start_step < _first_step || start_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+ throw StartStepError();
if (end_step < _first_step || end_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+ throw EndStepError();
if (_verbose)
{
@@ -1685,15 +1371,10 @@
int start_step, int end_step, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDestinationError();
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges and "
- "intensities command.");
- }
+ throw UnsupportedError(17);
else if (_scip_version == 2)
{
char buffer[14];
@@ -1729,18 +1410,10 @@
_read_line(response, 16); // Size is command(2)+params(13)+new line(1)
// Check the echo is correct
if (response[0] != 'M' || response[1] != 'E')
- {
- std::stringstream ss;
- ss << "Incorrect data prefix: ME" << " != " << response[0] <<
- response[1];
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw CommandEchoError("ME", response);
// Then compare the parameters
if (memcmp(&response[2], buffer, 13) != 0)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Incorrect paramaters prefix for ME data.");
- }
+ throw ParamEchoError("ME");
// The next line should be the status line
_read_line_with_check(response, 4);
if (_verbose)
@@ -1755,10 +1428,7 @@
// There is an extra line feed after an error status (signalling
// end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad status for ME data: " << response[0] << response[1] <<
- " " << scip2_error_to_string(response, "MD");
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ResponseError(response, "ME");
}
// Now the actual data will arrive
@@ -1766,15 +1436,14 @@
// 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)
- throw BaseError(HOKUYO_ERR_NODATA,
- "No data received. Check data error code.");
+ throw NoDataError();
data->_time = decode_4_byte_value(buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're
// sending the ME command
_read_3_byte_range_and_intensity_data(data, num_steps);
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
return data->_length;
}
@@ -1783,14 +1452,9 @@
double start_angle, double end_angle, unsigned int cluster_count)
{
if (data == NULL)
- throw BaseError(HOKUYO_ERR_NODESTINATION,
- "No data destination provided.");
+ throw NoDestinationError();
if (_scip_version == 1)
- {
- throw BaseError(HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges and "
- "intensities command.");
- }
+ throw UnsupportedError(17);
// Calculate the given angles in steps, rounding towards _front_step
int start_step, end_step;
@@ -1799,9 +1463,9 @@
// Check the steps are within the allowable range
if (start_step < _first_step || start_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+ throw StartStepError();
if (end_step < _first_step || end_step > _last_step)
- throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+ throw EndStepError();
if (_verbose)
{
@@ -1876,11 +1540,9 @@
}
// We need to get at least 1 byte in a line: the line feed.
if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
- throw BaseError(HOKUYO_ERR_READ,
- "Timed out trying to read a line.");
+ throw ReadError(0);
else if (linelength == 0)
- throw BaseError(HOKUYO_ERR_READ,
- "No data received when trying to read a line.");
+ throw ReadError(1);
// Replace the line feed with a NULL
buffer[linelength - 1] = '\0';
}
@@ -1894,19 +1556,11 @@
}
// expected_length+1 for the NULL
if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
- throw BaseError(HOKUYO_ERR_READ,
- "Timed out trying to read a line.");
+ throw ReadError(0);
else if (linelength == 0)
- throw BaseError(HOKUYO_ERR_READ,
- "No data received when trying to read a line.");
+ throw ReadError(1);
else if (linelength < expected_length)
- {
- std::stringstream ss;
- ss << "Sensor::" << __func__ <<
- "() Got an incorrect line length: " << linelength << " != " <<
- expected_length;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw LineLengthError(linelength, expected_length);
// Replace the line feed with a NULL
buffer[linelength - 1] = '\0';
}
@@ -1964,13 +1618,7 @@
linelength << " bytes." << std::endl;
}
if (bytesToConsider < 1)
- {
- std::stringstream ss;
- ss << "Not enough bytes to calculate checksum with: " <<
- bytesToConsider << " bytes (line length is " << linelength <<
- " bytes).";
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw InsufficientBytesError(bytesToConsider, linelength);
int checkSum = 0;
try
@@ -1978,10 +1626,9 @@
checkSum = _confirm_checksum(buffer, bytesToConsider,
static_cast<int>(buffer[checksumIndex]));
}
- catch (BaseError &e)
+ catch (ProtocolError &e)
{
- if (e.code() == HOKUYO_ERR_PROTOCOL && _sensorIsUTM30LX &&
- _enable_checksum_workaround)
+ if (_sensorIsUTM30LX && _enable_checksum_workaround)
{
// Here comes the UTM-30LX workaround
char* hasComment = strstr(buffer, "<-");
@@ -1997,11 +1644,8 @@
}
if (newBytesToConsider < 1)
{
- std::stringstream ss;
- ss << "Not enough bytes to calculate checksum with: " <<
- newBytesToConsider << " bytes (line length is " <<
- linelength << " bytes).";
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw InsufficientBytesError(newBytesToConsider,
+ linelength);
}
checkSum = _confirm_checksum(buffer, newBytesToConsider,
@@ -2029,7 +1673,7 @@
std::cerr << "Sensor::" << __func__ << "() Skipping " << count <<
" lines." << std::endl;
if (_port->SkipUntil(0x0A, count) < 0)
- throw BaseError(HOKUYO_ERR_READ, "Timed out while skipping.");
+ throw ReadError(18);
}
// Sends a command with optional parameters and checks that the echo of the
@@ -2060,17 +1704,14 @@
}
// Write the command
if (_port->Write(cmd, 1) < 1)
- throw BaseError(HOKUYO_ERR_WRITE,
- "Failed to write command byte.");
+ throw WriteError(19);
if (param_length > 0)
{
if (_port->Write(param, param_length) < param_length)
- throw BaseError(HOKUYO_ERR_WRITE,
- "Failed to write command parameters.");
+ throw WriteError(20);
}
if (_port->Write("\n", 1) < 1)
- throw BaseError(HOKUYO_ERR_WRITE,
- "Failed to write termination character.");
+ throw WriteError(21);
// Read back the response (should get at least 4 bytes , possibly up to
// 16 including \n's depending on the parameters): cmd[0] params \n
@@ -2081,18 +1722,23 @@
// First make sure that the echoed command matches
if (response[0] != cmd[0])
{
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- std::string("Incorrect command echo: ") + cmd[0] +
- std::string(" != ") + response[0]);
+ char temp_cmd[2];
+ temp_cmd[0] = cmd[0];
+ temp_cmd[1] = '\0';
+ char temp_echo[2];
+ temp_echo[0] = response[0];
+ temp_echo[1] = '\0';
+ throw CommandEchoError(temp_cmd, temp_echo);
}
// Then compare the parameters
if (param_length > 0)
{
if (memcmp(&response[1], param, param_length) != 0)
{
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- std::string("Incorrect paramaters echo for command ") +
- cmd[0]);
+ char temp_cmd[2];
+ temp_cmd[0] = cmd[0];
+ temp_cmd[1] = '\0';
+ throw ParamEchoError(temp_cmd);
}
}
// Next up, check the status byte
@@ -2111,11 +1757,7 @@
// There is an extra line feed after an error status
// (signalling end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad response to " << cmd[0] << " command: " <<
- " " << scip1_error_to_string(response[statusIndex],
- cmd[0]);
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw Scip1ResponseError(response[statusIndex], cmd[0]);
}
}
else
@@ -2123,11 +1765,7 @@
// There is an extra line feed after an error status
// (signalling end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad response to " << cmd[0] << " command: " <<
- response[statusIndex] << " " <<
- scip1_error_to_string(response[statusIndex], cmd[0]);
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw Scip1ResponseError(response[statusIndex], cmd[0]);
}
}
statusCode = atoi(&response[statusIndex]);
@@ -2143,36 +1781,25 @@
}
// Write the command
if (_port->Write(cmd, 2) < 2)
- throw BaseError(HOKUYO_ERR_WRITE, "Failed to write command byte.");
+ throw WriteError(19);
if (param_length > 0)
{
if (_port->Write(param, param_length) < param_length)
- throw BaseError(HOKUYO_ERR_WRITE,
- "Failed to write command parameters.");
+ throw WriteError(20);
}
if (_port->Write("\n", 1) < 1)
- throw BaseError(HOKUYO_ERR_WRITE,
- "Failed to write termination character.");
+ 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])
- {
- std::stringstream ss;
- ss << "Incorrect command echo: " << cmd << " != " << response[0] <<
- response[1];
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw CommandEchoError(cmd, response);
// Then compare the parameters
if (param_length > 0)
{
if (memcmp(&response[2], param, param_length) != 0)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- std::string("Incorrect paramaters echo for command ") +
- cmd);
- }
+ throw ParamEchoError(cmd);
}
// The next line should be the status line
@@ -2195,11 +1822,7 @@
// There is an extra line feed after an error status
// (signalling end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad response to " << cmd << " command: " <<
- response[0] << response[1] << " " <<
- scip2_error_to_string(response, cmd);
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ResponseError(response, cmd);
}
}
else
@@ -2207,18 +1830,14 @@
// There is an extra line feed after an error status
// (signalling end of message)
_skip_lines(1);
- std::stringstream ss;
- ss << "Bad response to " << cmd << " command: " <<
- response[0] << response[1] << " " <<
- scip2_error_to_string(response, cmd);
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ResponseError(response, cmd);
}
}
statusCode = atoi(response);
// All OK, data starts at beginning of port's buffer
}
else
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw UnknownScipVersionError();
return statusCode;
}
@@ -2258,8 +1877,7 @@
}
catch (BaseError)
{
- throw BaseError(HOKUYO_ERR_SCIPVERSION,
- "SCIP versions 1 and 2 failed.");
+ throw ScipVersionError();
}
// Skip the vendor and product info
_skip_lines(2);
@@ -2269,10 +1887,7 @@
_read_line(buffer);
if (strncmp(buffer, "FIRM:", 5) != 0)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "'FIRM:' was not found when checking firmware version.");
- }
+ throw MissingFirmSpecError();
// Pull out the major version number
// Note that although lasers such as the UTM-30LX appear to use a
// different firmware version format, that doesn't matter because they
@@ -2280,8 +1895,7 @@
// they do, it's an uncaught error.
int majorVer = strtol(&buffer[5], NULL, 10);
if (errno == ERANGE)
- throw BaseError(HOKUYO_ERR_BADFIRMWARE,
- "Out-of-range firmware version.");
+ throw FirmwareError();
if (_verbose)
{
std::cerr << "Sensor::" << __func__ <<
@@ -2348,7 +1962,7 @@
}
// Fallback case if didn't find a good SCIP version and return above
- throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw ScipVersionError();
}
void Sensor::_get_defaults()
@@ -2402,11 +2016,7 @@
else if (strncmp(buffer, "SERI", 4) == 0)
info->serial = &buffer[5]; // Serial number
else if (!_ignore_unknowns)
- {
- std::stringstream ss;
- ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw UnknownLineError(buffer);
}
void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
@@ -2429,11 +2039,7 @@
else if (strncmp(buffer, "SCAN", 4) == 0)
info->standard_speed = atoi(&buffer[5]);
else if (!_ignore_unknowns)
- {
- std::stringstream ss;
- ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw UnknownLineError(buffer);
}
void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
@@ -2455,9 +2061,7 @@
// Unchanged motor speed
if (sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
{
- std::stringstream ss;
- ss << "Motor speed line parse failed: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ParseError(buffer, "Motor speed");
}
info->speed_level = 0;
}
@@ -2468,9 +2072,7 @@
if (sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
&info->speed) != 2)
{
- std::stringstream ss;
- ss << "Motor speed line parse failed: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+ throw ParseError(buffer, "Motor speed");
}
}
}
@@ -2485,29 +2087,17 @@
info->baud = 0;
}
else if (sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
- {
- std::stringstream ss;
- ss << "Baud rate line parse failed: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw ParseError(buffer, "Baud rate");
}
else if (strncmp(buffer, "TIME", 4) == 0)
{
if (sscanf(buffer, "TIME:%x", &info->time) != 1)
- {
- std::stringstream ss;
- ss << "Timestamp line parse failed: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw ParseError(buffer, "Timestamp");
}
else if (strncmp(buffer, "STAT", 4) == 0)
info->sensor_diagnostic = &buffer[5];
else if (!_ignore_unknowns)
- {
- std::stringstream ss;
- ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ throw UnknownLineError(buffer);
}
void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
@@ -2539,8 +2129,7 @@
if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a data block? Why?
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Found line feed in a data block.");
+ throw MisplacedLineFeedError();
}
data->_ranges[currentStep] = decode_2_byte_value(&buffer[ii]);
if (data->_ranges[currentStep] < 20)
@@ -2553,8 +2142,7 @@
std::cerr << "Sensor::" << __func__ << "() Read " <<
currentStep << " ranges." << std::endl;
if (currentStep != num_steps)
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Read less range readings than were asked for.");
+ throw DataCountError();
}
void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
@@ -2589,8 +2177,7 @@
if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Found line feed in a data block.");
+ throw MisplacedLineFeedError();
}
if (ii == numBytesInLine - 2) // Short 1 byte
{
@@ -2654,10 +2241,7 @@
std::cerr << "Sensor::" << __func__ << "() Read " <<
currentStep << " ranges." << std::endl;
if (currentStep != num_steps)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Read a different number of range readings than were asked for.");
- }
+ throw DataCountError();
}
void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
@@ -2695,8 +2279,7 @@
if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Found line feed in a data block.");
+ throw MisplacedLineFeedError();
}
if (ii == numBytesInLine - 2) // Short 1 byte
{
@@ -2782,43 +2365,34 @@
" intensities (expected " << num_steps << ")." << std::endl;
}
if (currentRange != num_steps || currentIntensity != num_steps)
- {
- throw BaseError(HOKUYO_ERR_PROTOCOL,
- "Read a different number of range or intensity readings than "
- "were asked for.");
- }
+ throw DataCountError();
}
int Sensor::_confirm_checksum(const char *buffer, int length,
int expected_sum)
{
- int checkSum = 0;
+ int checksum = 0;
// Start by adding the byte values
for (int ii = 0; ii < length; ii++)
- checkSum += buffer[ii];
+ checksum += buffer[ii];
// Take the lowest 6 bits
- checkSum &= 0x3F;
+ checksum &= 0x3F;
// Add 0x30
- checkSum += 0x30;
+ checksum += 0x30;
if (_verbose)
{
std::cerr << "Sensor::" << __func__ <<
- "() Calculated checksum = " << checkSum << " (" <<
- static_cast<char>(checkSum) << "), given checksum = " <<
+ "() Calculated checksum = " << checksum << " (" <<
+ static_cast<char>(checksum) << "), given checksum = " <<
expected_sum << " (" << static_cast<char> (expected_sum) <<
")" << std::endl;
}
- if (checkSum != expected_sum)
- {
- std::stringstream ss;
- ss << "Invalid checksum - given: " << expected_sum <<
- ", calculated: " << checkSum;
- throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
- }
+ if (checksum != expected_sum)
+ throw ChecksumError(expected_sum, checksum);
- return checkSum;
+ return checksum;
}
-} // namespace hokuyo_aist
+}; // namespace hokuyo_aist
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2010-08-04 00:21:24 UTC (rev 500)
@@ -17,8 +17,8 @@
* http://www.opensource.org/licenses/eclipse-1.0.txt
*/
-#ifndef __HOKUYO_AIST_H
-#define __HOKUYO_AIST_H
+#ifndef HOKUYO_AIST_H__
+#define HOKUYO_AIST_H__
#include <flexiport/port.h>
#include <string>
@@ -45,35 +45,6 @@
namespace hokuyo_aist
{
-/// Hokuyo general error class.
-class HOKUYO_AIST_EXPORT BaseError : public std::exception
-{
- public:
- /** @brief Hokuyo error constructor.
-
- @param code Error code of the error.
- @param desc Description of the error. */
- BaseError(unsigned int code, std::string desc)
- : _error_code(code), _error_desc(desc)
- {}
- virtual ~BaseError() throw() {};
-
- virtual unsigned int code() const throw()
- { return _error_code; }
-
- virtual const char* what() const throw()
- { return _error_desc.c_str(); }
-
- virtual std::string as_string() const throw();
-
- private:
- /** Error code. */
- unsigned int _error_code;
- /** Error description. */
- std::string _error_desc;
-}; //class BaseError
-
-
#if defined(WIN32)
// Exporting data members on Windows is a bloody pain (they have to be
// initialised somewhere else), so we'll use #define's on Win32 and lose the
@@ -569,4 +540,5 @@
/** @} */
-#endif // _ _HOKUYO_AIST_H
+#endif // HOKUYO_AIST_H__
+
Added: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp 2010-08-04 00:21:24 UTC (rev 500)
@@ -0,0 +1,450 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * hokuyo_aist Hokuyo laser scanner driver.
+ *
+ * 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 file is part of hokuyo_aist.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * http://www.opensource.org/licenses/eclipse-1.0.txt
+ */
+
+#include "hokuyo_errors.h"
+
+#include <cstdlib>
+#include <cstring>
+
+namespace hokuyo_aist
+{
+
+// error must be null-terminated
+std::string scip2_error_to_string(char const* const error,
+ char const* const cmd)
+{
+ std::stringstream ss;
+
+ // Check for non-errors
+ if (error[0] == '0' && error[1] == '0')
+ return "Status OK - 00";
+ else if (error[0] == '9' && error[1] == '9')
+ return "Status OK - 99";
+
+ // Check for universal errors
+ 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')
+ return "Buffer shortage or command repeated that is already processed";
+ else if (error[0] == '0' && error[1] == 'C')
+ return "Command with insufficient parameters 1";
+ else if (error[0] == '0' && error[1] == 'D')
+ return "Undefined command 1";
+ else if (error[0] == '0' && error[1] == 'E')
+ return "Undefined command 2";
+ else if (error[0] == '0' && error[1] == 'F')
+ return "Command with insufficient parameters 2";
+ else if (error[0] == '0' && error[1] == 'G')
+ return "String character in command exceeds 16 letters";
+ else if (error[0] == '0' && error[1] == 'H')
+ return "String character has invalid letters";
+ 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')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Unable to control due to laser malfunction";
+ case 2:
+ return "Laser is already on";
+ }
+ }
+// No info in the manual for this.
+// else if (cmd[0] == 'Q' && cmd[1] == 'T')
+// {
+// switch (error_code)
+// {
+// default:
+// std::stringstream ss;
+// ss << "Unknown error code " << error_code <<
+// " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
+ else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+ (cmd[0] == 'G' && cmd[1] == 'S'))
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 10:
+ return "Laser is off";
+ default:
+ if (error_code >= 50)
+ ss << "Hardware error: " << error_code;
+ else
+ ss << "Unknown error code " << error_code <<
+ " for command " << cmd[0] << cmd[1];
+
+ return ss.str();
+ }
+ }
+ else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+ (cmd[0] == 'M' && cmd[1] == 'S'))
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 6:
+ return "Scan interval has non-numeric value";
+ case 7:
+ return "Number of scans is non-numeric";
+ default:
+ 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)
+ ss << "Hardware error: " << error_code;
+ else if (error_code == 98)
+ {
+ return "Resumption of processing after confirming normal "
+ "laser opteration. This function is not yet supported "
+ "by hokuyo_aist.";
+ }
+ else
+ ss << "Unknown error code " << error_code <<
+ " for command " << cmd[0] << cmd[1];
+
+ return ss.str();
+ }
+ }
+ else if (cmd[0] == 'T' && cmd[1] == 'M')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Invalid control code";
+ case 2:
+ return "Adjust mode on command received when sensor's adjust "
+ "mode is already on";
+ case 3:
+ return "Adjust mode off command received when sensor's adjust "
+ "mode is already off";
+ case 4:
+ return "Adjust mode is off when requested time";
+ }
+ }
+ else if (cmd[0] == 'S' && cmd[1] == 'S')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Baud rate has non-numeric value";
+ case 2:
+ return "Invalid baud rate";
+ case 3:
+ return "Sensor is already running at that baud rate";
+ case 4:
+ return "Not compatible with the sensor model";
+ }
+ }
+ else if (cmd[0] == 'C' && cmd[1] == 'R')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Invalid speed";
+ case 2:
+ return "Speed is out of range";
+ case 3:
+ return "Motor is already running at that speed";
+ case 4:
+ return "Not compatible with the sensor model";
+ }
+ }
+ else if (cmd[0] == 'H' && cmd[1] == 'S')
+ {
+ switch (error_code)
+ {
+ case 1:
+ return "Parameter error";
+ case 2:
+ return "Already running in the set mode";
+ case 3:
+ return "Not compatible with the sensor model";
+ }
+ }
+// No info in the manual for this.
+// else if (cmd[0] == 'R' && cmd[1] == 'S')
+// {
+// switch (error_code)
+// {
+// case :
+// return "";
+// default:
+// std::stringstream ss;
+// ss << "Unknown error code " << error_code <<
+// " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'V' && cmd[1] == 'V')
+// {
+// switch (error_code)
+// {
+// case :
+// return "";
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'P' && cmd[1] == 'P')
+// {
+// switch (error_code)
+// {
+// case :
+// return "";
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'I' && cmd[1] == 'I')
+// {
+// switch (error_code)
+// {
+// case :
+// return "";
+// }
+// }
+ else
+ {
+ ss << "Unknown command: " << cmd[0] << cmd[1];
+ return ss.str();
+ }
+
+ // Known commands with unknown error codes fall through to here
+ ss << "Unknown error code " << error_code << " for command " << cmd[0] <<
+ cmd[1];
+ return ss.str();
+}
+
+
+std::string desc_code_to_string(unsigned int code)
+{
+ static char const* const descriptions[] = {
+ "Timed out trying to read a line",
+ "No data received when trying to read a line.",
+ "Invalid data index.",
+ "Port is not open.",
+ "Unknown SCIP version.",
+ "Cannot change baud rate of non-serial connection.",
+ "Bad baud rate: ",
+ "SCIP version 1 does not support the reset command.",
+ "SCIP version 1 does not support the set motor speed command.",
+ "Invalid motor speed.",
+ "SCIP version 1 does not support the high sensitivity command.",
+ "No info object provided.",
+ "SCIP version 1 does not support the get time command.",
+ "No data received. Check data error code.",
+ "Start step is out of range.",
+ "End step is out of range.",
+ "SCIP version 1 does not support the get new ranges command.",
+ "SCIP version 1 does not support the get new ranges and intensities command.",
+ "Timed out while skipping.",
+ "Failed to write command byte.",
+ "Failed to write command parameters.",
+ "Failed to write termination character.",
+ "SCIP versions 1 and 2 failed.",
+ "Out-of-range firmware version.",
+ "Invalid checksum: ",
+ "Read a different number of range or intensity readings than were asked for.",
+ "Found line feed in a data block.",
+ "Unknown line: ",
+ "Parse error: ",
+ "'FIRM:' was not found when checking firmware version.",
+ "Bad response.",
+ "Incorrect command echo.",
+ "Incorrect parameters echo for command.",
+ "Not enough bytes to calculate checksum.",
+ "Incorrect line length received.",
+ };
+
+ return std::string(descriptions[code]);
+}
+
+
+const char* BaseError::what() throw()
+{
+ ss << _error_type << " (" << _desc_code << "): " <<
+ desc_code_to_string(_desc_code);
+ return ss.str().c_str();
+}
+
+
+const char* BaudrateError::what() throw()
+{
+ RuntimeError::what();
+ ss << _baud;
+ return ss.str().c_str();
+}
+
+
+const char* ChecksumError::what() throw()
+{
+ ProtocolError::what();
+ ss << "expected " << _expected << ", calculated " << _calculated;
+ return ss.str().c_str();
+}
+
+
+UnknownLineError::UnknownLineError(char const* const line)
+ : ProtocolError(27)
+{
+ strncpy(_line, line, 128);
+}
+
+
+const char* UnknownLineError::what() throw()
+{
+ ProtocolError::what();
+ ss << _line;
+ return ss.str().c_str();
+}
+
+
+ParseError::ParseError(char const* const line, char const* const type)
+ : ProtocolError(28)
+{
+ strncpy(_line, line, 128);
+ strncpy(_type, type, 16);
+}
+
+
+const char* ParseError::what() throw()
+{
+ ProtocolError::what();
+ ss << "Line type: " << _type << ". Line: " << _line;
+ return ss.str().c_str();
+}
+
+
+const char* ResponseError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Command: " << _cmd[0] << _cmd[1];
+ ss << " Error : (" << _error[0] << _error[1] << ") " <<
+ scip2_error_to_string(_error, _cmd);
+ return ss.str().c_str();
+}
+
+
+const char* Scip1ResponseError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Command: " << _cmd;
+ ss << " Error : " << _error;
+ return ss.str().c_str();
+}
+
+
+const char* CommandEchoError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Command: " << _cmd[0] << _cmd[1];
+ ss << " Received echo: " << _echo[0] << _echo[1];
+ return ss.str().c_str();
+}
+
+
+const char* ParamEchoError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Command: " << _cmd[0] << _cmd[1];
+ return ss.str().c_str();
+}
+
+
+const char* InsufficientBytesError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Number of bytes: " << _num;
+ ss << " Line length: " << _line_length;
+ return ss.str().c_str();
+}
+
+
+const char* LineLengthError::what() throw()
+{
+ ProtocolError::what();
+ ss << " Received length: " << _length;
+ ss << " Expected line length: " << _expected;
+ return ss.str().c_str();
+}
+
+
+// Static member initialisation
+
+char const* BaseError::_error_type = "BaseError";
+char const* LogicError::_error_type = "LogicError";
+char const* RuntimeError::_error_type = "RuntimeError";
+char const* ReadError::_error_type = "ReadError";
+char const* WriteError::_error_type = "WriteError";
+char const* BaudrateError::_error_type = "BaudrateError";
+char const* CloseError::_error_type = "CloseError";
+char const* NoDestinationError::_error_type = "NoDestinationError";
+char const* FirmwareError::_error_type = "FirmwareError";
+char const* ScipVersionError::_error_type = "ScipVersionError";
+char const* UnknownScipVersionError::_error_type = "UnknownScipVersionError";
+char const* UnsupportedError::_error_type = "UnsupportedError";
+char const* ArgError::_error_type = "ArgError";
+char const* NoDataError::_error_type = "NoDataError";
+char const* NotSerialError::_error_type = "NotSerialError";
+char const* IndexError::_error_type = "IndexError";
+char const* MotorSpeedError::_error_type = "MotorSpeedError";
+char const* StartStepError::_error_type = "StartStepError";
+char const* EndStepError::_error_type = "EndStepError";
+char const* ProtocolError::_error_type = "ProtocolError";
+char const* ChecksumError::_error_type = "ChecksumError";
+char const* DataCountError::_error_type = "DataCountError";
+char const* MisplacedLineFeedError::_error_type = "MisplacedLineFeedError";
+char const* UnknownLineError::_error_type = "UnknownLineError";
+char const* ParseError::_error_type = "ParseError";
+char const* MissingFirmSpecError::_error_type = "MissingFirmSpecError";
+char const* ResponseError::_error_type = "ResponseError";
+char const* Scip1ResponseError::_error_type = "Scip1ResponseError";
+char const* CommandEchoError::_error_type = "CommandEchoError";
+char const* ParamEchoError::_error_type = "ParamEchoError";
+char const* InsufficientBytesError::_error_type = "InsufficientBytesError";
+char const* LineLengthError::_error_type = "LineLengthError";
+
+}; // namespace hokuyo_aist
+
Added: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h 2010-08-04 00:21:24 UTC (rev 500)
@@ -0,0 +1,701 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * hokuyo_...
[truncated message content] |
|
From: <gb...@us...> - 2010-08-02 12:06:35
|
Revision: 499
http://gearbox.svn.sourceforge.net/gearbox/?rev=499&view=rev
Author: gbiggs
Date: 2010-08-02 12:06:28 +0000 (Mon, 02 Aug 2010)
Log Message:
-----------
Re-working API in preparation for 2.0 release
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/src/hokuyo_aist/test/example.cpp
Modified: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-07-30 03:33:10 UTC (rev 498)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-08-02 12:06:28 UTC (rev 499)
@@ -57,28 +57,28 @@
try
{
- hokuyo_aist::HokuyoLaser laser; // Laser scanner object
+ hokuyo_aist::Sensor laser; // Laser scanner object
// Set the laser to verbose mode (so we see more information in the console)
if (verbose)
- laser.SetVerbose (true);
+ laser.set_verbose (true);
// Open the laser
- laser.Open (portOptions);
+ laser.open (portOptions);
// Turn the laser on
- laser.SetPower (true);
+ laser.set_power (true);
// Get some laser info
//cout << "Laser sensor information:" << endl;
- hokuyo_aist::HokuyoSensorInfo info;
- laser.GetSensorInfo (&info);
+ hokuyo_aist::SensorInfo info;
+ laser.get_sensor_info (&info);
cout << info.serial << endl;
// Close the laser
- laser.Close ();
+ laser.close ();
}
- catch (hokuyo_aist::HokuyoError e)
+ catch (hokuyo_aist::BaseError e)
{
- cerr << "Caught exception: (" << e.Code () << ") " << e.what () << endl;
+ cerr << "Caught exception: (" << e.code () << ") " << e.what () << endl;
return 1;
}
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-07-30 03:33:10 UTC (rev 498)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-08-02 12:06:28 UTC (rev 499)
@@ -32,10 +32,10 @@
#include <math.h>
#include <sstream>
#include <iostream>
-using namespace std;
+
using namespace flexiport;
-#if defined (WIN32)
+#if defined(WIN32)
#define __func__ __FUNCTION__
#endif
@@ -43,37 +43,39 @@
{
#ifndef M_PI
- const double M_PI = 3.14159265358979323846;
+ const double M_PI = 3.14159265358979323846;
#endif
// Convert radians to degrees
#ifndef RTOD
- inline double RTOD (double rad)
+ inline double RTOD(double rad)
{
return rad * 180.0 / M_PI;
}
#endif
// Convert degrees to radians
#ifndef DTOR
- inline double DTOR (double deg)
+ inline double DTOR(double deg)
{
return deg * M_PI / 180.0;
}
#endif
// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
+const unsigned int SCIP1_LINE_LENGTH = 66;
// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
+const unsigned int SCIP2_LINE_LENGTH = 67;
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
// SCIP protocol version 1 notes
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
+/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
+(x) = x byte block
- No checksum
- Host to sensor: Command | Parameters... | LF
- Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF
- - Where a block of data would take more than 64 bytes, a line feed is inserted every 64 bytes.
+ - Where a block of data would take more than 64 bytes, a line feed is inserted
+ every 64 bytes.
- Status 0 is OK, anything else is an error.
L Power
@@ -90,41 +92,47 @@
16 byte command block
V Version info
V|LF
- V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|Serial...|LF|LF
+ V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|
+ Serial...|LF|LF
2 byte command block
*/
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
// SCIP protocol version 2 notes
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
- - We don't use the string block (which can be up to 16 bytes) so it's marked as size 0 and ignored
- in the command definitions below.
+/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
+(x) = x byte block
+ - We don't use the string block (which can be up to 16 bytes) so it's marked
+ as size 0 and ignored in the command definitions below.
- Host to sensor: Command(2) | Parameters... | String(0) | LF
- - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) | Sum | LF
+ - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) |
+ Sum | LF
- Each data row: Data (max 64) | Sum | LF
- - Data rows are broken after a maximum of 64 bytes, each one having a checksum and a line feed.
+ - Data rows are broken after a maximum of 64 bytes, each one having a checksum
+ and a line feed.
- Status codes 00 and 99 are OK, anything else is an error.
- Checksum is calculated by... well, see the code.
- - BIG NOTE: The URG-30LX has a probable bug in its response to the II code whereby it does not
- include anything after and including "<-" in the checksum calculation.
+ - BIG NOTE: The URG-30LX has a probable bug in its response to the II code
+ whereby it does not include anything after and including "<-" in the
+ checksum calculation.
VV Version info
V|V|LF
- V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|Firmware...|;|Sum|LF|
- Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
+ V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|
+ Firmware...|;|Sum|LF|Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
3 byte command block
PP Specification info
P|P|LF
- P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|MinRange...|;|Sum|LF|
- TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|
- MotorSpeed...|;|Sum|LF|LF
+ P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|
+ MinRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
+ LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF
3 byte command block
II Status info
I|I|LF
- I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|MotorSpeed...|;|Sum|LF|
- Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|LF|Diagnostic...|;|Sum|LF|LF
+ I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|
+ MotorSpeed...|;|Sum|LF|Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|
+ LF|Diagnostic...|;|Sum|LF|LF
3 byte command block
BM Power on
B|M|LF
@@ -140,11 +148,13 @@
9 byte command block
MDMS Get new data
M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|
+ LF|Data...|LF|LF
16 byte command block
ME Get new data, including intensity data
M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|
+ Data...|LF|LF
16 byte command block
Featured only on UTM-30LX for getting intensity data.
GDGS Get latest data
@@ -175,19 +185,19 @@
DB
*/
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
// Utility functions
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
-string SCIP1ErrorToString (char error, char cmd)
+std::string scip1_error_to_string(char error, char cmd)
{
- return string ("No error descriptions available");
+ return std::string("No error descriptions available");
}
// error must be null-terminated
-string SCIP2ErrorToString (const char *error, const char *cmd)
+std::string scip2_error_to_string(const char *error, const char *cmd)
{
- stringstream ss;
+ std::stringstream ss;
// Check for non-errors
if (error[0] == '0' && error[1] == '0')
@@ -215,11 +225,11 @@
else if (error[0] == '0' && error[1] == 'I')
return "Sensor is now in firmware update mode";
- int errorCode = atoi (error);
+ int error_code = atoi(error);
if (cmd[0] == 'B' && cmd[1] == 'M')
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Unable to control due to laser malfunction";
@@ -230,18 +240,19 @@
// No info in the manual for this.
// else if (cmd[0] == 'Q' && cmd[1] == 'T')
// {
-// switch (errorCode)
+// switch (error_code)
// {
// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// std::stringstream ss;
+// ss << "Unknown error code " << error_code <<
+// " for command " << cmd[0] << cmd[1];
// return ss.str ();
// }
// }
else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
(cmd[0] == 'G' && cmd[1] == 'S'))
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Starting step has non-numeric value";
@@ -256,18 +267,19 @@
case 10:
return "Laser is off";
default:
- if (errorCode >= 50)
- ss << "Hardware error: " << errorCode;
+ if (error_code >= 50)
+ ss << "Hardware error: " << error_code;
else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+ ss << "Unknown error code " << error_code <<
+ " for command " << cmd[0] << cmd[1];
- return ss.str ();
+ return ss.str();
}
}
else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
(cmd[0] == 'M' && cmd[1] == 'S'))
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Starting step has non-numeric value";
@@ -284,41 +296,45 @@
case 7:
return "Number of scans is non-numeric";
default:
- if (errorCode >= 21 && errorCode <= 49)
+ if (error_code >= 21 && error_code <= 49)
{
return "Processing stopped to verify error. "
- "This function is not yet supported by hokuyo_aist.";
+ "This function is not yet supported by hokuyo_aist.";
}
- else if (errorCode >= 50 && errorCode <= 97)
- ss << "Hardware error: " << errorCode;
- else if (errorCode == 98)
+ else if (error_code >= 50 && error_code <= 97)
+ ss << "Hardware error: " << error_code;
+ else if (error_code == 98)
{
- return "Resumption of processing after confirming normal laser opteration. "
- "This function is not yet supported by hokuyo_aist.";
+ return "Resumption of processing after confirming normal "
+ "laser opteration. This function is not yet supported "
+ "by hokuyo_aist.";
}
else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+ ss << "Unknown error code " << error_code <<
+ " for command " << cmd[0] << cmd[1];
- return ss.str ();
+ return ss.str();
}
}
else if (cmd[0] == 'T' && cmd[1] == 'M')
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Invalid control code";
case 2:
- return "Adjust mode on command received when sensor's adjust mode is already on";
+ return "Adjust mode on command received when sensor's adjust "
+ "mode is already on";
case 3:
- return "Adjust mode off command received when sensor's adjust mode is already off";
+ return "Adjust mode off command received when sensor's adjust "
+ "mode is already off";
case 4:
return "Adjust mode is off when requested time";
}
}
else if (cmd[0] == 'S' && cmd[1] == 'S')
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Baud rate has non-numeric value";
@@ -332,7 +348,7 @@
}
else if (cmd[0] == 'C' && cmd[1] == 'R')
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Invalid speed";
@@ -346,7 +362,7 @@
}
else if (cmd[0] == 'H' && cmd[1] == 'S')
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "Parameter error";
@@ -359,20 +375,21 @@
// No info in the manual for this.
// else if (cmd[0] == 'R' && cmd[1] == 'S')
// {
-// switch (errorCode)
+// switch (error_code)
// {
// case :
// return "";
// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// std::stringstream ss;
+// ss << "Unknown error code " << error_code <<
+// " for command " << cmd[0] << cmd[1];
// return ss.str ();
// }
// }
// No info in the manual for this.
// else if (cmd[0] == 'V' && cmd[1] == 'V')
// {
-// switch (errorCode)
+// switch (error_code)
// {
// case :
// return "";
@@ -381,7 +398,7 @@
// No info in the manual for this.
// else if (cmd[0] == 'P' && cmd[1] == 'P')
// {
-// switch (errorCode)
+// switch (error_code)
// {
// case :
// return "";
@@ -390,7 +407,7 @@
// No info in the manual for this.
// else if (cmd[0] == 'I' && cmd[1] == 'I')
// {
-// switch (errorCode)
+// switch (error_code)
// {
// case :
// return "";
@@ -399,15 +416,16 @@
else
{
ss << "Unknown command: " << cmd[0] << cmd[1];
- return ss.str ();
+ return ss.str();
}
// Known commands with unknown error codes fall through to here
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
- return ss.str ();
+ ss << "Unknown error code " << error_code << " for command " << cmd[0] <<
+ cmd[1];
+ return ss.str();
}
-unsigned int Decode2ByteValue (char *data)
+unsigned int decode_2_byte_value(char *data)
{
unsigned int byte1, byte2;
@@ -417,7 +435,7 @@
return (byte1 << 6) + (byte2);
}
-unsigned int Decode3ByteValue (char *data)
+unsigned int decode_3_byte_value(char *data)
{
unsigned int byte1, byte2, byte3;
@@ -428,7 +446,7 @@
return (byte1 << 12) + (byte2 << 6) + (byte3);
}
-unsigned int Decode4ByteValue (char *data)
+unsigned int decode_4_byte_value(char *data)
{
unsigned int byte1, byte2, byte3, byte4;
@@ -440,25 +458,25 @@
return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
}
-void NumberToString (unsigned int num, char *dest, int length)
+void number_to_string(unsigned int num, char *dest, int length)
{
-#if defined (WIN32)
- _snprintf (dest, length + 1, "%*d", length, num);
+#if defined(WIN32)
+ _snprintf(dest, length + 1, "%*d", length, num);
#else
- snprintf (dest, length + 1, "%*d", length, num);
+ snprintf(dest, length + 1, "%*d", length, num);
#endif
// Replace all leading spaces with '0'
for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
dest[ii] = '0';
}
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// HokuyoError class
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// BaseError class
+///////////////////////////////////////////////////////////////////////////////
-string HokuyoError::AsString () const throw ()
+std::string BaseError::as_string() const throw()
{
- switch (_errorCode)
+ switch (_error_code)
{
case HOKUYO_ERR_READ:
return "HOKUYO_ERR_READ";
@@ -492,80 +510,86 @@
return "UNKNOWN_ERROR";
}
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// HokuyoSensorInfo class
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// SensorInfo class
+///////////////////////////////////////////////////////////////////////////////
-HokuyoSensorInfo::HokuyoSensorInfo ()
- : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
- standardSpeed (0), power (false), speed (0), speedLevel (0), baud (0), time (0), minAngle (0.0),
- maxAngle (0.0), resolution (0.0), scanableSteps (0)
+SensorInfo::SensorInfo()
+ : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
+ front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
+ baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
+ scanable_steps(0)
{
}
// Set various known values based on what the manual says
-void HokuyoSensorInfo::SetDefaults ()
+void SensorInfo::set_defaults()
{
- minRange = 20;
- maxRange = 4095;
+ min_range = 20;
+ max_range = 4095;
steps = 1024;
- firstStep = 44;
- lastStep = 725;
- frontStep = 384;
+ first_step = 44;
+ last_step = 725;
+ front_step = 384;
}
-void HokuyoSensorInfo::CalculateValues ()
+void SensorInfo::calculate_values()
{
- resolution = DTOR (360.0) / steps;
+ resolution = DTOR(360.0) / steps;
// If any of the steps are beyond INT_MAX, we have problems.
// We also have an incredibly high-resolution sensor.
- minAngle = (static_cast<int> (firstStep) - static_cast<int> (frontStep)) * resolution;
- maxAngle = (static_cast<int> (lastStep) - static_cast<int> (frontStep)) * resolution;
- scanableSteps = lastStep - firstStep + 1;
+ min_angle = (static_cast<int>(first_step) - static_cast<int>(front_step)) *
+ resolution;
+ max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) *
+ resolution;
+ scanable_steps = last_step - first_step + 1;
}
-string HokuyoSensorInfo::AsString ()
+std::string SensorInfo::as_string()
{
- stringstream ss;
+ std::stringstream ss;
- ss << "Vendor: " << vendor << endl;
- ss << "Product: " << product << endl;
- ss << "Firmware: " << firmware << endl;
- ss << "Protocol: " << protocol << endl;
- ss << "Serial: " << serial << endl;
- ss << "Model: " << model << endl;
+ ss << "Vendor: " << vendor << std::endl;
+ ss << "Product: " << product << std::endl;
+ ss << "Firmware: " << firmware << std::endl;
+ ss << "Protocol: " << protocol << std::endl;
+ ss << "Serial: " << serial << std::endl;
+ ss << "Model: " << model << std::endl;
- ss << "Minimum range: " << minRange << "mm\tMaximum range: " << maxRange << "mm" << endl;
- ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << scanableSteps << endl;
- ss << "First step: " << firstStep << "\tFront step: " << frontStep << "\tLast step: " <<
- lastStep << endl;
- ss << "Resolution: " << resolution << " radians/step" << endl;
- ss << "Minimum angle: " << minAngle << " radians\tMaximum angle: " << maxAngle <<
- " radians" << endl;
- ss << "Standard motor speed: " << standardSpeed << "rpm" << endl;
+ ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range <<
+ "mm" << std::endl;
+ ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
+ scanable_steps << std::endl;
+ ss << "First step: " << first_step << "\tFront step: " << front_step <<
+ "\tLast step: " << last_step << std::endl;
+ ss << "Resolution: " << resolution << " radians/step" << std::endl;
+ ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
+ max_angle << " radians" << std::endl;
+ ss << "Standard motor speed: " << standard_speed << "rpm" << std::endl;
- ss << "Power status: " << (power ? "On" : "Off") << "\tMeasurement state: " <<
- measureState << endl;
- ss << "Motor speed: " << speed << "rpm (level " << speedLevel << ")\tBaud rate: " << baud <<
- "bps" << endl;
- ss << "Time stamp: " << time << "ms" << endl;
- ss << "Sensor diagnostic: " << sensorDiagnostic << endl;
+ ss << "Power status: " << (power ? "On" : "Off") <<
+ "\tMeasurement state: " << measure_state << std::endl;
+ ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
+ ")\tBaud rate: " << baud << "bps" << std::endl;
+ ss << "Time stamp: " << time << "ms" << std::endl;
+ ss << "Sensor diagnostic: " << sensor_diagnostic << std::endl;
- return ss.str ();
+ return ss.str();
}
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// HokuyoData class
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// ScanData class
+///////////////////////////////////////////////////////////////////////////////
-HokuyoData::HokuyoData ()
- : _ranges (NULL), _intensities (NULL), _length (0),
- _error (false), _time (0), _sensorIsUTM30LX (false)
+ScanData::ScanData()
+ : _ranges(NULL), _intensities(NULL), _length(0),
+ _error(false), _time(0)
{
}
-HokuyoData::HokuyoData (uint32_t *ranges, unsigned int length, bool error, unsigned int time)
- : _error (error), _time (time)
+ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
+ unsigned int time)
+ : _error(error), _time(time)
{
_length = length;
if (_length == 0)
@@ -584,15 +608,15 @@
_length = 0;
throw;
}
- memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
+ memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
_intensities = NULL; // No intensity data to copy
}
}
-HokuyoData::HokuyoData (uint32_t *ranges, uint32_t *intensities, unsigned int length, bool error,
- unsigned int time)
- : _error (error), _time (time)
+ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
+ unsigned int length, bool error, unsigned int time)
+ : _error(error), _time(time)
{
_length = length;
if (_length == 0)
@@ -611,16 +635,16 @@
_length = 0;
throw;
}
- memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
+ memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
_intensities = new uint32_t[_length];
- memcpy (_intensities, intensities, sizeof (uint32_t) * _length);
+ memcpy(_intensities, intensities, sizeof(uint32_t) * _length);
}
}
-HokuyoData::HokuyoData (const HokuyoData &rhs)
+ScanData::ScanData(const ScanData &rhs)
{
- _length = rhs.Length ();
+ _length = rhs.length();
if (_length == 0)
_ranges = NULL;
else
@@ -634,29 +658,30 @@
_length = 0;
throw;
}
- memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
+ 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 (), sizeof (uint32_t) * _length);
+ memcpy(_intensities, rhs.intensities(),
+ sizeof(uint32_t) * _length);
}
}
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
+ _error = rhs.get_error_status();
+ _time = rhs.time_stamp();
}
-HokuyoData::~HokuyoData ()
+ScanData::~ScanData()
{
- if (_ranges != NULL)
+ if(_ranges != NULL)
delete[] _ranges;
}
-string HokuyoData::ErrorCodeToString (uint32_t errorCode)
+std::string ScanData::error_code_to_string(uint32_t error_code)
{
if (_sensorIsUTM30LX)
{
- switch (errorCode)
+ switch (error_code)
{
case 1:
return "No object in the range.";
@@ -669,14 +694,14 @@
case 5:
return "Other error.";
default:
- stringstream ss;
- ss << "Unknown error code: " << errorCode;
- return ss.str ();
+ std::stringstream ss;
+ ss << "Unknown error code: " << error_code;
+ return ss.str();
}
}
else
{
- switch (errorCode)
+ switch (error_code)
{
case 0:
return "Detected object is possibly at 22m.";
@@ -693,7 +718,8 @@
case 6:
return "Possibility of detected object is at 5.7m.";
case 7:
- return "Distance data on the preceding and succeeding steps have errors.";
+ return "Distance data on the preceding and succeeding steps "
+ "have errors.";
case 8:
return "Others.";
case 9:
@@ -711,7 +737,8 @@
case 15:
return "Others.";
case 16:
- return "Possibility of detected object is in the range 4096mm.";
+ return "Possibility of detected object is in the range "
+ "4096mm.";
case 17:
return "Others.";
case 18:
@@ -719,16 +746,16 @@
case 19:
return "Non-measurable distance.";
default:
- stringstream ss;
- ss << "Unknown error code: " << errorCode;
- return ss.str ();
+ std::stringstream ss;
+ ss << "Unknown error code: " << error_code;
+ return ss.str();
}
}
}
-HokuyoData& HokuyoData::operator= (const HokuyoData &rhs)
+ScanData& ScanData::operator=(const ScanData &rhs)
{
- if (rhs.Length () == 0)
+ if (rhs.length() == 0)
{
_length = 0;
if (_ranges != NULL)
@@ -737,39 +764,42 @@
if (_intensities != NULL)
delete[] _intensities;
_intensities = NULL;
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
+ _error = rhs.get_error_status();
+ _time = rhs.time_stamp();
}
else
{
- unsigned int rhsLength = rhs.Length ();
+ unsigned int rhslength = rhs.length();
uint32_t *newData;
- if (rhsLength != _length)
+ 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);
+ // 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)
delete[] _ranges;
_ranges = newData;
- _length = rhs.Length ();
+ _length = rhs.length();
- if (rhs.Intensities () != NULL)
+ if (rhs.intensities() != NULL)
{
try
{
- newData = new uint32_t[rhsLength];
+ newData = new uint32_t[rhslength];
}
catch (std::bad_alloc &e)
{
- // We have to remove any old intensity data or the length won't match
+ // We have to remove any old intensity data or the length
+ // won't match
if (_intensities != NULL)
delete[] _intensities;
_intensities = NULL;
throw;
}
- memcpy (newData, rhs.Intensities (), sizeof (uint32_t) * rhsLength);
+ memcpy(newData, rhs.intensities(), sizeof(uint32_t) *
+ rhslength);
if (_intensities != NULL)
delete[] _intensities;
_intensities = newData;
@@ -778,56 +808,58 @@
else
{
// If lengths are the same, no need to reallocate
- memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
- if (rhs.Intensities () != NULL)
- memcpy (_intensities, rhs.Intensities (), sizeof (uint32_t) * _length);
+ memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
+ if (rhs.intensities() != NULL)
+ memcpy(_intensities, rhs.intensities(), sizeof(uint32_t) *
+ _length);
}
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
+ _error = rhs.get_error_status();
+ _time = rhs.time_stamp();
}
return *this;
}
-uint32_t HokuyoData::operator[] (unsigned int index)
+uint32_t ScanData::operator[](unsigned int index)
{
if (index >= _length)
- throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid data index.");
+ throw BaseError(HOKUYO_ERR_BADARG, "Invalid data index.");
return _ranges[index];
}
-string HokuyoData::AsString ()
+std::string ScanData::as_string()
{
- stringstream ss;
+ std::stringstream ss;
- ss << _length << " readings:" << endl;
+ ss << _length << " readings:" << std::endl;
for (unsigned int ii = 0; ii < _length; ii++)
ss << _ranges[ii] << "\t";
if (_intensities != NULL)
{
- ss << endl << _length << " intensities:" << endl;
+ ss << std::endl << _length << " intensities:" << std::endl;
for (unsigned int ii = 0; ii < _length; ii++)
ss << _intensities[ii] << "\t";
}
- ss << endl;
+ ss << std::endl;
if (_error)
{
- ss << "Detected data errors:" << endl;
+ ss << "Detected data errors:" << std::endl;
for (unsigned int ii = 0; ii < _length; ii++)
{
if (_ranges[ii] < 20)
- ss << ii << ": " << ErrorCodeToString (_ranges[ii]) << endl;
+ ss << ii << ": " << error_code_to_string(_ranges[ii]) <<
+ std::endl;
}
}
else
- ss << "No data errors." << endl;
- ss << "Time stamp: " << _time << endl;
+ ss << "No data errors." << std::endl;
+ ss << "Time stamp: " << _time << std::endl;
- return ss.str ();
+ return ss.str();
}
-void HokuyoData::CleanUp ()
+void ScanData::clean_up()
{
if (_ranges != NULL)
delete[] _ranges;
@@ -840,7 +872,7 @@
_time = 0;
}
-void HokuyoData::AllocateData (unsigned int length, bool includeIntensities)
+void ScanData::allocate_data(unsigned int length, bool include_intensities)
{
// If no data yet, allocate new
if (_ranges == NULL)
@@ -873,7 +905,7 @@
}
// Else data is already allocated to the right length, so do nothing
- if (includeIntensities)
+ if (include_intensities)
{
// If no data yet, allocate new
if (_intensities == NULL)
@@ -890,118 +922,127 @@
}
else if (_intensities != NULL)
{
- // If not told to allocate space for intensity data and it exists, remove it
+ // If not told to allocate space for intensity data and it exists,
+ // remove it
delete[] _intensities;
_intensities = NULL;
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// HokuyoLaser class
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// Sensor class
+///////////////////////////////////////////////////////////////////////////////
// Public API
-////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
-HokuyoLaser::HokuyoLaser ()
- : _port (NULL), _scipVersion (2), _verbose (false), _sensorIsUTM30LX (false),
- _enableCheckSumWorkaround (false), _ignoreUnknowns (false), _minAngle (0.0), _maxAngle (0.0),
- _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
+Sensor::Sensor()
+ : _port(NULL), _scip_version(2), _verbose(false), _sensorIsUTM30LX(false),
+ _enable_checksum_workaround(false), _ignore_unknowns(false), _min_angle(0.0),
+ _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
+ _front_step(0)
{
}
-HokuyoLaser::~HokuyoLaser ()
+Sensor::~Sensor()
{
if (_port != NULL)
delete _port;
}
-void HokuyoLaser::Open (string portOptions)
+void Sensor::open(std::string port_options)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
- portOptions << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Creating and opening port using options: " << port_options <<
+ std::endl;
}
- _port = flexiport::CreatePort (portOptions);
- _port->Open ();
+ _port = flexiport::CreatePort(port_options);
+ _port->Open();
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
- " connection." << endl;
- cerr << _port->GetStatus ();
+ std::cerr << "Sensor::" << __func__ << "() Connected using " <<
+ _port->GetPortType() << " connection." << std::endl;
+ std::cerr << _port->GetStatus();
}
- _port->Flush ();
+ _port->Flush();
- // Figure out the SCIP version currently in use and switch to a higher one if possible
- GetAndSetSCIPVersion ();
+ // Figure out the SCIP version currently in use and switch to a higher one
+ // if possible
+ _get_and_set_scip_version();
// Get some values we need for providing default ranges
- GetDefaults ();
+ _get_defaults();
}
-unsigned int HokuyoLaser::OpenWithProbing (string portOptions)
+unsigned int Sensor::open_with_probing(std::string port_options)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
- portOptions << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Creating and opening port using options: " << port_options <<
+ std::endl;
}
- _port = flexiport::CreatePort (portOptions);
- _port->Open ();
+ _port = flexiport::CreatePort(port_options);
+ _port->Open();
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
- " connection." << endl;
- cerr << _port->GetStatus ();
+ std::cerr << "Sensor::" << __func__ << "() Connected using " <<
+ _port->GetPortType() << " connection." << std::endl;
+ std::cerr << _port->GetStatus();
}
- _port->Flush ();
+ _port->Flush();
try
{
- // Figure out the SCIP version currently in use and switch to a higher one if possible
- GetAndSetSCIPVersion ();
+ // Figure out the SCIP version currently in use and switch to a higher
+ // one if possible
+ _get_and_set_scip_version();
// Get some values we need for providing default ranges
- GetDefaults ();
+ _get_defaults();
}
- catch (HokuyoError)
+ catch (BaseError)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ <<
- "() Failed to connect at the default baud rate." << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Failed to connect at the default baud rate." << std::endl;
}
- 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
+ // 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;
for (unsigned int ii = 0; ii < numBauds; ii++)
{
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (bauds[ii]);
+ reinterpret_cast<SerialPort*>(_port)->SetBaudRate(bauds[ii]);
try
{
- GetAndSetSCIPVersion ();
- GetDefaults ();
- // If the above two functions succeed, break out of the loop and be happy
+ _get_and_set_scip_version();
+ _get_defaults();
+ // If the above two functions succeed, break out of the
+ // loop and be happy
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Connected at " <<
- bauds[ii] << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Connected at " << bauds[ii] << std::endl;
}
return bauds[ii];
}
- catch (HokuyoError)
+ catch (BaseError)
{
if (ii == numBauds - 1)
{
// Last baud rate, give up and rethrow
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ <<
- "() Failed to connect at any baud rate." << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Failed to connect at any baud rate." <<
+ std::endl;
}
throw;
}
@@ -1013,152 +1054,159 @@
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Port is not serial, cannot probe." <<
- endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Port is not serial, cannot probe." << std::endl;
}
throw;
}
}
- if (_port->GetPortType () == "serial")
- return reinterpret_cast<SerialPort*> (_port)->GetBaudRate ();
+ if (_port->GetPortType() == "serial")
+ return reinterpret_cast<SerialPort*>(_port)->GetBaudRate();
else
return 0;
}
-void HokuyoLaser::Close ()
+void Sensor::close()
{
if (!_port)
- throw HokuyoError (HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
+ throw BaseError(HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Closing connection." << endl;
+ std::cerr << "Sensor::" << __func__ << "() Closing connection." <<
+ std::endl;
delete _port;
_port = NULL;
}
-bool HokuyoLaser::IsOpen () const
+bool Sensor::is_open() const
{
if (_port != NULL)
- return _port->IsOpen ();
+ return _port->IsOpen();
return false;
}
-void HokuyoLaser::SetPower (bool on)
+void Sensor::set_power(bool on)
{
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
if (on)
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("L", "1", 1, NULL);
+ std::cerr << "Sensor::" << __func__ <<
+ "() Turning laser on." << std::endl;
+ _send_command("L", "1", 1, NULL);
}
else
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("L", "0", 1, NULL);
+ std::cerr << "Sensor::" << __func__ <<
+ "() Turning laser off." << std::endl;
+ _send_command("L", "0", 1, NULL);
}
- SkipLines (1);
+ _skip_lines(1);
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
if (on)
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("BM", NULL, 0, "02");
+ std::cerr << "Sensor::" << __func__ <<
+ "() Turning laser on." << std::endl;
+ _send_command("BM", NULL, 0, "02");
}
else
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("QT", NULL, 0, "02");
+ std::cerr << "Sensor::" << __func__ <<
+ "() Turning laser off." << std::endl;
+ _send_command("QT", NULL, 0, "02");
}
- SkipLines (1);
+ _skip_lines(1);
}
else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-// This function assumes that both the port and the laser scanner are already set to the same baud.
-void HokuyoLaser::SetBaud (unsigned int baud)
+// This function assumes that both the port and the laser scanner are already
+// set to the same baud.
+void Sensor::set_baud(unsigned int baud)
{
- if (_port->GetPortType () != "serial")
+ if (_port->GetPortType() != "serial")
{
- throw HokuyoError (HOKUYO_ERR_NOTSERIAL,
+ throw BaseError(HOKUYO_ERR_NOTSERIAL,
"Cannot change baud rate of non-serial connection.");
}
char newBaud[13];
- memset (newBaud, 0, sizeof (char) * 13);
+ memset(newBaud, 0, sizeof(char) * 13);
if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
baud != 250000 && baud != 500000 && baud != 750000)
{
- stringstream ss;
- ss << "Bad baud rate: " << baud << endl;
- throw HokuyoError (HOKUYO_ERR_BADARG, ss.str ());
+ std::stringstream ss;
+ ss << "Bad baud rate: " << baud << std::endl;
+ throw BaseError(HOKUYO_ERR_BADARG, ss.str());
}
- NumberToString (baud, newBaud, 6);
+ number_to_string(baud, newBaud, 6);
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
// Send the command to change baud rate
- SendCommand ("S", newBaud, 13, NULL);
- SkipLines (1);
+ _send_command("S", newBaud, 13, NULL);
+ _skip_lines(1);
// Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
// Send the command to change baud rate
- SendCommand ("SS", newBaud, 6, "03");
- SkipLines (1);
+ _send_command("SS", newBaud, 6, "03");
+ _skip_lines(1);
// Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
}
else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void HokuyoLaser::Reset ()
+void Sensor::reset()
{
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
+ throw BaseError(HOKUYO_ERR_UNSUPPORTED,
"SCIP version 1 does not support the reset command.");
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Resetting laser." << endl;
- SendCommand ("RS", NULL, 0, NULL);
- SkipLines (1);
+ std::cerr << "Sensor::" << __func__ <<
+ "() Resetting laser." << std::endl;
+ _send_command("RS", NULL, 0, NULL);
+ _skip_lines(1);
}
else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void HokuyoLaser::SetMotorSpeed (unsigned int speed)
+void Sensor::set_motor_speed(unsigned int speed)
{
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set motor speed command.");
+ throw BaseError(HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the set motor speed command.");
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
// Sanity check the value
if (speed > 10 && speed != 99)
- throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid motor speed.");
+ throw BaseError(HOKUYO_ERR_BADARG, "Invalid motor speed.");
char buffer[3];
if (speed == 0)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
- endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Reseting motor speed to default." << std::endl;
}
buffer[0] = '0';
buffer[1] = '0';
@@ -1168,8 +1216,8 @@
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
- endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Reseting motor speed to default." << std::endl;
}
buffer[0] = '9';
buffer[1] = '9';
@@ -1179,707 +1227,785 @@
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Setting motor speed to ratio " <<
- speed << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Setting motor speed to ratio " << speed << std::endl;
}
- NumberToString (speed, buffer, 2);
+ number_to_string(speed, buffer, 2);
}
- SendCommand ("CR", buffer, 2, "03");
- SkipLines (1);
+ _send_command("CR", buffer, 2, "03");
+ _skip_lines(1);
}
else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void HokuyoLaser::SetHighSensitivity (bool on)
+void Sensor::set_high_sensitivity(bool on)
{
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set high sensitivity command.");
+ throw BaseError(HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the high sensitivity command.");
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
if (on)
{
if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Switching to high sensitivity." << endl;
- SendCommand ("HS", "1", 1, "02");
+ std::cerr << "Sensor::" << __func__ <<
+ "() Switching to high sensitivity." << std::endl;
+ _send_command("HS", "1", 1, "02");
}
else
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ << "() Switching to normal sensitivity." <<
- endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Switching to normal sensitivity." << std::endl;
}
- SendCommand ("HS", "0", 1, "02");
+ _send_command("HS", "0", 1, "02");
}
- SkipLines (1);
+ _skip_lines(1);
}
else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void HokuyoLaser::GetSensorInfo (HokuyoSensorInfo *info)
+void Sensor::get_sensor_info(SensorInfo *info)
{
if (info == NULL)
- throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No info object provided.");
+ throw BaseError(HOKUYO_ERR_NODESTINATION,
+ "No info object provided.");
- if (_scipVersion == 1)
+ if (_scip_version == 1)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ <<
- "() Getting sensor information using SCIP version 1." << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Getting sensor information using SCIP version 1." <<
+ std::endl;
}
- info->SetDefaults ();
+ info->set_defaults();
char buffer[SCIP1_LINE_LENGTH];
- memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
+ memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
- SendCommand ("V", NULL, 0, NULL);
+ _send_command("V", NULL, 0, NULL);
// Get the vendor info line
- ReadLine (buffer);
+ _read_line(buffer);
info->vendor = &buffer[5]; // Chop off the "VEND:" tag
// Get the product info line
- ReadLine (buffer);
+ _read_line(buffer);
info->product = &buffer[5];
// Get the firmware line
- ReadLine (buffer);
+ _read_line(buffer);
info->firmware = &buffer[5];
// Get the protocol version line
- ReadLine (buffer);
+ _read_line(buffer);
info->protocol = &buffer[5];
// Get the serial number
- ReadLine (buffer);
+ _read_line(buffer);
info->serial = &buffer[5];
// Get either the status line or the end of message
- ReadLine (buffer);
+ _read_line(buffer);
if (buffer[0] != '\0')
{
// Got a status line
- info->sensorDiagnostic = &buffer[5];
- SkipLines (1);
+ info->sensor_diagnostic = &buffer[5];
+ _skip_lines(1);
}
- // Check the firmware version major number. If it's >=3 there is probably some extra info
- // in the firmware line.
- // e.g.: 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)
+ // Check the firmware version major number. If it's >=3 there is
+ // probably some extra info in the firmware line.
+ // 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 (_verbose)
- cerr << "SCIP1 Firmware line for parsing: " << info->firmware << endl;
- // 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.
+ std::cerr << "SCIP1 Firmware line for parsing: " <<
+ info->firmware << std::endl;
+ // 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)
+ if ((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
{
- // No bracket? Crud. Fail and use the hard-coded values from the manual.
- info->CalculateValues ();
+ // No bracket? Crud. Fail and use the hard-coded values from
+ // the manual.
+ info->calculate_values();
}
// Now put it through sscanf and hope...
int aperture;
- int numFound = sscanf (valueStart, "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]",
- &info->minRange, &info->maxRange, &aperture,
- &info->firstStep, &info->lastStep, &info->speed);
+ int numFound = sscanf(valueStart,
+ "(%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)
{
- // Didn't get enough values out, assume unknown format and fall back on the defaults
- info->SetDefaults ();
- info->CalculateValues ();
+ // Didn't get enough values out, assume unknown format and fall
+ // back on the defaults
+ info->set_defaults();
+ info->calculate_values();
if (_verbose)
{
- cerr << "Retrieved sensor info (hard-coded, not enough values):" << endl;
- cerr << info->AsString ();
+ std::cerr << "Retrieved sensor info (hard-coded, not "
+ "enough values):" << std::endl;
+ std::cerr << info->as_string();
}
}
else
{
- // Need to calculate stuff differently since it gave us an aperture value
- info->resolution = DTOR (static_cast<double> (aperture)) /
- static_cast<double> (info->lastStep - info->firstStep);
+ // Need to calculate stuff differently since it gave us an
+ // aperture value
+ info->resolution = DTOR(static_cast<double>(aperture)) /
+ static_cast<double>(info->last_step - info->first_step);
// Assume that the range is evenly spread
- info->scanableSteps = info->lastStep - info->firstStep + 1;
- info->frontStep = info->scanableSteps / 2 + info->firstStep - 1;
- info->minAngle = (static_cast<int> (info->firstStep) -
- static_cast<int> (info->frontStep)) * info->resolution;
- info->maxAngle = (info->lastStep - info->frontStep) * info->resolution;
+ info->scanable_steps = info->last_step - info->first_step + 1;
+ info->front_step = info->scanable_steps / 2 +
+ info->first_step - 1;
+ info->min_angle = (static_cast<int>(info->first_step) -
+ static_cast<int>(info->front_step)) * info->resolution;
+ info->max_angle = (info->last_step - info->front_step) *
+ info->resolution;
if (_verbose)
{
- cerr << "Retrieved sensor info (from FIRM line):" << endl;
- cerr << info->AsString ();
+ std::cerr << "Retrieved sensor info (from FIRM line):" <<
+ std::endl;
+ std::cerr << info->as_string();
}
}
}
else
{
- // We're stuck with hard-coded defaults from the manual (already set earlier).
- info->CalculateValues ();
+ // We're stuck with hard-coded defaults from the manual (already
+ // set earlier).
+ info->calculate_values();
if (_verbose)
{
- cerr << "Retrieved sensor info (hard-coded):" << endl;
- cerr << info->AsString ();
+ std::cerr << "Retrieved sensor info (hard-coded):" <<
+ std::endl;
+ std::cerr << info->as_string();
}
}
}
- else if (_scipVersion == 2)
+ else if (_scip_version == 2)
{
if (_verbose)
{
- cerr << "HokuyoLaser::" << __func__ <<
- "() Getting sensor information using SCIP version 2." << endl;
+ std::cerr << "Sensor::" << __func__ <<
+ "() Getting sensor information using SCIP version 2." <<
+ std::endl;
}
- info->SetDefaults ();
+ info->set_defaults();
char buffer[SCIP2_LINE_LENGTH];
- memset (buffer, 0, sizeof (char) * SCIP2_LINE_LENGTH);
+ memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
- // We need to send three commands to get all the info we want: VV, PP and II
- SendCommand ("VV", NULL, 0, NULL);
- while (ReadLineWithCheck (buffer, -1, true) != 0)
- ProcessVVLine (buffer, info);
+ // 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)
+ _process_vv_line(buffer, info);
// Next up, PP
- SendCommand ("PP", NULL, 0, NULL);
- while (ReadLineWithCheck (buffer, -1, true) != 0)
- ProcessPPLine (buffer, info);
+ _send_command("PP", NULL, 0, NULL);
+ while (_read_line_with_check(buffer, -1, true) != 0)
+ _process_pp_line(buffer, info);
// Command II: Revenge of the Commands.
- SendCommand ("II", NULL, 0, NULL);
- while (ReadLineWithCheck (buffer, -1, true) != 0)
- ProcessIILine (buffer, info);
+ _send_command("II", NULL, 0, NULL);
+ ...
[truncated message content] |
|
From: <gb...@us...> - 2010-07-30 03:33:17
|
Revision: 498
http://gearbox.svn.sourceforge.net/gearbox/?rev=498&view=rev
Author: gbiggs
Date: 2010-07-30 03:33:10 +0000 (Fri, 30 Jul 2010)
Log Message:
-----------
Adding new commands (work in progress)
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-05-19 10:15:29 UTC (rev 497)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2010-07-30 03:33:10 UTC (rev 498)
@@ -5,11 +5,11 @@
*
* hokuyo_aist Hokuyo 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.
*
@@ -36,7 +36,7 @@
using namespace flexiport;
#if defined (WIN32)
- #define __func__ __FUNCTION__
+ #define __func__ __FUNCTION__
#endif
namespace hokuyo_aist
@@ -49,14 +49,14 @@
#ifndef RTOD
inline double RTOD (double rad)
{
- return rad * 180.0 / M_PI;
+ return rad * 180.0 / M_PI;
}
#endif
// Convert degrees to radians
#ifndef DTOR
inline double DTOR (double deg)
{
- return deg * M_PI / 180.0;
+ return deg * M_PI / 180.0;
}
#endif
@@ -164,6 +164,15 @@
R|S|LF
R|S|LF|Status(2)|Sum|LF|LF
3 byte command block
+GE
+ND
+NE
+HD
+HE
+HS
+$IP
+RT
+DB
*/
////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -172,275 +181,275 @@
string SCIP1ErrorToString (char error, char cmd)
{
- return string ("No error descriptions available");
+ return string ("No error descriptions available");
}
// error must be null-terminated
string SCIP2ErrorToString (const char *error, const char *cmd)
{
- stringstream ss;
+ stringstream ss;
- // Check for non-errors
- if (error[0] == '0' && error[1] == '0')
- return "Status OK - 00";
- else if (error[0] == '9' && error[1] == '9')
- return "Status OK - 99";
+ // Check for non-errors
+ if (error[0] == '0' && error[1] == '0')
+ return "Status OK - 00";
+ else if (error[0] == '9' && error[1] == '9')
+ return "Status OK - 99";
- // Check for universal errors
- 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')
- return "Buffer shortage or command repeated that is already processed";
- else if (error[0] == '0' && error[1] == 'C')
- return "Command with insufficient parameters 1";
- else if (error[0] == '0' && error[1] == 'D')
- return "Undefined command 1";
- else if (error[0] == '0' && error[1] == 'E')
- return "Undefined command 2";
- else if (error[0] == '0' && error[1] == 'F')
- return "Command with insufficient parameters 2";
- else if (error[0] == '0' && error[1] == 'G')
- return "String character in command exceeds 16 letters";
- else if (error[0] == '0' && error[1] == 'H')
- return "String character has invalid letters";
- else if (error[0] == '0' && error[1] == 'I')
- return "Sensor is now in firmware update mode";
+ // Check for universal errors
+ 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')
+ return "Buffer shortage or command repeated that is already processed";
+ else if (error[0] == '0' && error[1] == 'C')
+ return "Command with insufficient parameters 1";
+ else if (error[0] == '0' && error[1] == 'D')
+ return "Undefined command 1";
+ else if (error[0] == '0' && error[1] == 'E')
+ return "Undefined command 2";
+ else if (error[0] == '0' && error[1] == 'F')
+ return "Command with insufficient parameters 2";
+ else if (error[0] == '0' && error[1] == 'G')
+ return "String character in command exceeds 16 letters";
+ else if (error[0] == '0' && error[1] == 'H')
+ return "String character has invalid letters";
+ else if (error[0] == '0' && error[1] == 'I')
+ return "Sensor is now in firmware update mode";
- int errorCode = atoi (error);
+ int errorCode = atoi (error);
- if (cmd[0] == 'B' && cmd[1] == 'M')
- {
- switch (errorCode)
- {
- case 1:
- return "Unable to control due to laser malfunction";
- case 2:
- return "Laser is already on";
- }
- }
+ if (cmd[0] == 'B' && cmd[1] == 'M')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Unable to control due to laser malfunction";
+ case 2:
+ return "Laser is already on";
+ }
+ }
// No info in the manual for this.
-// else if (cmd[0] == 'Q' && cmd[1] == 'T')
-// {
-// switch (errorCode)
-// {
-// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
- else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
- (cmd[0] == 'G' && cmd[1] == 'S'))
- {
- switch (errorCode)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 10:
- return "Laser is off";
- default:
- if (errorCode >= 50)
- ss << "Hardware error: " << errorCode;
- else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// else if (cmd[0] == 'Q' && cmd[1] == 'T')
+// {
+// switch (errorCode)
+// {
+// default:
+// stringstream ss;
+// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
+ else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+ (cmd[0] == 'G' && cmd[1] == 'S'))
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 10:
+ return "Laser is off";
+ default:
+ if (errorCode >= 50)
+ ss << "Hardware error: " << errorCode;
+ else
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
- return ss.str ();
- }
- }
- else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
- (cmd[0] == 'M' && cmd[1] == 'S'))
- {
- switch (errorCode)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 6:
- return "Scan interval has non-numeric value";
- case 7:
- return "Number of scans is non-numeric";
- default:
- if (errorCode >= 21 && errorCode <= 49)
- {
- return "Processing stopped to verify error. "
- "This function is not yet supported by hokuyo_aist.";
- }
- else if (errorCode >= 50 && errorCode <= 97)
- ss << "Hardware error: " << errorCode;
- else if (errorCode == 98)
- {
- return "Resumption of processing after confirming normal laser opteration. "
- "This function is not yet supported by hokuyo_aist.";
- }
- else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+ return ss.str ();
+ }
+ }
+ else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+ (cmd[0] == 'M' && cmd[1] == 'S'))
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 6:
+ return "Scan interval has non-numeric value";
+ case 7:
+ return "Number of scans is non-numeric";
+ default:
+ if (errorCode >= 21 && errorCode <= 49)
+ {
+ return "Processing stopped to verify error. "
+ "This function is not yet supported by hokuyo_aist.";
+ }
+ else if (errorCode >= 50 && errorCode <= 97)
+ ss << "Hardware error: " << errorCode;
+ else if (errorCode == 98)
+ {
+ return "Resumption of processing after confirming normal laser opteration. "
+ "This function is not yet supported by hokuyo_aist.";
+ }
+ else
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
- return ss.str ();
- }
- }
- else if (cmd[0] == 'T' && cmd[1] == 'M')
- {
- switch (errorCode)
- {
- case 1:
- return "Invalid control code";
- case 2:
- return "Adjust mode on command received when sensor's adjust mode is already on";
- case 3:
- return "Adjust mode off command received when sensor's adjust mode is already off";
- case 4:
- return "Adjust mode is off when requested time";
- }
- }
- else if (cmd[0] == 'S' && cmd[1] == 'S')
- {
- switch (errorCode)
- {
- case 1:
- return "Baud rate has non-numeric value";
- case 2:
- return "Invalid baud rate";
- case 3:
- return "Sensor is already running at that baud rate";
- case 4:
- return "Not compatible with the sensor model";
- }
- }
- else if (cmd[0] == 'C' && cmd[1] == 'R')
- {
- switch (errorCode)
- {
- case 1:
- return "Invalid speed";
- case 2:
- return "Speed is out of range";
- case 3:
- return "Motor is already running at that speed";
- case 4:
- return "Not compatible with the sensor model";
- }
- }
- else if (cmd[0] == 'H' && cmd[1] == 'S')
- {
- switch (errorCode)
- {
- case 1:
- return "Parameter error";
- case 2:
- return "Already running in the set mode";
- case 3:
- return "Not compatible with the sensor model";
- }
- }
+ return ss.str ();
+ }
+ }
+ else if (cmd[0] == 'T' && cmd[1] == 'M')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Invalid control code";
+ case 2:
+ return "Adjust mode on command received when sensor's adjust mode is already on";
+ case 3:
+ return "Adjust mode off command received when sensor's adjust mode is already off";
+ case 4:
+ return "Adjust mode is off when requested time";
+ }
+ }
+ else if (cmd[0] == 'S' && cmd[1] == 'S')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Baud rate has non-numeric value";
+ case 2:
+ return "Invalid baud rate";
+ case 3:
+ return "Sensor is already running at that baud rate";
+ case 4:
+ return "Not compatible with the sensor model";
+ }
+ }
+ else if (cmd[0] == 'C' && cmd[1] == 'R')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Invalid speed";
+ case 2:
+ return "Speed is out of range";
+ case 3:
+ return "Motor is already running at that speed";
+ case 4:
+ return "Not compatible with the sensor model";
+ }
+ }
+ else if (cmd[0] == 'H' && cmd[1] == 'S')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Parameter error";
+ case 2:
+ return "Already running in the set mode";
+ case 3:
+ return "Not compatible with the sensor model";
+ }
+ }
// No info in the manual for this.
-// else if (cmd[0] == 'R' && cmd[1] == 'S')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
+// else if (cmd[0] == 'R' && cmd[1] == 'S')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// default:
+// stringstream ss;
+// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
// No info in the manual for this.
-// else if (cmd[0] == 'V' && cmd[1] == 'V')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
+// else if (cmd[0] == 'V' && cmd[1] == 'V')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
// No info in the manual for this.
-// else if (cmd[0] == 'P' && cmd[1] == 'P')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
+// else if (cmd[0] == 'P' && cmd[1] == 'P')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
// No info in the manual for this.
-// else if (cmd[0] == 'I' && cmd[1] == 'I')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
- else
- {
- ss << "Unknown command: " << cmd[0] << cmd[1];
- return ss.str ();
- }
+// else if (cmd[0] == 'I' && cmd[1] == 'I')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
+ else
+ {
+ ss << "Unknown command: " << cmd[0] << cmd[1];
+ return ss.str ();
+ }
- // Known commands with unknown error codes fall through to here
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
- return ss.str ();
+ // Known commands with unknown error codes fall through to here
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+ return ss.str ();
}
unsigned int Decode2ByteValue (char *data)
{
- unsigned int byte1, byte2;
+ unsigned int byte1, byte2;
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
- return (byte1 << 6) + (byte2);
+ return (byte1 << 6) + (byte2);
}
unsigned int Decode3ByteValue (char *data)
{
- unsigned int byte1, byte2, byte3;
+ unsigned int byte1, byte2, byte3;
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
- return (byte1 << 12) + (byte2 << 6) + (byte3);
+ return (byte1 << 12) + (byte2 << 6) + (byte3);
}
unsigned int Decode4ByteValue (char *data)
{
- unsigned int byte1, byte2, byte3, byte4;
+ unsigned int byte1, byte2, byte3, byte4;
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
- byte4 = data[3] - 0x30;
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
+ byte4 = data[3] - 0x30;
- return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
+ return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
}
void NumberToString (unsigned int num, char *dest, int length)
{
#if defined (WIN32)
- _snprintf (dest, length + 1, "%*d", length, num);
+ _snprintf (dest, length + 1, "%*d", length, num);
#else
- snprintf (dest, length + 1, "%*d", length, num);
+ snprintf (dest, length + 1, "%*d", length, num);
#endif
- // Replace all leading spaces with '0'
- for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
- dest[ii] = '0';
+ // Replace all leading spaces with '0'
+ for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
+ dest[ii] = '0';
}
////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -449,38 +458,38 @@
string HokuyoError::AsString () const throw ()
{
- switch (_errorCode)
- {
- case HOKUYO_ERR_READ:
- return "HOKUYO_ERR_READ";
- case HOKUYO_ERR_WRITE:
- return "HOKUYO_ERR_WRITE";
- case HOKUYO_ERR_PROTOCOL:
- return "HOKUYO_ERR_PROTOCOL";
- case HOKUYO_ERR_CHANGEBAUD:
- return "HOKUYO_ERR_CHANGEBAUD";
- case HOKUYO_ERR_CONNECT_FAILED:
- return "HOKUYO_ERR_CONNECT_FAILED";
- case HOKUYO_ERR_CLOSE_FAILED:
- return "HOKUYO_ERR_CLOSE_FAILED";
- case HOKUYO_ERR_NODESTINATION:
- return "HOKUYO_ERR_NODESTINATION";
- case HOKUYO_ERR_BADFIRMWARE:
- return "HOKUYO_ERR_BADFIRMWARE";
- case HOKUYO_ERR_SCIPVERSION:
- return "HOKUYO_ERR_SCIPVERSION";
- case HOKUYO_ERR_MEMORY:
- return "HOKUYO_ERR_MEMORY";
- case HOKUYO_ERR_UNSUPPORTED:
- return "HOKUYO_ERR_UNSUPPORTED";
- case HOKUYO_ERR_BADARG:
- return "HOKUYO_ERR_BADARG";
- case HOKUYO_ERR_NODATA:
- return "HOKUYO_ERR_NODATA";
- case HOKUYO_ERR_NOTSERIAL:
- return "HOKUYO_ERR_NOTSERIAL";
- }
- return "UNKNOWN_ERROR";
+ switch (_errorCode)
+ {
+ case HOKUYO_ERR_READ:
+ return "HOKUYO_ERR_READ";
+ case HOKUYO_ERR_WRITE:
+ return "HOKUYO_ERR_WRITE";
+ case HOKUYO_ERR_PROTOCOL:
+ return "HOKUYO_ERR_PROTOCOL";
+ case HOKUYO_ERR_CHANGEBAUD:
+ return "HOKUYO_ERR_CHANGEBAUD";
+ case HOKUYO_ERR_CONNECT_FAILED:
+ return "HOKUYO_ERR_CONNECT_FAILED";
+ case HOKUYO_ERR_CLOSE_FAILED:
+ return "HOKUYO_ERR_CLOSE_FAILED";
+ case HOKUYO_ERR_NODESTINATION:
+ return "HOKUYO_ERR_NODESTINATION";
+ case HOKUYO_ERR_BADFIRMWARE:
+ return "HOKUYO_ERR_BADFIRMWARE";
+ case HOKUYO_ERR_SCIPVERSION:
+ return "HOKUYO_ERR_SCIPVERSION";
+ case HOKUYO_ERR_MEMORY:
+ return "HOKUYO_ERR_MEMORY";
+ case HOKUYO_ERR_UNSUPPORTED:
+ return "HOKUYO_ERR_UNSUPPORTED";
+ case HOKUYO_ERR_BADARG:
+ return "HOKUYO_ERR_BADARG";
+ case HOKUYO_ERR_NODATA:
+ return "HOKUYO_ERR_NODATA";
+ case HOKUYO_ERR_NOTSERIAL:
+ return "HOKUYO_ERR_NOTSERIAL";
+ }
+ return "UNKNOWN_ERROR";
}
////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -488,61 +497,61 @@
////////////////////////////////////////////////////////////////////////////////////////////////////
HokuyoSensorInfo::HokuyoSensorInfo ()
- : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
- standardSpeed (0), power (false), speed (0), speedLevel (0), baud (0), time (0), minAngle (0.0),
- maxAngle (0.0), resolution (0.0), scanableSteps (0)
+ : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
+ standardSpeed (0), power (false), speed (0), speedLevel (0), baud (0), time (0), minAngle (0.0),
+ maxAngle (0.0), resolution (0.0), scanableSteps (0)
{
}
// Set various known values based on what the manual says
void HokuyoSensorInfo::SetDefaults ()
{
- minRange = 20;
- maxRange = 4095;
- steps = 1024;
- firstStep = 44;
- lastStep = 725;
- frontStep = 384;
+ minRange = 20;
+ maxRange = 4095;
+ steps = 1024;
+ firstStep = 44;
+ lastStep = 725;
+ frontStep = 384;
}
void HokuyoSensorInfo::CalculateValues ()
{
- resolution = DTOR (360.0) / steps;
- // If any of the steps are beyond INT_MAX, we have problems.
- // We also have an incredibly high-resolution sensor.
- minAngle = (static_cast<int> (firstStep) - static_cast<int> (frontStep)) * resolution;
- maxAngle = (static_cast<int> (lastStep) - static_cast<int> (frontStep)) * resolution;
- scanableSteps = lastStep - firstStep + 1;
+ resolution = DTOR (360.0) / steps;
+ // If any of the steps are beyond INT_MAX, we have problems.
+ // We also have an incredibly high-resolution sensor.
+ minAngle = (static_cast<int> (firstStep) - static_cast<int> (frontStep)) * resolution;
+ maxAngle = (static_cast<int> (lastStep) - static_cast<int> (frontStep)) * resolution;
+ scanableSteps = lastStep - firstStep + 1;
}
string HokuyoSensorInfo::AsString ()
{
- stringstream ss;
+ stringstream ss;
- ss << "Vendor: " << vendor << endl;
- ss << "Product: " << product << endl;
- ss << "Firmware: " << firmware << endl;
- ss << "Protocol: " << protocol << endl;
- ss << "Serial: " << serial << endl;
- ss << "Model: " << model << endl;
+ ss << "Vendor: " << vendor << endl;
+ ss << "Product: " << product << endl;
+ ss << "Firmware: " << firmware << endl;
+ ss << "Protocol: " << protocol << endl;
+ ss << "Serial: " << serial << endl;
+ ss << "Model: " << model << endl;
- ss << "Minimum range: " << minRange << "mm\tMaximum range: " << maxRange << "mm" << endl;
- ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << scanableSteps << endl;
- ss << "First step: " << firstStep << "\tFront step: " << frontStep << "\tLast step: " <<
- lastStep << endl;
- ss << "Resolution: " << resolution << " radians/step" << endl;
- ss << "Minimum angle: " << minAngle << " radians\tMaximum angle: " << maxAngle <<
- " radians" << endl;
- ss << "Standard motor speed: " << standardSpeed << "rpm" << endl;
+ ss << "Minimum range: " << minRange << "mm\tMaximum range: " << maxRange << "mm" << endl;
+ ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << scanableSteps << endl;
+ ss << "First step: " << firstStep << "\tFront step: " << frontStep << "\tLast step: " <<
+ lastStep << endl;
+ ss << "Resolution: " << resolution << " radians/step" << endl;
+ ss << "Minimum angle: " << minAngle << " radians\tMaximum angle: " << maxAngle <<
+ " radians" << endl;
+ ss << "Standard motor speed: " << standardSpeed << "rpm" << endl;
- ss << "Power status: " << (power ? "On" : "Off") << "\tMeasurement state: " <<
- measureState << endl;
- ss << "Motor speed: " << speed << "rpm (level " << speedLevel << ")\tBaud rate: " << baud <<
- "bps" << endl;
- ss << "Time stamp: " << time << "ms" << endl;
- ss << "Sensor diagnostic: " << sensorDiagnostic << endl;
+ ss << "Power status: " << (power ? "On" : "Off") << "\tMeasurement state: " <<
+ measureState << endl;
+ ss << "Motor speed: " << speed << "rpm (level " << speedLevel << ")\tBaud rate: " << baud <<
+ "bps" << endl;
+ ss << "Time stamp: " << time << "ms" << endl;
+ ss << "Sensor diagnostic: " << sensorDiagnostic << endl;
- return ss.str ();
+ return ss.str ();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -550,341 +559,341 @@
////////////////////////////////////////////////////////////////////////////////////////////////////
HokuyoData::HokuyoData ()
- : _ranges (NULL), _intensities (NULL), _length (0),
- _error (false), _time (0), _sensorIsUTM30LX (false)
+ : _ranges (NULL), _intensities (NULL), _length (0),
+ _error (false), _time (0), _sensorIsUTM30LX (false)
{
}
HokuyoData::HokuyoData (uint32_t *ranges, unsigned int length, bool error, unsigned int time)
- : _error (error), _time (time)
+ : _error (error), _time (time)
{
- _length = length;
- if (_length == 0)
- {
- _ranges = NULL;
- _intensities = NULL;
- }
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
+ _length = length;
+ if (_length == 0)
+ {
+ _ranges = NULL;
+ _intensities = NULL;
+ }
+ else
+ {
+ try
+ {
+ _ranges = new uint32_t[_length];
+ }
+ catch (std::bad_alloc &e)
+ {
+ _length = 0;
+ throw;
+ }
+ memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
- _intensities = NULL; // No intensity data to copy
- }
+ _intensities = NULL; // No intensity data to copy
+ }
}
HokuyoData::HokuyoData (uint32_t *ranges, uint32_t *intensities, unsigned int length, bool error,
- unsigned int time)
- : _error (error), _time (time)
+ unsigned int time)
+ : _error (error), _time (time)
{
- _length = length;
- if (_length == 0)
- {
- _ranges = NULL;
- _intensities = NULL;
- }
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
+ _length = length;
+ if (_length == 0)
+ {
+ _ranges = NULL;
+ _intensities = NULL;
+ }
+ else
+ {
+ try
+ {
+ _ranges = new uint32_t[_length];
+ }
+ catch (std::bad_alloc &e)
+ {
+ _length = 0;
+ throw;
+ }
+ memcpy (_ranges, ranges, sizeof (uint32_t) * _length);
- _intensities = new uint32_t[_length];
- memcpy (_intensities, intensities, sizeof (uint32_t) * _length);
- }
+ _intensities = new uint32_t[_length];
+ memcpy (_intensities, intensities, sizeof (uint32_t) * _length);
+ }
}
HokuyoData::HokuyoData (const HokuyoData &rhs)
{
- _length = rhs.Length ();
- if (_length == 0)
- _ranges = NULL;
- else
- {
- try
- {
- _ranges = new uint32_t[_length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
+ _length = rhs.Length ();
+ if (_length == 0)
+ _ranges = NULL;
+ else
+ {
+ try
+ {
+ _ranges = new uint32_t[_length];
+ }
+ catch (std::bad_alloc &e)
+ {
+ _length = 0;
+ throw;
+ }
+ memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
- if (rhs.Intensities () != NULL)
- {
- _intensities = new uint32_t[_length];
- memcpy (_intensities, rhs.Intensities (), sizeof (uint32_t) * _length);
- }
- }
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
+ if (rhs.Intensities () != NULL)
+ {
+ _intensities = new uint32_t[_length];
+ memcpy (_intensities, rhs.Intensities (), sizeof (uint32_t) * _length);
+ }
+ }
+ _error = rhs.GetErrorStatus ();
+ _time = rhs.TimeStamp ();
}
HokuyoData::~HokuyoData ()
{
- if (_ranges != NULL)
- delete[] _ranges;
+ if (_ranges != NULL)
+ delete[] _ranges;
}
string HokuyoData::ErrorCodeToString (uint32_t errorCode)
{
- if (_sensorIsUTM30LX)
- {
- switch (errorCode)
- {
- case 1:
- return "No object in the range.";
- case 2:
- return "Object is too near (internal error).";
- case 3:
- return "Measurement error (may be due to interference).";
- case 4:
- return "Object out of range (at the near end).";
- case 5:
- return "Other error.";
- default:
- stringstream ss;
- ss << "Unknown error code: " << errorCode;
- return ss.str ();
- }
- }
- else
- {
- switch (errorCode)
- {
- case 0:
- return "Detected object is possibly at 22m.";
- case 1:
- return "Reflected light has low intensity.";
- case 2:
- return "Reflected light has low intensity.";
- case 3:
- return "Reflected light has low intensity.";
- case 4:
- return "Reflected light has low intensity.";
- case 5:
- return "Reflected light has low intensity.";
- case 6:
- return "Possibility of detected object is at 5.7m.";
- case 7:
- return "Distance data on the preceding and succeeding steps have errors.";
- case 8:
- return "Others.";
- case 9:
- return "The same step had error in the last two scan.";
- case 10:
- return "Others.";
- case 11:
- return "Others.";
- case 12:
- return "Others.";
- case 13:
- return "Others.";
- case 14:
- return "Others.";
- case 15:
- return "Others.";
- case 16:
- return "Possibility of detected object is in the range 4096mm.";
- case 17:
- return "Others.";
- case 18:
- return "Unspecified.";
- case 19:
- return "Non-measurable distance.";
- default:
- stringstream ss;
- ss << "Unknown error code: " << errorCode;
- return ss.str ();
- }
- }
+ if (_sensorIsUTM30LX)
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "No object in the range.";
+ case 2:
+ return "Object is too near (internal error).";
+ case 3:
+ return "Measurement error (may be due to interference).";
+ case 4:
+ return "Object out of range (at the near end).";
+ case 5:
+ return "Other error.";
+ default:
+ stringstream ss;
+ ss << "Unknown error code: " << errorCode;
+ return ss.str ();
+ }
+ }
+ else
+ {
+ switch (errorCode)
+ {
+ case 0:
+ return "Detected object is possibly at 22m.";
+ case 1:
+ return "Reflected light has low intensity.";
+ case 2:
+ return "Reflected light has low intensity.";
+ case 3:
+ return "Reflected light has low intensity.";
+ case 4:
+ return "Reflected light has low intensity.";
+ case 5:
+ return "Reflected light has low intensity.";
+ case 6:
+ return "Possibility of detected object is at 5.7m.";
+ case 7:
+ return "Distance data on the preceding and succeeding steps have errors.";
+ case 8:
+ return "Others.";
+ case 9:
+ return "The same step had error in the last two scan.";
+ case 10:
+ return "Others.";
+ case 11:
+ return "Others.";
+ case 12:
+ return "Others.";
+ case 13:
+ return "Others.";
+ case 14:
+ return "Others.";
+ case 15:
+ return "Others.";
+ case 16:
+ return "Possibility of detected object is in the range 4096mm.";
+ case 17:
+ return "Others.";
+ case 18:
+ return "Unspecified.";
+ case 19:
+ return "Non-measurable distance.";
+ default:
+ stringstream ss;
+ ss << "Unknown error code: " << errorCode;
+ return ss.str ();
+ }
+ }
}
HokuyoData& HokuyoData::operator= (const HokuyoData &rhs)
{
- if (rhs.Length () == 0)
- {
- _length = 0;
- if (_ranges != NULL)
- delete[] _ranges;
- _ranges = NULL;
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
- }
- else
- {
- unsigned int rhsLength = rhs.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)
- delete[] _ranges;
- _ranges = newData;
- _length = rhs.Length ();
+ if (rhs.Length () == 0)
+ {
+ _length = 0;
+ if (_ranges != NULL)
+ delete[] _ranges;
+ _ranges = NULL;
+ if (_intensities != NULL)
+ delete[] _intensities;
+ _intensities = NULL;
+ _error = rhs.GetErrorStatus ();
+ _time = rhs.TimeStamp ();
+ }
+ else
+ {
+ unsigned int rhsLength = rhs.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)
+ delete[] _ranges;
+ _ranges = newData;
+ _length = rhs.Length ();
- if (rhs.Intensities () != NULL)
- {
- try
- {
- newData = new uint32_t[rhsLength];
- }
- catch (std::bad_alloc &e)
- {
- // We have to remove any old intensity data or the length won't match
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- throw;
- }
- memcpy (newData, rhs.Intensities (), sizeof (uint32_t) * rhsLength);
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = newData;
- }
- }
- else
- {
- // If lengths are the same, no need to reallocate
- memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
- if (rhs.Intensities () != NULL)
- memcpy (_intensities, rhs.Intensities (), sizeof (uint32_t) * _length);
- }
+ if (rhs.Intensities () != NULL)
+ {
+ try
+ {
+ newData = new uint32_t[rhsLength];
+ }
+ catch (std::bad_alloc &e)
+ {
+ // We have to remove any old intensity data or the length won't match
+ if (_intensities != NULL)
+ delete[] _intensities;
+ _intensities = NULL;
+ throw;
+ }
+ memcpy (newData, rhs.Intensities (), sizeof (uint32_t) * rhsLength);
+ if (_intensities != NULL)
+ delete[] _intensities;
+ _intensities = newData;
+ }
+ }
+ else
+ {
+ // If lengths are the same, no need to reallocate
+ memcpy (_ranges, rhs.Ranges (), sizeof (uint32_t) * _length);
+ if (rhs.Intensities () != NULL)
+ memcpy (_intensities, rhs.Intensities (), sizeof (uint32_t) * _length);
+ }
- _error = rhs.GetErrorStatus ();
- _time = rhs.TimeStamp ();
- }
+ _error = rhs.GetErrorStatus ();
+ _time = rhs.TimeStamp ();
+ }
- return *this;
+ return *this;
}
uint32_t HokuyoData::operator[] (unsigned int index)
{
- if (index >= _length)
- throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid data index.");
- return _ranges[index];
+ if (index >= _length)
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid data index.");
+ return _ranges[index];
}
string HokuyoData::AsString ()
{
- stringstream ss;
+ stringstream ss;
- ss << _length << " readings:" << endl;
- for (unsigned int ii = 0; ii < _length; ii++)
- ss << _ranges[ii] << "\t";
- if (_intensities != NULL)
- {
- ss << endl << _length << " intensities:" << endl;
- for (unsigned int ii = 0; ii < _length; ii++)
- ss << _intensities[ii] << "\t";
- }
- ss << endl;
- if (_error)
- {
- ss << "Detected data errors:" << endl;
- for (unsigned int ii = 0; ii < _length; ii++)
- {
- if (_ranges[ii] < 20)
- ss << ii << ": " << ErrorCodeToString (_ranges[ii]) << endl;
- }
- }
- else
- ss << "No data errors." << endl;
- ss << "Time stamp: " << _time << endl;
+ ss << _length << " readings:" << endl;
+ for (unsigned int ii = 0; ii < _length; ii++)
+ ss << _ranges[ii] << "\t";
+ if (_intensities != NULL)
+ {
+ ss << endl << _length << " intensities:" << endl;
+ for (unsigned int ii = 0; ii < _length; ii++)
+ ss << _intensities[ii] << "\t";
+ }
+ ss << endl;
+ if (_error)
+ {
+ ss << "Detected data errors:" << endl;
+ for (unsigned int ii = 0; ii < _length; ii++)
+ {
+ if (_ranges[ii] < 20)
+ ss << ii << ": " << ErrorCodeToString (_ranges[ii]) << endl;
+ }
+ }
+ else
+ ss << "No data errors." << endl;
+ ss << "Time stamp: " << _time << endl;
- return ss.str ();
+ return ss.str ();
}
void HokuyoData::CleanUp ()
{
- if (_ranges != NULL)
- delete[] _ranges;
- _ranges = NULL;
- if (_intensities != NULL)
- delete[] _intensities;
- _intensities = NULL;
- _length = 0;
- _error = false;
- _time = 0;
+ if (_ranges != NULL)
+ delete[] _ranges;
+ _ranges = NULL;
+ if (_intensities != NULL)
+ delete[] _intensities;
+ _intensities = NULL;
+ _length = 0;
+ _error = false;
+ _time = 0;
}
void HokuyoData::AllocateData (unsigned int length, bool includeIntensities)
{
- // If no data yet, allocate new
- if (_ranges == NULL)
- {
- try
- {
- _ranges = new uint32_t[length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- _length = length;
- }
- // If there is data, reallocate only if the length is different
- else if (length != _length)
- {
- delete[] _ranges;
- try
- {
- _ranges = new uint32_t[length];
- }
- catch (std::bad_alloc &e)
- {
- _length = 0;
- throw;
- }
- _length = length;
- }
- // Else data is already allocated to the right length, so do nothing
+ // If no data yet, allocate new
+ if (_ranges == NULL)
+ {
+ try
+ {
+ _ranges = new uint32_t[length];
+ }
+ catch (std::bad_alloc &e)
+ {
+ _length = 0;
+ throw;
+ }
+ _length = length;
+ }
+ // If there is data, reallocate only if the length is different
+ else if (length != _length)
+ {
+ delete[] _ranges;
+ try
+ {
+ _ranges = new uint32_t[length];
+ }
+ catch (std::bad_alloc &e)
+ {
+ _length = 0;
+ throw;
+ }
+ _length = length;
+ }
+ // Else data is already allocated to the right length, so do nothing
- if (includeIntensities)
- {
- // If no data yet, allocate new
- if (_intensities == NULL)
- {
- _intensities = new uint32_t[length];
- }
- // If there is data, reallocate only if the length is different
- 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)
- {
- // If not told to allocate space for intensity data and it exists, remove it
- delete[] _intensities;
- _intensities = NULL;
- }
+ if (includeIntensities)
+ {
+ // If no data yet, allocate new
+ if (_intensities == NULL)
+ {
+ _intensities = new uint32_t[length];
+ }
+ // If there is data, reallocate only if the length is different
+ 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)
+ {
+ // If not told to allocate space for intensity data and it exists, remove it
+ delete[] _intensities;
+ _intensities = NULL;
+ }
}
////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -895,834 +904,834 @@
////////////////////////////////////////////////////////////////////////////////////////////////////
HokuyoLaser::HokuyoLaser ()
- : _port (NULL), _scipVersion (2), _verbose (false), _sensorIsUTM30LX (false),
- _enableCheckSumWorkaround (false), _ignoreUnknowns (false), _minAngle (0.0), _maxAngle (0.0),
- _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
+ : _port (NULL), _scipVersion (2), _verbose (false), _sensorIsUTM30LX (false),
+ _enableCheckSumWorkaround (false), _ignoreUnknowns (false), _minAngle (0.0), _maxAngle (0.0),
+ _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
{
}
HokuyoLaser::~HokuyoLaser ()
{
- if (_port != NULL)
- delete _port;
+ if (_port != NULL)
+ delete _port;
}
void HokuyoLaser::Open (string portOptions)
{
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
- portOptions << endl;
- }
- _port = flexiport::CreatePort (portOptions);
- _port->Open ();
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
+ portOptions << endl;
+ }
+ _port = flexiport::CreatePort (portOptions);
+ _port->Open ();
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
- " connection." << endl;
- cerr << _port->GetStatus ();
- }
- _port->Flush ();
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
+ " connection." << endl;
+ cerr << _port->GetStatus ();
+ }
+ _port->Flush ();
- // Figure out the SCIP version currently in use and switch to a higher one if possible
- GetAndSetSCIPVersion ();
- // Get some values we need for providing default ranges
- GetDefaults ();
+ // Figure out the SCIP version currently in use and switch to a higher one if possible
+ GetAndSetSCIPVersion ();
+ // Get some values we need for providing default ranges
+ GetDefaults ();
}
unsigned int HokuyoLaser::OpenWithProbing (string portOptions)
{
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
- portOptions << endl;
- }
- _port = flexiport::CreatePort (portOptions);
- _port->Open ();
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
+ portOptions << endl;
+ }
+ _port = flexiport::CreatePort (portOptions);
+ _port->Open ();
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
- " connection." << endl;
- cerr << _port->GetStatus ();
- }
- _port->Flush ();
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
+ " connection." << endl;
+ cerr << _port->GetStatus ();
+ }
+ _port->Flush ();
- try
- {
- // Figure out the SCIP version currently in use and switch to a higher one if possible
- GetAndSetSCIPVersion ();
- // Get some values we need for providing default ranges
- GetDefaults ();
- }
- catch (HokuyoError)
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ <<
- "() Failed to connect at the default baud rate." << endl;
- }
- 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;
- for (unsigned int ii = 0; ii < numBauds; ii++)
- {
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (bauds[ii]);
- try
- {
- GetAndSetSCIPVersion ();
- GetDefaults ();
- // If the above two functions succeed, break out of the loop and be happy
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Connected at " <<
- bauds[ii] << endl;
- }
- return bauds[ii];
- }
- catch (HokuyoError)
- {
- if (ii == numBauds - 1)
- {
- // Last baud rate, give up and rethrow
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ <<
- "() Failed to connect at any baud rate." << endl;
- }
- throw;
- }
- // Otherwise go around again
- }
- }
- }
- else
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Port is not serial, cannot probe." <<
- endl;
- }
- throw;
- }
- }
+ try
+ {
+ // Figure out the SCIP version currently in use and switch to a higher one if possible
+ GetAndSetSCIPVersion ();
+ // Get some values we need for providing default ranges
+ GetDefaults ();
+ }
+ catch (HokuyoError)
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ <<
+ "() Failed to connect at the default baud rate." << endl;
+ }
+ 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;
+ for (unsigned int ii = 0; ii < numBauds; ii++)
+ {
+ reinterpret_cast<SerialPort*> (_port)->SetBaudRate (bauds[ii]);
+ try
+ {
+ GetAndSetSCIPVersion ();
+ GetDefaults ();
+ // If the above two functions succeed, break out of the loop and be happy
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Connected at " <<
+ bauds[ii] << endl;
+ }
+ return bauds[ii];
+ }
+ catch (HokuyoError)
+ {
+ if (ii == numBauds - 1)
+ {
+ // Last baud rate, give up and rethrow
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ <<
+ "() Failed to connect at any baud rate." << endl;
+ }
+ throw;
+ }
+ // Otherwise go around again
+ }
+ }
+ }
+ else
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Port is not serial, cannot probe." <<
+ endl;
+ }
+ throw;
+ }
+ }
- if (_port->GetPortType () == "serial")
- return reinterpret_cast<SerialPort*> (_port)->GetBaudRate ();
- else
- return 0;
+ if (_port->GetPortType () == "serial")
+ return reinterpret_cast<SerialPort*> (_port)->GetBaudRate ();
+ else
+ return 0;
}
void HokuyoLaser::Close ()
{
- if (!_port)
- throw HokuyoError (HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Closing connection." << endl;
- delete _port;
- _port = NULL;
+ if (!_port)
+ throw HokuyoError (HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Closing connection." << endl;
+ delete _port;
+ _port = NULL;
}
bool HokuyoLaser::IsOpen () const
{
- if (_port != NULL)
- return _port->IsOpen ();
- return false;
+ if (_port != NULL)
+ return _port->IsOpen ();
+ return false;
}
void HokuyoLaser::SetPower (bool on)
{
- if (_scipVersion == 1)
- {
- if (on)
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("L", "1", 1, NULL);
- }
- else
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("L", "0", 1, NULL);
- }
- SkipLines (1);
- }
- else if (_scipVersion == 2)
- {
- if (on)
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("BM", NULL, 0, "02");
- }
- else
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("QT", NULL, 0, "02");
- }
- SkipLines (1);
- }
- else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ if (_scipVersion == 1)
+ {
+ if (on)
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
+ SendCommand ("L", "1", 1, NULL);
+ }
+ else
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
+ SendCommand ("L", "0", 1, NULL);
+ }
+ SkipLines (1);
+ }
+ else if (_scipVersion == 2)
+ {
+ if (on)
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
+ SendCommand ("BM", NULL, 0, "02");
+ }
+ else
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
+ SendCommand ("QT", NULL, 0, "02");
+ }
+ SkipLines (1);
+ }
+ else
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
// This function assumes that both the port and the laser scanner are already set to the same baud.
void HokuyoLaser::SetBaud (unsigned int baud)
{
- if (_port->GetPortType () != "serial")
- {
- throw HokuyoError (HOKUYO_ERR_NOTSERIAL,
- "Cannot change baud rate of non-serial connection.");
- }
+ if (_port->GetPortType () != "serial")
+ {
+ throw HokuyoError (HOKUYO_ERR_NOTSERIAL,
+ "Cannot change baud rate of non-serial connection.");
+ }
- char newBaud[13];
- memset (newBaud, 0, sizeof (char) * 13);
+ char newBaud[13];
+ memset (newBaud, 0, sizeof (char) * 13);
- if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
- baud != 250000 && baud != 500000 && baud != 750000)
- {
- stringstream ss;
- ss << "Bad baud rate: " << baud << endl;
- throw HokuyoError (HOKUYO_ERR_BADARG, ss.str ());
- }
- NumberToString (baud, newBaud, 6);
+ if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
+ baud != 250000 && baud != 500000 && baud != 750000)
+ {
+ stringstream ss;
+ ss << "Bad baud rate: " << baud << endl;
+ throw HokuyoError (HOKUYO_ERR_BADARG, ss.str ());
+ }
+ NumberToString (baud, newBaud, 6);
- if (_scipVersion == 1)
- {
- // Send the command to change baud rate
- SendCommand ("S", newBaud, 13, NULL);
- SkipLines (1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
- }
- else if (_scipVersion == 2)
- {
- // Send the command to change baud rate
- SendCommand ("SS", newBaud, 6, "03");
- SkipLines (1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
- }
- else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ if (_scipVersion == 1)
+ {
+ // Send the command to change baud rate
+ SendCommand ("S", newBaud, 13, NULL);
+ SkipLines (1);
+ // Change the port's baud rate
+ reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ }
+ else if (_scipVersion == 2)
+ {
+ // Send the command to change baud rate
+ SendCommand ("SS", newBaud, 6, "03");
+ SkipLines (1);
+ // Change the port's baud rate
+ reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ }
+ else
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
void HokuyoLaser::Reset ()
{
- if (_scipVersion == 1)
- {
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the reset command.");
- }
- else if (_scipVersion == 2)
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Resetting laser." << endl;
- SendCommand ("RS", NULL, 0, NULL);
- SkipLines (1);
- }
- else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ if (_scipVersion == 1)
+ {
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the reset command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Resetting laser." << endl;
+ SendCommand ("RS", NULL, 0, NULL);
+ SkipLines (1);
+ }
+ else
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
void HokuyoLaser::SetMotorSpeed (unsigned int speed)
{
- if (_scipVersion == 1)
- {
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set motor speed command.");
- }
- else if (_scipVersion == 2)
- {
- // Sanity check the value
- if (speed > 10 && speed != 99)
- throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid motor speed.");
- char buffer[3];
- if (speed == 0)
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
- endl;
- }
- buffer[0] = '0';
- buffer[1] = '0';
- buffer[2] = '\0';
- }
- else if (speed == 99)
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
- endl;
- }
- buffer[0] = '9';
- buffer[1] = '9';
- buffer[2] = '\0';
- }
- else
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Setting motor speed to ratio " <<
- speed << endl;
- }
- NumberToString (speed, buffer, 2);
- }
- SendCommand ("CR", buffer, 2, "03");
- SkipLines (1);
- }
- else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ if (_scipVersion == 1)
+ {
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the set motor speed command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ // Sanity check the value
+ if (speed > 10 && speed != 99)
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid motor speed.");
+ char buffer[3];
+ if (speed == 0)
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
+ endl;
+ }
+ buffer[0] = '0';
+ buffer[1] = '0';
+ buffer[2] = '\0';
+ }
+ else if (speed == 99)
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
+ endl;
+ }
+ buffer[0] = '9';
+ buffer[1] = '9';
+ buffer[2] = '\0';
+ }
+ else
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Setting motor speed to ratio " <<
+ speed << endl;
+ }
+ NumberToString (speed, buffer, 2);
+ }
+ SendCommand ("CR", buffer, 2, "03");
+ SkipLines (1);
+ }
+ else
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
void HokuyoLaser::SetHighSensitivity (bool on)
{
- if (_scipVersion == 1)
- {
- throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set high sensitivity command.");
- }
- else if (_scipVersion == 2)
- {
- if (on)
- {
- if (_verbose)
- cerr << "HokuyoLaser::" << __func__ << "() Switching to high sensitivity." << endl;
- SendCommand ("HS", "1", 1, "02");
- }
- else
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ << "() Switching to normal sensitivity." <<
- endl;
- }
- SendCommand ("HS", "0", 1, "02");
- }
- SkipLines (1);
- }
- else
- throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+ if (_scipVersion == 1)
+ {
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the set high sensitivity command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ if (on)
+ {
+ if (_verbose)
+ cerr << "HokuyoLaser::" << __func__ << "() Switching to high sensitivity." << endl;
+ SendCommand ("HS", "1", 1, "02");
+ }
+ else
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Switching to normal sensitivity." <<
+ endl;
+ }
+ SendCommand ("HS", "0", 1, "02");
+ }
+ SkipLines (1);
+ }
+ else
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
void HokuyoLaser::GetSensorInfo (HokuyoSensorInfo *info)
{
- if (info == NULL)
- throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No info object provided.");
+ if (info == NULL)
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No info object provided.");
- if (_scipVersion == 1)
- {
- if (_verbose)
- {
- cerr << "HokuyoLaser::" << __func__ <<
- "() Getting sensor information using SCIP version 1." << endl;
- }
+ if (_scipVersion == 1)
+ {
+ if (_verbose)
+ {
+ cerr << "HokuyoLaser::" << __func__ <<
+ "() Getting sensor information using SCIP version 1." << endl;
+ }
- info->SetDefaults ();
+ info->SetDefaults ();
- char buffer[SCIP1_LINE_LENGTH];
- memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
+ char buffer[SCIP1_LINE_LENGTH];
+ memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
- SendCommand ("V", NULL, 0, NULL);
- // Get the vendor info line
- ReadLine (buffer);
- info->vendor = &buffer[5]; // Chop off the "VEND:" tag
- // Get the product info line
- ReadLine (buffer);
- info->product = &buffer[5];
- // Get the firmware line
- ReadLine (buffer);
- info->firmware = &buffer[5];
- // Get the protocol version line
- ReadLine (buffer);
- info->protocol = &buffer[5];
- // Get the serial number
- ReadLine (buffer);
- info->serial = &buffer[5];
- // Get either the status line or the end of message
- ReadLine (buffer);
- if (buffer[0] != '\0')
- {
- // Got a status line
- info->sensorDiagnostic = &buffer[5];
- SkipLines (1);
- }
+ SendCommand ("V", NULL, 0, NULL);
+ // Get the vendor info line
+ ReadLine (buffer);
+ info->vendor = &buffer[5]; // Chop off the "VEND:" tag
+ // Get the product info line
+ ReadLine (buffer);
+ info->product = &buffer[5];
+ // Get the firmware line
+ ReadLine (buffer);
+ info->firmware = &buffer[5];
+ // Get the protocol version line
+ ReadLine (buffer);
+ info->protocol = &buffer[5];
+ // Get the serial number
+ ReadLine (buffer);
+ info->serial = &buffer[5];
+ // Get either the status line or the end of message
+ ReadLine (buffer);
+ if (buffer[0] != '\0')
+ {
+ // Got a status line
+ info->sensorDiagnostic = &buffer[5];
+ SkipLines (1);
+ }
- // Check the firmware version major number. If it's >=3 there is probably some extra info
- // in the firmware line.
- // e.g.: 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 (_verbose)
- cerr << "SCIP1 Firmware line for parsing: " << info->firmware << endl;
- // 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 th...
[truncated message content] |
|
From: <gb...@us...> - 2010-05-19 10:15:35
|
Revision: 497
http://gearbox.svn.sourceforge.net/gearbox/?rev=497&view=rev
Author: gbiggs
Date: 2010-05-19 10:15:29 +0000 (Wed, 19 May 2010)
Log Message:
-----------
Only open UDP receiver/sender when can read/write
Modified Paths:
--------------
gearbox/trunk/src/flexiport/udpport.cpp
Modified: gearbox/trunk/src/flexiport/udpport.cpp
===================================================================
--- gearbox/trunk/src/flexiport/udpport.cpp 2010-05-13 08:08:36 UTC (rev 496)
+++ gearbox/trunk/src/flexiport/udpport.cpp 2010-05-19 10:15:29 UTC (rev 497)
@@ -181,8 +181,10 @@
if (_open)
throw PortException ("Attempt to open already-opened port.");
- OpenSender ();
- OpenReceiver ();
+ if (_canWrite)
+ OpenSender ();
+ if (_canRead)
+ OpenReceiver ();
SetSocketBlockingFlag ();
_open = true;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-05-13 08:08:43
|
Revision: 496
http://gearbox.svn.sourceforge.net/gearbox/?rev=496&view=rev
Author: gbiggs
Date: 2010-05-13 08:08:36 +0000 (Thu, 13 May 2010)
Log Message:
-----------
Applied patch from David Lobato to correct version checking
Modified Paths:
--------------
gearbox/trunk/src/flexiport/python/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt
Modified: gearbox/trunk/src/flexiport/python/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/flexiport/python/CMakeLists.txt 2010-05-10 03:42:28 UTC (rev 495)
+++ gearbox/trunk/src/flexiport/python/CMakeLists.txt 2010-05-13 08:08:36 UTC (rev 496)
@@ -5,8 +5,8 @@
endif(NOT PYTHON_LIBRARIES)
# Find Boost::Python
-# There is a new, much better, FindBoost.cmake in 2.6
-if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+# There is a new, much better, FindBoost.cmake since 2.6
+if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" OFF)
MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS)
@@ -28,7 +28,7 @@
MESSAGE (STATUS
"Boost libraries were not found. Cannot build Python bindings for Flexiport.")
endif(Boost_FOUND)
-else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
FIND_PACKAGE (Boost)
if(Boost_FOUND)
# For 2.4, assume that if boost is found then boost::python is present
@@ -48,7 +48,7 @@
MESSAGE (STATUS
"Boost libraries were not found. Cannot build Python bindings for Flexiport.")
endif(Boost_FOUND)
-endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
if(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
MESSAGE (STATUS "Flexiport Python bindings will be built.")
Modified: gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt 2010-05-10 03:42:28 UTC (rev 495)
+++ gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt 2010-05-13 08:08:36 UTC (rev 496)
@@ -5,8 +5,8 @@
endif(NOT PYTHON_LIBRARIES)
# Find Boost::Python
-# There is a new, much better, FindBoost.cmake in 2.6
-if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+# There is a new, much better, FindBoost.cmake since 2.6
+if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" OFF)
MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS)
@@ -28,7 +28,7 @@
MESSAGE (STATUS
"Boost libraries were not found. Cannot build Python bindings for Hokuyo_aist.")
endif(Boost_FOUND)
-else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
FIND_PACKAGE (Boost)
if(Boost_FOUND)
# For 2.4, assume that if boost is found then boost::python is present
@@ -48,7 +48,7 @@
MESSAGE (STATUS
"Boost libraries were not found. Cannot build Python bindings for Hokuyo_aist.")
endif(Boost_FOUND)
-endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION EQUAL 6)
+endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5)
if(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
MESSAGE (STATUS "Hokuyo_aist Python bindings will be built.")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-05-10 03:42:34
|
Revision: 495
http://gearbox.svn.sourceforge.net/gearbox/?rev=495&view=rev
Author: gbiggs
Date: 2010-05-10 03:42:28 +0000 (Mon, 10 May 2010)
Log Message:
-----------
Added history note for new Hokuyo tool
Modified Paths:
--------------
gearbox/trunk/doc/history.dox
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
Modified: gearbox/trunk/doc/history.dox
===================================================================
--- gearbox/trunk/doc/history.dox 2010-05-10 03:38:09 UTC (rev 494)
+++ gearbox/trunk/doc/history.dox 2010-05-10 03:42:28 UTC (rev 495)
@@ -47,6 +47,9 @@
- added a function to access the component status.
- removed functions to specify "infrastructure state" which is now expected to be an optional subsystem.
+- libhokuyo_aist:
+ - Added a useful utility to get a laser's ID and use that to create nodes in /dev. Thanks to Luiz Mirisola.
+
@par Retired libraries
@section gbx_doc_history_911 Changes in Release 9.11
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-05-10 03:38:09 UTC (rev 494)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-05-10 03:42:28 UTC (rev 495)
@@ -29,7 +29,6 @@
GBX_ADD_HEADERS (${libName} ${hdrs})
add_subdirectory (getid)
-
if (GBX_BUILD_TESTS)
add_subdirectory (test)
endif (GBX_BUILD_TESTS)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-05-10 03:38:17
|
Revision: 494
http://gearbox.svn.sourceforge.net/gearbox/?rev=494&view=rev
Author: gbiggs
Date: 2010-05-10 03:38:09 +0000 (Mon, 10 May 2010)
Log Message:
-----------
Added useful utility from Luiz Mirisola
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/src/hokuyo_aist/getid/
gearbox/trunk/src/hokuyo_aist/getid/96-hokuyo.rules
gearbox/trunk/src/hokuyo_aist/getid/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/getid/doc.dox
gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-05-10 03:38:09 UTC (rev 494)
@@ -28,6 +28,8 @@
GBX_ADD_HEADERS (${libName} ${hdrs})
+ add_subdirectory (getid)
+
if (GBX_BUILD_TESTS)
add_subdirectory (test)
endif (GBX_BUILD_TESTS)
Added: gearbox/trunk/src/hokuyo_aist/getid/96-hokuyo.rules
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/96-hokuyo.rules (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/getid/96-hokuyo.rules 2010-05-10 03:38:09 UTC (rev 494)
@@ -0,0 +1 @@
+ACTION=="add|change", KERNEL=="ttyACM*", ATTRS{idVendor}=="15d1", PROGRAM="/usr/local/bin/hokuyo_aist_getid -o type=serial,timeout=1,device=%N", SYMLINK+="hokuyo_%c"
Added: gearbox/trunk/src/hokuyo_aist/getid/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/CMakeLists.txt (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/getid/CMakeLists.txt 2010-05-10 03:38:09 UTC (rev 494)
@@ -0,0 +1,5 @@
+INCLUDE (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
+
+GBX_ADD_EXECUTABLE(hokuyo_aist_getid getid.cpp)
+TARGET_LINK_LIBRARIES (hokuyo_aist_getid hokuyo_aist flexiport)
+
Added: gearbox/trunk/src/hokuyo_aist/getid/doc.dox
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/doc.dox (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/getid/doc.dox 2010-05-10 03:38:09 UTC (rev 494)
@@ -0,0 +1,37 @@
+/*!
+
+@ingroup gbx_libs
+@ingroup gbx_hardware
+@ingroup gbx_cpp
+@defgroup gbx_library_hokuyo_aist libhokuyo_aist
+@brief Utility to get the serial number of an Hokuyo laser scanner driver.
+
+This code is a stripped down version of the hokuyo_aist_example that just connects to the device, prints out its serial number, and closes the device.
+
+It is useful to write udev scripts to create permanent symlinks to your lasers, in the case you have more than one laser.
+Acutally iy would be easier if the serial number were provided by hardware as a udev attribute, but it is not. So, this small soft must be called to find the serial number, in order to discriminate between various laser scanners.
+
+The file 96-hokuyo.rules should be copied to /etc/udev/rules.d, and then the udev deamon should be restarted with:
+
+sudo /etc/init.d/udev stop
+sudo /etc/init.d/udev start
+
+Attention: use stop and start instead of only restart. It appears that the restart script does not reload the rules. You should see a message about reloading hardware drivers when the start command is issued.
+
+Afterwards when a hokuyo is plugged, the device /dev/ttyACM* will be generated as before, but besides that a symbolic link called hokuyo_XXXXXX will be generated, where XXXXX is the laser serial number.
+
+@par Responsible Developer
+ Luiz Mirisola
+
+@par License
+ LGPL
+
+@par Dependencies
+ @ref Flexiport
+
+@par Known issues
+ - When the user plugs the hokuyo, the udev rules call this program, that will connect to the laser. This process is quite slow.
+ - When the rule fires, there is no /dev/ttyACM* yet. Therefore, the %N tag in the rule returns the name of a temporary device in order that the software may connect to the laser. This solves the chicken-egg problem.
+
+*/
+
Added: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp 2010-05-10 03:38:09 UTC (rev 494)
@@ -0,0 +1,86 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008 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 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.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * http://www.opensource.org/licenses/eclipse-1.0.txt
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <iostream>
+using namespace std;
+
+#include <hokuyo_aist/hokuyo_aist.h>
+
+int main(int argc, char **argv)
+{
+ string portOptions = "type=serial,device=/dev/ttyACM0,timeout=1";
+ bool verbose = false;
+
+#if defined (WIN32)
+ portOptions = "type=serial,device=COM3,timeout=1";
+#else
+ int opt;
+ // Get some options from the command line
+ while ((opt = getopt(argc, argv, "b:c:e:f: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;
+ }
+ }
+#endif // defined (WIN32)
+
+ try
+ {
+ hokuyo_aist::HokuyoLaser laser; // Laser scanner object
+ // Set the laser to verbose mode (so we see more information in the console)
+ if (verbose)
+ laser.SetVerbose (true);
+
+ // Open the laser
+ laser.Open (portOptions);
+ // Turn the laser on
+ laser.SetPower (true);
+
+ // Get some laser info
+ //cout << "Laser sensor information:" << endl;
+ hokuyo_aist::HokuyoSensorInfo info;
+ laser.GetSensorInfo (&info);
+ cout << info.serial << endl;
+
+ // Close the laser
+ laser.Close ();
+ }
+ catch (hokuyo_aist::HokuyoError e)
+ {
+ cerr << "Caught exception: (" << e.Code () << ") " << e.what () << endl;
+ return 1;
+ }
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-04-05 00:47:45
|
Revision: 493
http://gearbox.svn.sourceforge.net/gearbox/?rev=493&view=rev
Author: gbiggs
Date: 2010-04-05 00:47:38 +0000 (Mon, 05 Apr 2010)
Log Message:
-----------
Applied patch #2951375: pkg-config generation
Modified Paths:
--------------
gearbox/trunk/src/gbxgarminacfr/CMakeLists.txt
gearbox/trunk/src/gbxnovatelacfr/CMakeLists.txt
gearbox/trunk/src/gbxserialacfr/CMakeLists.txt
gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
Modified: gearbox/trunk/src/gbxgarminacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/gbxgarminacfr/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -19,7 +19,7 @@
GBX_ADD_LIBRARY( ${lib_name} DEFAULT ${lib_version} ${srcs} )
target_link_libraries( ${lib_name} ${dep_libs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} dep_libs "" "" "" ${lib_version} )
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" dep_libs "" "" ${lib_version} )
GBX_ADD_HEADERS( gbxgarminacfr ${hdrs} )
Modified: gearbox/trunk/src/gbxnovatelacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxnovatelacfr/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/gbxnovatelacfr/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -23,7 +23,7 @@
GBX_ADD_LIBRARY( ${lib_name} DEFAULT ${lib_version} ${srcs} )
target_link_libraries( ${lib_name} ${dep_libs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} dep_libs "" "" "" ${lib_version} )
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" dep_libs "" "" ${lib_version} )
GBX_ADD_HEADERS( gbxnovatelacfr ${hdrs} )
Modified: gearbox/trunk/src/gbxserialacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxserialacfr/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/gbxserialacfr/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -25,7 +25,7 @@
GBX_ADD_LIBRARY( ${lib_name} DEFAULT ${lib_version} ${srcs} )
target_link_libraries( ${lib_name} ${dep_libs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} dep_libs "" "" "" ${lib_version})
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" dep_libs "" "" ${lib_version})
GBX_ADD_HEADERS( gbxserialacfr ${hdrs} )
Modified: gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -27,7 +27,7 @@
GBX_ADD_LIBRARY( ${lib_name} DEFAULT ${lib_version} ${srcs} )
target_link_libraries( ${lib_name} ${dep_libs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} dep_libs "" "" "" ${lib_version} )
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" dep_libs "" "" ${lib_version} )
GBX_ADD_HEADERS( gbxsickacfr ${hdrs} )
Modified: gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -20,7 +20,7 @@
GBX_ADD_LIBRARY( ${lib_name} DEFAULT ${lib_version} ${srcs} )
target_link_libraries( ${lib_name} ${dep_libs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} dep_libs "" "" "" ${lib_version} )
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" dep_libs "" "" ${lib_version} )
GBX_ADD_HEADERS( gbxsmartbatteryacfr ${hdrs} )
Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-04-04 07:45:13 UTC (rev 492)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt 2010-04-05 00:47:38 UTC (rev 493)
@@ -24,7 +24,7 @@
endif (WIN32)
GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
target_link_libraries (${libName} ${reqLibs})
- GBX_ADD_PKGCONFIG (${libName} ${libDesc} reqLibs "" "" "" ${libVersion})
+ GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
GBX_ADD_HEADERS (${libName} ${hdrs})
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rus...@us...> - 2010-04-04 07:45:19
|
Revision: 492
http://gearbox.svn.sourceforge.net/gearbox/?rev=492&view=rev
Author: russo2503v
Date: 2010-04-04 07:45:13 +0000 (Sun, 04 Apr 2010)
Log Message:
-----------
Modified Paths:
--------------
gearbox/trunk/LICENSE
Modified: gearbox/trunk/LICENSE
===================================================================
--- gearbox/trunk/LICENSE 2010-04-04 07:43:48 UTC (rev 491)
+++ gearbox/trunk/LICENSE 2010-04-04 07:45:13 UTC (rev 492)
@@ -15,4 +15,4 @@
src/gbxsickacfr/gbxiceutilacfr LGPL2+
src/gbxsickacfr/gbxserialdeviceacfr LGPL2+
src/gbxsmartbatteryacfr LGPL2+
-src/hokuyo_aist LGPL3
+src/hokuyo_aist EPL1.0
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rus...@us...> - 2010-04-04 07:43:55
|
Revision: 491
http://gearbox.svn.sourceforge.net/gearbox/?rev=491&view=rev
Author: russo2503v
Date: 2010-04-04 07:43:48 +0000 (Sun, 04 Apr 2010)
Log Message:
-----------
comments only
Modified Paths:
--------------
gearbox/trunk/cmake/gearbox-use-file.cmake
Modified: gearbox/trunk/cmake/gearbox-use-file.cmake
===================================================================
--- gearbox/trunk/cmake/gearbox-use-file.cmake 2010-03-21 05:24:44 UTC (rev 490)
+++ gearbox/trunk/cmake/gearbox-use-file.cmake 2010-04-04 07:43:48 UTC (rev 491)
@@ -1,9 +1,12 @@
# This script is indended for Gearbox users (not for internal use)
-# Assumes that a variable GEARBOX_INCLUDE_DIR is defined.
+# Assumes that the following variables are already defined
+# (typically by first loading gearbox-config.cmake)
+# - GEARBOX_INCLUDE_DIR
+# - GEARBOX_LINK_DIR
include_directories( ${GEARBOX_INCLUDE_DIR} )
-# Variable GEARBOX_LINK_DIR only needs to be defined if you're using a gearbox
+# Variable GEARBOX_LINK_DIR only needs to be defined if you're using a gearbox
# installation (as opposed to a local gearbox repository)
link_directories( ${GEARBOX_LINK_DIR} )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-03-21 05:24:56
|
Revision: 490
http://gearbox.svn.sourceforge.net/gearbox/?rev=490&view=rev
Author: borax00
Date: 2010-03-21 05:24:44 +0000 (Sun, 21 Mar 2010)
Log Message:
-----------
Added Ben Kehoe's suggestion
Modified Paths:
--------------
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/threadutils.cpp
Modified: gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/threadutils.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/threadutils.cpp 2010-03-19 02:35:39 UTC (rev 489)
+++ gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/threadutils.cpp 2010-03-21 05:24:44 UTC (rev 490)
@@ -18,6 +18,13 @@
{
assert( activity && "Null activity pointer" );
+ // Handle durations less than the check interval
+ if ( duration.toMilliSeconds() < checkIntervalMs )
+ {
+ IceUtil::ThreadControl::sleep( duration );
+ return;
+ }
+
IceUtil::Time wakeupTime = IceUtil::Time::now() + duration;
IceUtil::Time checkInterval = IceUtil::Time::milliSeconds( checkIntervalMs );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-03-19 02:35:50
|
Revision: 489
http://gearbox.svn.sourceforge.net/gearbox/?rev=489&view=rev
Author: borax00
Date: 2010-03-19 02:35:39 +0000 (Fri, 19 Mar 2010)
Log Message:
-----------
Modified Paths:
--------------
gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
Modified: gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp 2010-03-18 07:54:14 UTC (rev 488)
+++ gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp 2010-03-19 02:35:39 UTC (rev 489)
@@ -3,6 +3,7 @@
#include "nmeasentence.h"
#include <memory>
#include <cstdlib>
+#include <cstdio>
#include <gbxutilacfr/exceptions.h>
#include <gbxutilacfr/mathdefs.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-03-18 07:54:25
|
Revision: 488
http://gearbox.svn.sourceforge.net/gearbox/?rev=488&view=rev
Author: borax00
Date: 2010-03-18 07:54:14 +0000 (Thu, 18 Mar 2010)
Log Message:
-----------
MR Changes
Modified Paths:
--------------
gearbox/trunk/doc/history.dox
gearbox/trunk/src/gbxgarminacfr/driver.cpp
gearbox/trunk/src/gbxgarminacfr/driver.h
gearbox/trunk/src/gbxgarminacfr/test/test.cpp
gearbox/trunk/src/gbxsickacfr/driver.cpp
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/buffer.h
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/test/buffertest.cpp
gearbox/trunk/src/gbxsickacfr/gbxiceutilacfr/thread.cpp
gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/CMakeLists.txt
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserver.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserver.h
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserverhealthchecks.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserversystem.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/oceanserversystem.h
gearbox/trunk/src/gbxsmartbatteryacfr/smartbattery.h
gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/test/shorttest.cpp
gearbox/trunk/src/gbxutilacfr/exceptions.cpp
gearbox/trunk/src/gbxutilacfr/status.cpp
gearbox/trunk/src/gbxutilacfr/status.h
gearbox/trunk/src/gbxutilacfr/subhealth.h
gearbox/trunk/src/gbxutilacfr/substatus.h
gearbox/trunk/src/gbxutilacfr/test/trivialstatustest.cpp
gearbox/trunk/src/gbxutilacfr/trivialstatus.cpp
gearbox/trunk/src/gbxutilacfr/trivialstatus.h
Added Paths:
-----------
gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
gearbox/trunk/src/gbxgarminacfr/nmeamessages.h
gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp
gearbox/trunk/src/gbxgarminacfr/nmeasentence.h
Removed Paths:
-------------
gearbox/trunk/src/gbxgarminacfr/nmea.cpp
gearbox/trunk/src/gbxgarminacfr/nmea.h
Modified: gearbox/trunk/doc/history.dox
===================================================================
--- gearbox/trunk/doc/history.dox 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/doc/history.dox 2010-03-18 07:54:14 UTC (rev 488)
@@ -21,15 +21,6 @@
Developers: as you make substantial changes in the code (add features, fix bugs, etc.), add an item in the section on changes. When the next version is released, this list will be copied into the release notes.
-POST 9.11 CHANGES
-
-@par Project wide
-
-- Build system
- - GBX_REQUIRE_LIB macro now uses a direct check for target existence.
- - new GBX_ADD_HEADER_ONLY_LIBRARY macro for handling dependencies on header-only libraries.
-
-
@section gbx_doc_history_todo To-Do List for Next Release
None.
@@ -38,10 +29,24 @@
@par Project wide
+- Build system
+ - GBX_REQUIRE_LIB macro now uses a direct check for target existence.
+ - new GBX_ADD_HEADER_ONLY_LIBRARY macro for handling dependencies on header-only libraries.
+
@par New libraries
@par Updated libraries
+- libGbxUtilAcfr
+ - Status class:
+ - functions for changing the state, initialising(), working(), finalising(), no longer
+ change the message. A separate call to one of the health-related function is needed if a change is
+ required.
+ - Renamed health SubsystemFault to SubsystemCritical, and function fault() to critical().
+ - defined enums for component state and health.
+ - added a function to access the component status.
+ - removed functions to specify "infrastructure state" which is now expected to be an optional subsystem.
+
@par Retired libraries
@section gbx_doc_history_911 Changes in Release 9.11
@@ -55,6 +60,9 @@
@par Updated libraries
+- libGbxSmartBatteryAcfr (tobi)
+ - Oceanserver class now has its own thread which reads continuously from the hardware
+
- libGbxNovatelAcfr:
- Bugfix: matched GPS status/solution enums to Novatel's internal types, there were gaps due to reserved values before.
Note: this will create mismatches with old log-files (not data, status/solution-type only)! (MichaelM, patch by Ian Mahon)
Modified: gearbox/trunk/src/gbxgarminacfr/driver.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.cpp 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/driver.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -12,14 +12,13 @@
#include <iostream>
#include <sstream>
#include <gbxutilacfr/gbxutilacfr.h>
-#include "nmea.h"
+#include "nmeasentence.h"
#include <cstdlib>
#include <cstring>
#include <sys/time.h>
#include <time.h>
#include <errno.h>
#include <stdio.h>
-
#include "driver.h"
using namespace std;
@@ -28,243 +27,27 @@
namespace gbxgarminacfr {
-namespace {
-
-// Get the useful bits from a GGA message
-GenericData* extractGgaData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
+bool
+Config::isValid( std::string &reason ) const
{
- std::auto_ptr<GgaData> data( new GgaData );
-
- data->timeStampSec = timeSec;
- data->timeStampUsec = timeUsec;
-
- //Names for the tokens in the GGA message
- enum GgaTokens{MsgType=0,UTC,Lat,LatDir,Lon,LonDir,FixType,
- NSatsUsed,HDOP,Hgt,M1,GeoidHgt,M2,DiffAge,DiffId};
-
- //UTC time
- sscanf(msg.getDataToken(UTC).c_str(),"%02d%02d%lf",
- &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
-
- //number of satellites in use
- data->satellites = atoi(msg.getDataToken(NSatsUsed).c_str());
-
- // fix type
- switch ( msg.getDataToken(FixType)[0] )
+ if ( device.empty() )
{
- case '0':
- data->fixType = Invalid;
- // NOTE: not processing the rest!
- data->latitude = 0.0;
- data->longitude = 0.0;
- data->altitude = 0.0;
- data->geoidalSeparation = 0.0;
- return data.release();
- case '1':
- data->fixType = Autonomous;
- break;
- case '2':
- data->fixType = Differential;
- break;
- default :
- throw gbxutilacfr::Exception( ERROR_INFO, "GGA sentence contains unknown GPS fix type: '"+msg.getDataToken(FixType)+"'" );
+ reason = "device not set";
+ return false;
}
-
- //position
- int deg;
- double min;
- double dir;
-
- //latitude
- sscanf(msg.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
- dir = (*msg.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
- data->latitude=dir*(deg+(min/60.0));
- //longitude
- sscanf(msg.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
- dir = (*msg.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
- data->longitude=dir*(deg+(min/60.0));
-
- //altitude
- data->isAltitudeKnown = !msg.isDataTokenEmpty(Hgt);
- if ( data->isAltitudeKnown )
- data->altitude=atof(msg.getDataToken(Hgt).c_str());
-
- //geoidal Separation
- data->geoidalSeparation=atof(msg.getDataToken(GeoidHgt).c_str());
- return data.release();
-}
-
-// VTG provides velocity and heading information
-GenericData* extractVtgData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<VtgData> data( new VtgData );
-
- data->timeStampSec = timeSec;
- data->timeStampUsec = timeUsec;
-
- //Names for the VTG message items
- enum VtgTokens{MsgType=0,HeadingTrue,T,HeadingMag,M,SpeedKnots,
- N,SpeedKPH,K,ModeInd};
-
- //Check for an empty string. Means that we can't tell anything useful.
- if ( msg.isDataTokenEmpty(HeadingTrue) )
+ if ( !( readGga || readVtg || readRme || readRmc ) )
{
- data->isValid = false;
- data->headingTrue = 0.0;
- data->headingMagnetic = 0.0;
- data->speed = 0.0;
- return data.release();
+ reason = "not configured to read any message types";
+ return false;
}
- data->isValid = true;
- // true heading
- double headingRad = DEG2RAD(atof(msg.getDataToken(HeadingTrue).c_str()));
- NORMALISE_ANGLE( headingRad );
- data->headingTrue=headingRad;
-
- // magnetic heading
- headingRad = DEG2RAD(atof(msg.getDataToken(HeadingMag).c_str()));
- NORMALISE_ANGLE( headingRad );
- data->headingMagnetic=headingRad;
-
- //speed - converted to m/s
- data->speed=atof(msg.getDataToken(SpeedKPH).c_str());
- data->speed*=(1000/3600.0);
-
- return data.release();
-}
-
-// RME message. This one is garmin specific... Give position error estimates
-// See doc.dox for a discussion of the position errors as reported here.
-// Essentially the EPE reported by the garmin is a 1 sigma error (RMS) or a
-// 68% confidence bounds.
-GenericData* extractRmeData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<RmeData> data( new RmeData );
-
- data->timeStampSec = timeSec;
- data->timeStampUsec = timeUsec;
-
- //Names for the RME message items
- enum RmeTokens{MsgType=0,HError,M1,VError,M2,EPE,M3};
-
- if ( msg.isDataTokenEmpty(HError) )
+ if ( protocol != "Garmin" && protocol != "NMEA" )
{
- // No valid information
- data->isValid = false;
- data->isVerticalPositionErrorValid = false;
- data->horizontalPositionError = 0.0;
- data->verticalPositionError = 0.0;
- data->estimatedPositionError = 0.0;
- return data.release();
+ reason = "invalid protocol";
+ return false;
}
- data->isValid = true;
- data->horizontalPositionError = atof(msg.getDataToken(HError).c_str());
-
- if ( msg.isDataTokenEmpty(VError) )
- {
- data->isVerticalPositionErrorValid = false;
- data->verticalPositionError = -1;
- }
- else
- {
- data->isVerticalPositionErrorValid = true;
- data->verticalPositionError = atof(msg.getDataToken(VError).c_str());
- }
-
- data->estimatedPositionError = atof(msg.getDataToken(EPE).c_str());
- return data.release();
-}
-
-// RMC provides a combination of GGA and VTG data
-GenericData* extractRmcData( const gbxgpsutilacfr::NmeaMessage& msg, int timeSec, int timeUsec )
-{
- std::auto_ptr<RmcData> data( new RmcData );
-
- data->timeStampSec = timeSec;
- data->timeStampUsec = timeUsec;
-
- //Names for the RMC message items
- enum RmcTokens{MsgType=0,UTC,Status,Lat,LatDir,Lon,LonDir,SpeedKnots,
- HeadingTrue,DiffAge,MagneticVar,MagneticDir,ModeInd};
-
- //Check for an empty string. Means that we can't tell anything useful.
- if ( msg.isDataTokenEmpty(HeadingTrue) )
- {
- data->isValid = false;
- data->headingTrue = 0.0;
- data->headingMagnetic = 0.0;
- data->speed = 0.0;
- return data.release();
- }
- data->isValid = true;
-
- //UTC time
- sscanf(msg.getDataToken(UTC).c_str(),"%02d%02d%lf",
- &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
-
- //position
- int deg;
- double min;
- double dir;
-
- //latitude
- sscanf(msg.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
- dir = (*msg.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
- data->latitude=dir*(deg+(min/60.0));
- //longitude
- sscanf(msg.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
- dir = (*msg.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
- data->longitude=dir*(deg+(min/60.0));
-
- // true heading
- double headingRad = DEG2RAD(atof(msg.getDataToken(HeadingTrue).c_str()));
- NORMALISE_ANGLE( headingRad );
- data->headingTrue=headingRad;
-
- // magnetic heading
- double headingVar;
- try {
- // Magnetic deviation from true
- headingVar = DEG2RAD(atof(msg.getDataToken(MagneticVar).c_str()));
- // Direction of magnetic deviation
- if (*msg.getDataToken(MagneticDir).c_str() == 'E')
- headingRad -= headingVar;
- else
- headingRad += headingVar;
- }
- catch ( const gbxgpsutilacfr::NmeaException& e ) {
- // If this occurs, cannot get magnetic heading. Set it to 0.
- headingRad = 0.0;
- }
- NORMALISE_ANGLE( headingRad );
- data->headingMagnetic=headingRad;
-
- //speed - converted from knots to m/s
- data->speed=atof(msg.getDataToken(SpeedKnots).c_str());
- // knots to kph
- data->speed*=1.852;
- // kph to m/s
- data->speed*=(1000/3600.0);
-
- return data.release();
-}
-
-}
-
-///////////////////////////////////////
-
-bool
-Config::isValid() const
-{
- if ( device.empty() )
- return false;
-
- if ( !( readGga || readVtg || readRme ) )
- return false;
-
return true;
}
@@ -274,23 +57,26 @@
std::stringstream ss;
ss << "Garmin driver config: " << endl
<< "\tdevice="<<device << endl
+ << "\tprotocol="<<protocol << endl
<< "\twill read sentences: GPGGA="<<readGga<<" GPVTG="<<readVtg<<" PGRME="<<readRme<<" GPRMC="<<readRmc;
return ss.str();
}
///////////////////////////////////////
-Driver::Driver( const Config &config,
- gbxutilacfr::Tracer &tracer,
- gbxutilacfr::Status &status ) :
+Driver::Driver( const Config &config,
+ gbxutilacfr::Tracer &tracer,
+ gbxutilacfr::Status &status,
+ int serialDebugLevel ) :
config_(config),
tracer_(tracer),
status_(status)
{
- if ( !config_.isValid() )
+ std::string reason;
+ if ( !config_.isValid(reason) )
{
stringstream ss;
- ss << "Invalid config: " << config_.toString();
+ ss << "Invalid config: " << reason << endl << config_.toString();
throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
}
@@ -302,8 +88,10 @@
int baud = 4800;
// the first 5 initialization messages come in 1 sec. Therefore, 2 secs should be conservative.
- serial_.reset( new gbxserialacfr::Serial( config_.device, baud, gbxserialacfr::Serial::Timeout(2,0) ) );
-
+ serial_.reset( new gbxserialacfr::Serial( config_.device,
+ baud,
+ gbxserialacfr::Serial::Timeout(2, 0),
+ serialDebugLevel ) );
init();
}
@@ -346,37 +134,37 @@
// Non-Garmin devices do not support message enabling and disabling
if (config_.protocol != "Garmin") return;
- //Create the messages that we are going to send and add the checksums
- //Note that the checksum field is filled with 'x's before we start
+ // Create the messages that we are going to send and add the checksums
+ // Note that the checksum field is filled with 'x's before we start
//First disables all output messages then enable selected ones only.
tracer_.debug( "Driver::enableDevice(): calling disableAllMsg", 10 );
- gbxgpsutilacfr::NmeaMessage disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( disableAllMsg.sentence() );
// alexb: what is this sleep for?
- sleep(1);
+ //sleep(1);
if ( config_.readGga ) {
tracer_.debug( "Driver::enableDevice(): calling enableGgaMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableGgaMsg( "$PGRMO,GPGGA,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableGgaMsg( "$PGRMO,GPGGA,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableGgaMsg.sentence() );
}
if ( config_.readVtg ) {
tracer_.debug( "Driver::enableDevice(): calling enableVtgMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableVtgMsg( "$PGRMO,GPVTG,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableVtgMsg( "$PGRMO,GPVTG,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableVtgMsg.sentence() );
}
if ( config_.readRme ) {
tracer_.debug( "Driver::enableDevice(): calling enableRmeMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableRmeMsg( "$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableRmeMsg( "$PGRMO,PGRME,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableRmeMsg.sentence() );
}
if ( config_.readRmc ) {
tracer_.debug( "Driver::enableDevice(): calling enableRmcMsg", 10 );
- gbxgpsutilacfr::NmeaMessage enableRmcMsg( "$PGRMO,GPRMC,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence enableRmcMsg( "$PGRMO,GPRMC,1*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( enableRmcMsg.sentence() );
}
@@ -388,7 +176,7 @@
Driver::disableDevice()
{
// Simply send the no messages command!
- gbxgpsutilacfr::NmeaMessage disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
+ gbxgpsutilacfr::NmeaSentence disableAllMsg( "$PGRMO,,2*xx\r\n",gbxgpsutilacfr::AddChecksum );
serial_->writeString( disableAllMsg.sentence() );
}
@@ -396,7 +184,7 @@
Driver::read()
{
std::auto_ptr<GenericData> genericData;
- gbxgpsutilacfr::NmeaMessage nmeaMessage;
+ gbxgpsutilacfr::NmeaSentence nmeaMessage;
int nmeaExceptionCount = 0;
int nmeaFailChecksumCount = 0;
@@ -434,18 +222,6 @@
assert( *(serialData.end()-1) == '\n' && "Driver: Serial data did not end in '\n'" );
serialData.erase( serialData.end()-1 );
- // dealing with unexplained cntrl characters in the message
-/*
- for ( string::iterator it=serialData.begin(); it!=serialData.end(); ++it ) {
- if ( iscntrl( *it ) ) {
- // debug
- cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<" before='"<<serialData<<"'"<<endl;
- cout<<now.tv_sec<<" "<<now.tv_usec<<" removing control char="<<std::hex<<(unsigned int)*it<<std::dec<<endl;
- it = serialData.erase( it );
- cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<" after='"<<serialData<<"'"<<endl;
- }
- }
-*/
//Put it into the message object and checksum the data
try {
// This throws if it cannot find the * to deliminate the checksum field
@@ -463,19 +239,13 @@
}
nmeaExceptionCount = 0;
- // debug
-// if ( nmeaFailChecksumCount>0 )
-// cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<nmeaFailChecksumCount<<" the sentence AFTER, raw='"<<serialData<<"'"<<endl<<endl;
-
// Only populate the data structures if our message passes the checksum!
const int nmeaFailChecksumMaxCount = 10;
if ( !nmeaMessage.haveValidChecksum() ) {
// Dont throw an exception on the first failed checksum.
if ( nmeaFailChecksumCount++ < nmeaFailChecksumMaxCount ) {
-// tracer_.warning("Gps driver: Single message failed checksum. Not throwing an exception yet!" );
// debug
cout<<now.tv_sec<<" "<<now.tv_usec<<" "<<nmeaFailChecksumCount<<endl;
-// <<" sentence='"<<nmeaMessage.sentence()<<"' raw='"<<serialData<<"'"<<endl;
continue;
}
else {
@@ -486,7 +256,7 @@
}
nmeaFailChecksumCount = 0;
- //First split up the data fields in the string we have read.
+ // First split up the data fields in the string we have read.
nmeaMessage.parseTokens();
// We should not get any messages with failed checksums, but just in case
@@ -548,7 +318,7 @@
}
else {
stringstream ss; ss << "Message type unknown " << MsgType <<endl;
- // if we get here the msg is unknown
+ // if we get here the MsgType is unknown
if ( config_.ignoreUnknown )
tracer_.debug( ss.str() );
else
Modified: gearbox/trunk/src/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.h 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/driver.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -15,6 +15,7 @@
#include <gbxutilacfr/tracer.h>
#include <gbxutilacfr/status.h>
#include <memory>
+#include <gbxgarminacfr/nmeamessages.h>
namespace gbxgarminacfr {
@@ -23,6 +24,7 @@
{
public:
Config() :
+ protocol("Garmin"),
readGga(true),
readVtg(true),
readRme(true),
@@ -30,9 +32,7 @@
ignoreUnknown(false) {};
//! Returns true if the configuration is sane. Checks include:
- //! - a non-empty device name
- //! - at least one read-sentence flag set to TRUE
- bool isValid() const;
+ bool isValid( std::string &reason ) const;
//! Returns human-readable configuration description.
std::string toString() const;
@@ -58,200 +58,6 @@
bool ignoreUnknown;
};
-//! Possible types GenericData can contain
-enum DataType
-{
- //! Contents of PGGGA message.
- GpGga,
- //! Contents of PGVTG message.
- GpVtg,
- //! Contents of PGRME message.
- PgRme,
- //! Contents of GPRMC message.
- GpRmc
-};
-
-//! Generic data type returned by a read
-class GenericData
-{
-public:
- virtual ~GenericData() {};
- //! Returns data type.
- virtual DataType type() const=0;
-
-private:
-};
-
-//! GPS fix types.
-enum FixType
-{
- //! Invalid or not available
- Invalid,
- //! Autonomous position
- //! (This is the normal case for non-differential GPS)
- Autonomous,
- //! Differentially corrected
- Differential
-};
-std::string toString( const FixType &f );
-
-//! Fix data structure. Note that when fixType is Invalid, all other data except the time stamps
-//! are meaningless.
-struct GgaData : public GenericData
-{
-public:
- DataType type() const { return GpGga; }
-
- //! Time (according to the computer clock) when data was measured.
- //! Number of seconds
- int timeStampSec;
- //! Time (according to the computer clock) when data was measured.
- //! Number of microseconds
- int timeStampUsec;
-
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Hour [0..23]
- int utcTimeHrs;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Minutes [0..59]
- int utcTimeMin;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Seconds [0.0..59.9999(9)]
- double utcTimeSec;
-
- //! Latitude [degrees]
- double latitude;
- //! Longitude [degrees]
- double longitude;
- //! Altitude is meaningful if and only if isAltitudeKnown
- bool isAltitudeKnown;
- //! Altitude [metres above ellipsoid] (only meaningful if isAltitudeKnown)
- double altitude;
-
- //! Fix type. When fixType is Invalid, all other data except the time stamps
- //! are meaningless.
- FixType fixType;
-
- //! Number of satellites
- int satellites;
-
- //! Horizontal dilution of position [metres]
- double horizontalDilutionOfPosition;
-
- //! Height of geoid (mean sea level) above WGS84 ellipsoid [metres]
- double geoidalSeparation;
-};
-std::string toString( const GgaData &d );
-inline std::ostream &operator<<( std::ostream &s, const GgaData &d )
-{ return s << toString(d); }
-
-//! Vector track and speed over ground data structure.
-class VtgData : public GenericData
-{
-public:
- DataType type() const { return GpVtg; }
-
- //! Time (according to the computer clock) when data was measured.
- //! Number of seconds
- int timeStampSec;
- //! Time (according to the computer clock) when data was measured.
- //! Number of microseconds
- int timeStampUsec;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! Heading/track/course with respect to true North [rad]
- double headingTrue;
- //! Heading/track/course with respect to magnetic North [rad]
- double headingMagnetic;
- //! Horizontal velocity [metres/second]
- double speed;
-};
-std::string toString( const VtgData &d );
-inline std::ostream &operator<<( std::ostream &s, const VtgData &d )
-{ return s << toString(d); }
-
-//! Gps data structure
-//! (This one is Garmin-specific)
-class RmeData : public GenericData
-{
-public:
- DataType type() const { return PgRme; }
-
- //! Time (according to the computer clock) when data was measured.
- //! Number of seconds
- int timeStampSec;
- //! Time (according to the computer clock) when data was measured.
- //! Number of microseconds
- int timeStampUsec;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! When false, means that the GPS unit can't tell us anything
- //! about our vertical error
- bool isVerticalPositionErrorValid;
-
- //! Horizontal position error: one standard deviation [metres)]
- double horizontalPositionError;
- //! Vertical position error: one standard deviation [metres]
- double verticalPositionError;
-
- //! Estimated position error.
- double estimatedPositionError;
-};
-std::string toString( const RmeData &d );
-inline std::ostream &operator<<( std::ostream &s, const RmeData &d )
-{ return s << toString(d); }
-
-//! Gps data structure
-class RmcData : public GenericData
-{
-public:
- DataType type() const { return GpRmc; }
-
- //! Time (according to the computer clock) when data was measured.
- //! Number of seconds
- int timeStampSec;
- //! Time (according to the computer clock) when data was measured.
- //! Number of microseconds
- int timeStampUsec;
-
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Hour [0..23]
- int utcTimeHrs;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Minutes [0..59]
- int utcTimeMin;
- //! UTC time (according to the GPS device), reference is Greenwich.
- //! Seconds [0.0..59.9999(9)]
- double utcTimeSec;
-
- //! Latitude [degrees]
- double latitude;
- //! Longitude [degrees]
- double longitude;
-
- //! When false, means that the GPS unit can't make a valid measurement
- //! (so all data other than the timestamp is meaningless).
- bool isValid;
-
- //! Heading/track/course with respect to true North [rad]
- double headingTrue;
- //! Heading/track/course with respect to magnetic North [rad]
- double headingMagnetic;
- //! Horizontal velocity [metres/second]
- double speed;
-};
-std::string toString( const RmcData &d );
-inline std::ostream &operator<<( std::ostream &s, const RmcData &d )
-{ return s << toString(d); }
-
-
-
/*!
Garmin GPS driver.
@@ -260,10 +66,14 @@
This standard dictates a transfer rate of 4800 baud.
This driver can read only the following messages (sentences):
-- GPGGA fix data
-- PGRME (estimated error) - not sent if set to 0183 1.5 (garmin-specific)
-- GPVTG vector track and speed over ground
-- GPRMC
+- GPGGA: fix data
+- PGRME: (estimated error) - not sent if set to 0183 1.5 (garmin-specific)
+- GPVTG: vector track and speed over ground
+- GPRMC: known as the "Recommended Minimum" sentence, is the most
+ common sentence transmitted by GPS devices. This one sentence
+ contains nearly everything a GPS application needs: latitude,
+ longitude, speed, bearing, satellite-derived time, fix status
+ and magnetic variation.
Processing of individual messages can be disabled in the Config structure.
@@ -284,9 +94,10 @@
//! gbxutilacfr::Tracer and gbxutilacfr::Status allow
//! (human-readable and machine-readable respectively) external
//! monitorining of the driver's internal state.
- Driver( const Config& config,
- gbxutilacfr::Tracer& tracer,
- gbxutilacfr::Status& status );
+ Driver( const Config &config,
+ gbxutilacfr::Tracer &tracer,
+ gbxutilacfr::Status &status,
+ int serialDebugLevel = 0 );
~Driver();
Deleted: gearbox/trunk/src/gbxgarminacfr/nmea.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmea.cpp 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/nmea.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -1,254 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2004-2008 Mathew Ridley, Alex Brooks, Alexei Makarenko, Tobias Kaupp
- *
- * This distribution is licensed to you under the terms described in
- * the LICENSE file included in this distribution.
- *
- */
-
-#include <stdio.h>
-#include <string>
-#include <iostream>
-#include <assert.h>
-#include <sstream>
-#include <gbxutilacfr/tokenise.h>
-
-// //////////////////////////////
-
-// // Ensure we have strnlen
-// // eg. Solaris doesn't define strnlen in string.h, so define it here.
-// #if !HAVE_STRNLEN
-
-// #include <cstring>
-
-// // inline the fucker to guard against multiple inclusion, without the
-// // hassle of a special lib.
-// inline size_t strnlen(const char *s, size_t maxlen)
-// {
-// char *p;
-// if (s == NULL) {
-// return maxlen;
-// }
-// p = (char *)memchr(s, 0, maxlen);
-// if (p == NULL) {
-// return maxlen;
-// }
-// return ((p - s) + 1);
-// }
-// #endif
-
-// //////////////////////////////
-
-#include "nmea.h"
-
-using namespace std;
-using namespace gbxgpsutilacfr;
-
-const char NMEAStartOfSentence = '$';
-const char NMEAChecksumDelim = '*';
-
-
-//The blank constructor
-NmeaMessage::NmeaMessage()
-{
- init();
-}
-
-void NmeaMessage::init()
-{
- haveCheckSum_ = false;
- checkSumOK_ = false;
-
- // Now clear the internal data store
- sentence_.clear();
- dataTokens_.clear();
-}
-
-NmeaMessage::NmeaMessage(const std::string &sentence, NmeaMessageOptions addOrTestCheckSum)
-{
- init();
- setSentence(sentence,addOrTestCheckSum);
-}
-
-
-//Load the data as requested and test the checksum if we are asked to.
-void
-NmeaMessage::setSentence(const std::string &data, NmeaMessageOptions addOrTestCheckSum)
-{
- init();
-
- sentence_ = data;
-
- switch ( addOrTestCheckSum )
- {
- case TestChecksum: {
- // This is for Rx'd data that we need to test for correct reception
- // (internally it will also call addCheckSum())
- testChecksumOk();
- break;
- }
- case AddChecksum: {
- // This is for Tx data that needs to checksummed before sending
- addCheckSum();
- checkSumOK_ = true;
- break;
- }
- case DontTestOrAddChecksum:
- break;
- default:
- assert( false && "unrecognized message option" );
- }
-}
-
-bool
-NmeaMessage::testChecksumOk()
-{
- haveCheckSum_ = true;
- checkSumOK_ = false;
-
- //First save the existing two checksum chars from the message
- //These are straight after the '*' character
- const size_t starPos = sentence_.find( NMEAChecksumDelim );
- if ( starPos == std::string::npos )
- {
- // cout<<"device: no checksum delimiter"<<endl;
- return false;
- }
-
- if ( starPos+2 >= sentence_.size() )
- {
- // cout<<"device: no checksum after delimiter"<<endl;
- return false;
- }
-
- //save the high and low bytes of the checksum
- //Make sure they are in upper case!
- const int checksumPos = starPos+1;
- const char chksum_HIB = (char)toupper(sentence_[checksumPos]);
- const char chksum_LOB = (char)toupper(sentence_[checksumPos+1]);
-
- //invalidate the existing checksum
- sentence_[checksumPos] = 'x';
- sentence_[checksumPos+1] = 'x';
-
- //Re-calculate our own copy of the checksum
- addCheckSum();
-
- //Now compare our saved version with our new ones
- if( (chksum_HIB == sentence_[checksumPos]) && (chksum_LOB == sentence_[checksumPos+1]) ) {
- //all looked good!
- checkSumOK_ = true;
- return true;
- }
-
- //failed the checksum!
-// cout<<"device: '"<<chksum_HIB<<chksum_LOB <<"' ("<<std::hex<<(unsigned int)chksum_HIB<<","<<(unsigned int)chksum_LOB<<std::dec<<") "
-// <<"driver: '"<<*ptr<<*(ptr+1)<<"'" <<"' ("<<std::hex<<(unsigned int)*ptr<<","<<(unsigned int)*(ptr+1)<<std::dec<<") "<<endl;
- return false;
-}
-
-// Add the checksum chars to an existing message
-// NOTE: this assumes that there is allready space in the message for
-// the checksum, and that the checksum delimiter is there
-void
-NmeaMessage::addCheckSum()
-{
- assert( haveSentence() && "calling addCheckSum() without a sentence" );
-
- haveCheckSum_ = true;
-
- //check that we have the '$' at the start
- if ( sentence_[0]!= NMEAStartOfSentence ) {
- throw NmeaException("cannot calculate checksum, missing leading '$'");
- }
-
- unsigned char chkRunning = 0;
-
- // we start from 1 to skip the leading '$'
- int loopCount;
- for ( loopCount = 1; loopCount < (int)(sentence_.size()); loopCount++ )
- {
- unsigned char nextChar = static_cast<unsigned char>(sentence_[loopCount]);
-
- // no delimiter uh oh
- if( (nextChar=='\r') || (nextChar=='\n') || (nextChar=='\0') ) {
- throw NmeaException("cannot calculate checksum, missing final '*'");
- }
-
- // goodie we found it (the '*' is not included into the checksum)
- if ( nextChar==NMEAChecksumDelim ) {
- break;
- }
-
- // alexm: gcc-4.3 is very strict with the next line.
- // not sure what to do about it yet.
- // http://gcc.gnu.org/ml/gcc/2008-05/msg00363.html
-
- // Keep the running XOR total
- chkRunning ^= nextChar;
- }
-
- if ( loopCount+2 >= (int)(sentence_.size()) )
- {
- throw NmeaException("addCheckSum(): no space for checksum of '*' not found.");
- }
-
- //Put the byte values as upper case HEX back into the message
- sprintf( &(sentence_[loopCount + 1]),"%02X", chkRunning );
-}
-
-// Parse the data fields of our message...
-void
-NmeaMessage::parseTokens()
-{
- //We should not attempt to be parsing a message twice...
- assert( numDataTokens()==0 && "calling parseTokens() with tokens" );
-
- //Split the message at the commas
- //TODO cope with missing fields
- dataTokens_ = gbxutilacfr::tokenise(sentence_, ",");
-
- //Now discard the $ and the * from the first and last tokens...
- //TODO : - dataTokens_[0] =
-
- //keep track of what we have done.
- haveTokens_ = true;
-}
-
-bool
-NmeaMessage::isDataTokenEmpty(int i) const
-{
- if ( i >= (int)(dataTokens_.size()) )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but only " << dataTokens_.size() << " exist in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- return dataTokens_[i].empty();
-}
-
-const std::string &
-NmeaMessage::getDataToken(int i) const
-{
- if ( i >= (int)(dataTokens_.size()) )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but only " << dataTokens_.size() << " exist in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- if ( dataTokens_[i].empty() )
- {
- stringstream ss;
- ss << "NmeaMessage::" << __func__
- << ": attempt to getDataToken("<<i<<") but this token is empty in sentence: "
- << sentence_;
- throw NmeaException( ss.str() );
- }
- return dataTokens_[i];
-}
Deleted: gearbox/trunk/src/gbxgarminacfr/nmea.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmea.h 2010-03-17 06:19:56 UTC (rev 487)
+++ gearbox/trunk/src/gbxgarminacfr/nmea.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -1,136 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, Duncan Mercer
- *
- * This distribution is licensed to you under the terms described in
- * the LICENSE file included in this distribution.
- *
- */
-
-#ifndef GBXGPSUTILACFR_NMEA_H
-#define GBXGPSUTILACFR_NMEA_H
-
-#include <vector>
-#include <string>
-
-/*
-
-for further info:
-
-http://www.kh-gps.de/nmea-faq.htm
-http://vancouver-webpages.com/peter/nmeafaq.txt
-
-NMEA-0183 sentence
-
-$aaccc,c--c*hh<CR><LF>
-|| || || |
-|| || || \________ <CR><LF> - End of sentence (0xOD 0xOA)
-|| || |\__________ hh - Checksum field hexadecimal [optional]
-|| || \___________ * - Checksum delimiter (0x2A) [optional]
-|| |\_______________ c--c - Data sentence block
-|| \________________ , - Field delimiter (0x2c)
-|\_____________________ aaccc - Address field/Command
-\______________________ $ - Start of sentence
-
- The optional checksum field consists of a "*" and two hex digits
- representing the exclusive OR of all characters between, but not
- including, the "$" and "*". A checksum is required on some
- sentences.
-
-*/
-
-namespace gbxgpsutilacfr {
-
-
-// class SOEXPORT NmeaException : public std::exception
-class NmeaException : public std::exception
-{
-public:
- NmeaException(const char *message)
- : message_(message) {}
-
- NmeaException(const std::string &message)
- : message_(message) {}
-
- virtual ~NmeaException() throw() {}
-
- virtual const char* what() const throw() { return message_.c_str(); }
-
-protected:
- std::string message_;
-};
-
-
-#define MAX_SENTENCE_LEN 256
-
-// When using class to send data, need to add checksum, when reciving data need to test checksum
-// Checksums are usually optional
-enum NmeaMessageOptions
-{
- TestChecksum,
- AddChecksum,
- DontTestOrAddChecksum
-};
-
-// class SOEXPORT NmeaMessage{
-class NmeaMessage
-{
-public:
- NmeaMessage();
- NmeaMessage(const std::string &sentence, NmeaMessageOptions addOrTestCheckSum=DontTestOrAddChecksum );
-
- // Set up the internal data for a sentence.
- // May throw NmeaException if TestChecksum is specified.
- void setSentence(const std::string &data, NmeaMessageOptions addOrTestCheckSum=DontTestOrAddChecksum );
-
- // Do we have the raw string?
- bool haveSentence() const { return !sentence_.empty(); };
-
- // Do we have parsed fields?
- bool haveTokens() const { return haveTokens_; };
-
- // Do we have a valid checksum?
- bool haveValidChecksum() const { return checkSumOK_; };
-
- // Have we checked the checksum?
- bool haveTestedChecksum()const { return haveCheckSum_; };
-
- // calculate the checksum from sentence
- // May throw NmeaException.
- bool testChecksumOk();
-
- // Return the raw sentence string
- const std::string &sentence() const { return sentence_; };
-
- // Return a single data token as a string.
- // Throws an exception if that token is empty (see 'isDataTokenEmpty()')
- const std::string &getDataToken(int i) const;
-
- bool isDataTokenEmpty(int i) const;
-
- // Return the number of fields
- int numDataTokens() const { return dataTokens_.size(); };
-
- //Tokenise the string that we received
- void parseTokens();
-
-private:
- void init();
- // May throw NmeaException.
- void addCheckSum();
- // Have we parsed data into tokens ?
- bool haveTokens_;
- // Have we a checksum and is it valid?
- bool haveCheckSum_;
- bool checkSumOK_;
- // The raw sentence, allow for terminator
-// char sentence_[MAX_SENTENCE_LEN+1];
- std::string sentence_;
- // The tokenised data
- std::vector<std::string> dataTokens_;
-};
-
-}
-
-#endif
Added: gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeamessages.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,236 @@
+#include "nmeamessages.h"
+#include <iostream>
+#include "nmeasentence.h"
+#include <memory>
+#include <cstdlib>
+#include <gbxutilacfr/exceptions.h>
+#include <gbxutilacfr/mathdefs.h>
+
+using namespace std;
+
+namespace gbxgarminacfr {
+
+// Get the useful bits from a GGA message
+GenericData* extractGgaData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<GgaData> data( new GgaData );
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the tokens in the GGA message
+ enum GgaTokens{MsgType=0,UTC,Lat,LatDir,Lon,LonDir,FixType,
+ NSatsUsed,HDOP,Hgt,M1,GeoidHgt,M2,DiffAge,DiffId};
+
+ //UTC time
+ sscanf(sentence.getDataToken(UTC).c_str(),"%02d%02d%lf",
+ &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
+
+ //number of satellites in use
+ data->satellites = atoi(sentence.getDataToken(NSatsUsed).c_str());
+
+ // fix type
+ switch ( sentence.getDataToken(FixType)[0] )
+ {
+ case '0':
+ data->fixType = Invalid;
+ // NOTE: not processing the rest!
+ data->latitude = 0.0;
+ data->longitude = 0.0;
+ data->altitude = 0.0;
+ data->geoidalSeparation = 0.0;
+ return data.release();
+ case '1':
+ data->fixType = Autonomous;
+ break;
+ case '2':
+ data->fixType = Differential;
+ break;
+ default :
+ throw gbxutilacfr::Exception( ERROR_INFO, "GGA sentence contains unknown GPS fix type: '"+sentence.getDataToken(FixType)+"'" );
+ }
+
+ //position
+ int deg;
+ double min;
+ double dir;
+
+ //latitude
+ sscanf(sentence.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
+ dir = (*sentence.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
+ data->latitude=dir*(deg+(min/60.0));
+ //longitude
+ sscanf(sentence.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
+ dir = (*sentence.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
+ data->longitude=dir*(deg+(min/60.0));
+
+ //altitude
+ data->isAltitudeKnown = !sentence.isDataTokenEmpty(Hgt);
+ if ( data->isAltitudeKnown )
+ data->altitude=atof(sentence.getDataToken(Hgt).c_str());
+
+ //geoidal Separation
+ data->geoidalSeparation=atof(sentence.getDataToken(GeoidHgt).c_str());
+
+ return data.release();
+}
+
+// VTG provides velocity and heading information
+GenericData* extractVtgData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<VtgData> data( new VtgData );
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the VTG message items
+ enum VtgTokens{MsgType=0,HeadingTrue,T,HeadingMag,M,SpeedKnots,
+ N,SpeedKPH,K,ModeInd};
+
+ //Check for an empty string. Means that we can't tell anything useful.
+ if ( sentence.isDataTokenEmpty(HeadingTrue) )
+ {
+ data->isValid = false;
+ data->headingTrue = 0.0;
+ data->headingMagnetic = 0.0;
+ data->speed = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ // true heading
+ double headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingTrue).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingTrue=headingRad;
+
+ // magnetic heading
+ headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingMag).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingMagnetic=headingRad;
+
+ //speed - converted to m/s
+ data->speed=atof(sentence.getDataToken(SpeedKPH).c_str());
+ data->speed*=(1000/3600.0);
+
+ return data.release();
+}
+
+// RME message. This one is garmin specific... Give position error estimates
+// See doc.dox for a discussion of the position errors as reported here.
+// Essentially the EPE reported by the garmin is a 1 sigma error (RMS) or a
+// 68% confidence bounds.
+GenericData* extractRmeData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<RmeData> data( new RmeData );
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the RME message items
+ enum RmeTokens{MsgType=0,HError,M1,VError,M2,EPE,M3};
+
+ if ( sentence.isDataTokenEmpty(HError) )
+ {
+ // No valid information
+ data->isValid = false;
+ data->isVerticalPositionErrorValid = false;
+ data->horizontalPositionError = 0.0;
+ data->verticalPositionError = 0.0;
+ data->estimatedPositionError = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ data->horizontalPositionError = atof(sentence.getDataToken(HError).c_str());
+
+ if ( sentence.isDataTokenEmpty(VError) )
+ {
+ data->isVerticalPositionErrorValid = false;
+ data->verticalPositionError = -1;
+ }
+ else
+ {
+ data->isVerticalPositionErrorValid = true;
+ data->verticalPositionError = atof(sentence.getDataToken(VError).c_str());
+ }
+
+ data->estimatedPositionError = atof(sentence.getDataToken(EPE).c_str());
+ return data.release();
+}
+
+// RMC provides a combination of GGA and VTG data
+GenericData* extractRmcData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec )
+{
+ std::auto_ptr<RmcData> data( new RmcData );
+
+ data->timeStampSec = timeSec;
+ data->timeStampUsec = timeUsec;
+
+ //Names for the RMC message items
+ enum RmcTokens{MsgType=0,UTC,Status,Lat,LatDir,Lon,LonDir,SpeedKnots,
+ HeadingTrue,DiffAge,MagneticVar,MagneticDir,ModeInd};
+
+ //Check for an empty string. Means that we can't tell anything useful.
+ if ( sentence.isDataTokenEmpty(HeadingTrue) )
+ {
+ data->isValid = false;
+ data->headingTrue = 0.0;
+ data->headingMagnetic = 0.0;
+ data->speed = 0.0;
+ return data.release();
+ }
+ data->isValid = true;
+
+ //UTC time
+ sscanf(sentence.getDataToken(UTC).c_str(),"%02d%02d%lf",
+ &data->utcTimeHrs, &data->utcTimeMin, &data->utcTimeSec );
+
+ //position
+ int deg;
+ double min;
+ double dir;
+
+ //latitude
+ sscanf(sentence.getDataToken(Lat).c_str(),"%02d%lf",°,&min);
+ dir = (*sentence.getDataToken(LatDir).c_str()=='N') ? 1.0 : -1.0;
+ data->latitude=dir*(deg+(min/60.0));
+ //longitude
+ sscanf(sentence.getDataToken(Lon).c_str(),"%03d%lf",°,&min);
+ dir = (*sentence.getDataToken(LonDir).c_str()=='E') ? 1.0 : -1.0;
+ data->longitude=dir*(deg+(min/60.0));
+
+ // true heading
+ double headingRad = DEG2RAD(atof(sentence.getDataToken(HeadingTrue).c_str()));
+ NORMALISE_ANGLE( headingRad );
+ data->headingTrue=headingRad;
+
+ // magnetic heading
+ double headingVar;
+ try {
+ // Magnetic deviation from true
+ headingVar = DEG2RAD(atof(sentence.getDataToken(MagneticVar).c_str()));
+ // Direction of magnetic deviation
+ if (*sentence.getDataToken(MagneticDir).c_str() == 'E')
+ headingRad -= headingVar;
+ else
+ headingRad += headingVar;
+ }
+ catch ( const gbxgpsutilacfr::NmeaException& e ) {
+ // If this occurs, cannot get magnetic heading. Set it to 0.
+ headingRad = 0.0;
+ }
+ NORMALISE_ANGLE( headingRad );
+ data->headingMagnetic=headingRad;
+
+ //speed - converted from knots to m/s
+ data->speed=atof(sentence.getDataToken(SpeedKnots).c_str());
+ // knots to kph
+ data->speed*=1.852;
+ // kph to m/s
+ data->speed*=(1000/3600.0);
+
+ return data.release();
+}
+
+}
+
Added: gearbox/trunk/src/gbxgarminacfr/nmeamessages.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeamessages.h (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeamessages.h 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,216 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp, Duncan Mercer
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+#ifndef GBXGPSUTILACFR_NMEAMESSAGES_H
+#define GBXGPSUTILACFR_NMEAMESSAGES_H
+
+#include <string>
+#include <gbxgarminacfr/nmeasentence.h>
+
+namespace gbxgarminacfr {
+
+//! Possible types GenericData can contain
+enum DataType
+{
+ //! Contents of PGGGA message.
+ GpGga,
+ //! Contents of PGVTG message.
+ GpVtg,
+ //! Contents of PGRME message.
+ PgRme,
+ //! Contents of GPRMC message.
+ GpRmc
+};
+
+//! Generic data type returned by a read
+class GenericData
+{
+public:
+ virtual ~GenericData() {};
+ //! Returns data type.
+ virtual DataType type() const=0;
+
+private:
+};
+
+//! GPS fix types.
+enum FixType
+{
+ //! Invalid or not available
+ Invalid,
+ //! Autonomous position
+ //! (This is the normal case for non-differential GPS)
+ Autonomous,
+ //! Differentially corrected
+ Differential
+};
+std::string toString( const FixType &f );
+
+//! Fix data structure. Note that when fixType is Invalid, all other data except the time stamps
+//! are meaningless.
+struct GgaData : public GenericData
+{
+public:
+ DataType type() const { return GpGga; }
+
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Hour [0..23]
+ int utcTimeHrs;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Minutes [0..59]
+ int utcTimeMin;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Seconds [0.0..59.9999(9)]
+ double utcTimeSec;
+
+ //! Latitude [degrees]
+ double latitude;
+ //! Longitude [degrees]
+ double longitude;
+ //! Altitude is meaningful if and only if isAltitudeKnown
+ bool isAltitudeKnown;
+ //! Altitude [metres above ellipsoid] (only meaningful if isAltitudeKnown)
+ double altitude;
+
+ //! Fix type. When fixType is Invalid, all other data except the time stamps
+ //! are meaningless.
+ FixType fixType;
+
+ //! Number of satellites
+ int satellites;
+
+ //! Horizontal dilution of position [metres]
+ double horizontalDilutionOfPosition;
+
+ //! Height of geoid (mean sea level) above WGS84 ellipsoid [metres]
+ double geoidalSeparation;
+};
+std::string toString( const GgaData &d );
+inline std::ostream &operator<<( std::ostream &s, const GgaData &d )
+{ return s << toString(d); }
+GenericData* extractGgaData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Vector track and speed over ground data structure.
+class VtgData : public GenericData
+{
+public:
+ DataType type() const { return GpVtg; }
+
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! Heading/track/course with respect to true North [rad]
+ double headingTrue;
+ //! Heading/track/course with respect to magnetic North [rad]
+ double headingMagnetic;
+ //! Horizontal velocity [metres/second]
+ double speed;
+};
+std::string toString( const VtgData &d );
+inline std::ostream &operator<<( std::ostream &s, const VtgData &d )
+{ return s << toString(d); }
+GenericData* extractVtgData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Gps data structure
+//! (This one is Garmin-specific)
+class RmeData : public GenericData
+{
+public:
+ DataType type() const { return PgRme; }
+
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! When false, means that the GPS unit can't tell us anything
+ //! about our vertical error
+ bool isVerticalPositionErrorValid;
+
+ //! Horizontal position error: one standard deviation [metres)]
+ double horizontalPositionError;
+ //! Vertical position error: one standard deviation [metres]
+ double verticalPositionError;
+
+ //! Estimated position error.
+ double estimatedPositionError;
+};
+std::string toString( const RmeData &d );
+inline std::ostream &operator<<( std::ostream &s, const RmeData &d )
+{ return s << toString(d); }
+GenericData* extractRmeData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+//! Gps data structure
+class RmcData : public GenericData
+{
+public:
+ DataType type() const { return GpRmc; }
+
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of seconds
+ int timeStampSec;
+ //! Time (according to the computer clock) when data was measured.
+ //! Number of microseconds
+ int timeStampUsec;
+
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Hour [0..23]
+ int utcTimeHrs;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Minutes [0..59]
+ int utcTimeMin;
+ //! UTC time (according to the GPS device), reference is Greenwich.
+ //! Seconds [0.0..59.9999(9)]
+ double utcTimeSec;
+
+ //! Latitude [degrees]
+ double latitude;
+ //! Longitude [degrees]
+ double longitude;
+
+ //! When false, means that the GPS unit can't make a valid measurement
+ //! (so all data other than the timestamp is meaningless).
+ bool isValid;
+
+ //! Heading/track/course with respect to true North [rad]
+ double headingTrue;
+ //! Heading/track/course with respect to magnetic North [rad]
+ double headingMagnetic;
+ //! Horizontal velocity [metres/second]
+ double speed;
+};
+std::string toString( const RmcData &d );
+inline std::ostream &operator<<( std::ostream &s, const RmcData &d )
+{ return s << toString(d); }
+GenericData* extractRmcData( const gbxgpsutilacfr::NmeaSentence& sentence, int timeSec, int timeUsec );
+
+}
+
+#endif
Added: gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp (rev 0)
+++ gearbox/trunk/src/gbxgarminacfr/nmeasentence.cpp 2010-03-18 07:54:14 UTC (rev 488)
@@ -0,0 +1,254 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Mathew Ridley, Alex Brooks, Alexei Makarenko, Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <stdio.h>
+#include <string>
+#include <iostream>
+#include <assert.h>
+#include <sstream>
+#include <gbxutilacfr/tokenise.h>
+
+// //////////////////////////////
+
+// // Ensure we have strnlen
+// // eg. Solaris doesn't define strnlen in string.h, so define it here.
+// #if !HAVE_STRNLEN
+
+// #include <cstring>
+
+// // inline the fucker to guard against multiple inclusion, without the
+// // hassle of a special lib.
+// inline size_t strnlen(const char *s, size_t maxlen)
+// {
+// char *p;
+// if (s == NULL) {
+// return maxlen;
+// }
+// p = (char *)memchr(s, 0, maxlen);
+// if (p == NULL) {
+// return maxlen;
+// }
+// return ((p - s) + 1);
+// }
+// #endif
+
+// //////////////////////////////
+
+#include "nmeasentence.h"
+
+using namespace std;
+using namespace gbxgpsutilacfr;
+
+const char NMEAStartOfSentence = '$';
+const char NMEAChecksumDelim = '*';
+
+
+//The blank constructor
+NmeaSentence::NmeaSentence()
+{
+ init();
+}
+
+void NmeaSentence::init()
+{
+ haveCheckSum_ = false;
+ checkSumOK_ = false;
+
+ // Now clear the internal data store
+ sentence_.clear();
+ dataTokens_.clear();
+}
+
+NmeaSentence::NmeaSentence(const std::string &sentence, NmeaSentenceOptions addOrTestCheckSum)
+{
+ init();
+ setSentence(sentence,addOrTestCheckSum);
+}
+
+
+//Load the data as requested and test the checksum if we are asked to.
+void
+NmeaSentence::setSentence(const std::string &data, NmeaSentenceOptions addOrTestCheckSum)
+{
+ init();
+
+ sentence_ = data;
+
+ switch ( addOrTestCheckSum )
+ {
+ case TestChecksum: {
+ // This is for Rx'd data that we need to test for correct reception
+ // (internally it will also call addCheckSum())
+ testChecksumOk();
+ break;
+ }
+ case AddChecksum: {
+ // This is for Tx data that needs to checksummed before sending
+ addCheckSum();
+ checkSumOK_ = true;
+ break;
+ }
+ case DontTestOrAddChecksum:
+ break;
+ default:
+ assert( false && "unrecognized message option" );
+ }
+}
+
+bool
+NmeaSentence::testChecksumOk()
+{
+ haveCheckSum_ = true;
+ checkSumOK_ = false;
+
+ //First save the existing two checksum chars from the message
+ //These are straight after the '*' character
+ const size_t starPos = sentence_.find( NMEAChecksumDelim );
+ if ( starPos == std::string::npos )
+ {
+ // cout<<"device: no checksum delimiter"<<endl;
+ return false;
+ }
+
+ if ( starPos+2 >= sentence_.size() )
+ {
+ // cout<<"device: no checksum after delimiter"<<endl;
+ return false;
+ }
+
+ //save the high and low bytes of the checksum
+ //Make sure they are in upper case!
+ const int checksumPos = starPos+1;
+ const char chksum_HIB = (char)toupper(sentence_[checksumPos]);
+ const char chksum_LOB = (char)toupper(sentence_[checksumPos+1]);
+
+ //invalidate the existing checksum
+ sentence_[checksumPos] = 'x';
+ sentence_[checksumPos+1] = 'x';
+
+ //Re-calculate our own copy of the checksum
+ addCheckSum();
+
+ //Now compare our saved version with our new ones
+ if( (chksum_HIB == sentence_[checksumPos]) && (chksum_LOB == sentence_[checksumPos+1]) ) {
+ //all looked good!
+ checkSumOK_ = true;
+ return true;
+ }
+
+ //failed the checksum!
+// cout<<"device: '"<<chksum_HIB<<chksum_LOB <<"' ("<<std::hex<<(unsigned int)chksum_HIB<<","<<(unsigned int)chksum_LOB<<std::dec<<") "
+// <<"driver: '"<<*ptr<<*(ptr+1)<<"'" <<"' ("<<std::hex<<(unsigned int)*ptr<<","<<(unsigned int)*(ptr+1)<<std::dec<<") "<<endl;
+ return false;
+}
+
+// Add the checksum chars to an existing message
+// NOTE: this assumes that there is allready space in the message for
+// the checksum, and that the checksum delimiter is there
+void
+NmeaSentence::addCheckSum()
+{
+ assert( haveSentence() && "calling addCheckSum() without a sentence" );
+
+ haveCheckSum_ = true;
+
+ //check that we have the '$' at the start
+ if ( sentence_[0]!= NMEAStartOfSentence ) {
+ throw NmeaException("cannot calculate checksum, missing leading '$'");
+ }
+
+ unsigned char chkRunning = 0;
+
+ // we start from 1 to skip the leading '$'
+ int loopCount;
+ for ( loopCount = 1; loopCount < (int)(sentence_.size()); loopCount++ )
+ {
+ un...
[truncated message content] |
|
From: <bo...@us...> - 2010-03-17 06:20:05
|
Revision: 487
http://gearbox.svn.sourceforge.net/gearbox/?rev=487&view=rev
Author: borax00
Date: 2010-03-17 06:19:56 +0000 (Wed, 17 Mar 2010)
Log Message:
-----------
Applied patch suggested by Ben Kehoe
Modified Paths:
--------------
gearbox/trunk/src/gbxutilacfr/exceptions.cpp
Modified: gearbox/trunk/src/gbxutilacfr/exceptions.cpp
===================================================================
--- gearbox/trunk/src/gbxutilacfr/exceptions.cpp 2010-02-23 23:20:33 UTC (rev 486)
+++ gearbox/trunk/src/gbxutilacfr/exceptions.cpp 2010-03-17 06:19:56 UTC (rev 487)
@@ -28,9 +28,9 @@
Exception::basename( const char *s ) const
{
#ifndef WIN32
- char *slashPos = strrchr( s, '/' );
+ const char *slashPos = strrchr( s, '/' );
#else
- char *slashPos = strrchr( s, '\\' );
+ const char *slashPos = strrchr( s, '\\' );
#endif
if ( slashPos == NULL )
return s; // no slash found
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-02-23 23:20:42
|
Revision: 486
http://gearbox.svn.sourceforge.net/gearbox/?rev=486&view=rev
Author: borax00
Date: 2010-02-23 23:20:33 +0000 (Tue, 23 Feb 2010)
Log Message:
-----------
Fix for bug reported by David Lobato
Modified Paths:
--------------
gearbox/trunk/src/gbxutilacfr/exceptions.cpp
Modified: gearbox/trunk/src/gbxutilacfr/exceptions.cpp
===================================================================
--- gearbox/trunk/src/gbxutilacfr/exceptions.cpp 2010-02-05 08:04:39 UTC (rev 485)
+++ gearbox/trunk/src/gbxutilacfr/exceptions.cpp 2010-02-23 23:20:33 UTC (rev 486)
@@ -28,10 +28,14 @@
Exception::basename( const char *s ) const
{
#ifndef WIN32
- return strrchr( s, '/' )+1;
+ char *slashPos = strrchr( s, '/' );
#else
- return strrchr( s, '\\' )+1;
+ char *slashPos = strrchr( s, '\\' );
#endif
+ if ( slashPos == NULL )
+ return s; // no slash found
+ else
+ return slashPos+1;
};
std::string
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-02-05 08:04:48
|
Revision: 485
http://gearbox.svn.sourceforge.net/gearbox/?rev=485&view=rev
Author: borax00
Date: 2010-02-05 08:04:39 +0000 (Fri, 05 Feb 2010)
Log Message:
-----------
Fixed uninitialised readRmc
Modified Paths:
--------------
gearbox/trunk/src/gbxgarminacfr/driver.h
Modified: gearbox/trunk/src/gbxgarminacfr/driver.h
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/driver.h 2010-02-01 03:55:11 UTC (rev 484)
+++ gearbox/trunk/src/gbxgarminacfr/driver.h 2010-02-05 08:04:39 UTC (rev 485)
@@ -26,6 +26,7 @@
readGga(true),
readVtg(true),
readRme(true),
+ readRmc(false),
ignoreUnknown(false) {};
//! Returns true if the configuration is sane. Checks include:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2010-02-01 03:55:22
|
Revision: 484
http://gearbox.svn.sourceforge.net/gearbox/?rev=484&view=rev
Author: borax00
Date: 2010-02-01 03:55:11 +0000 (Mon, 01 Feb 2010)
Log Message:
-----------
MR Changes
Modified Paths:
--------------
gearbox/trunk/cmake/DependencyUtils.cmake
gearbox/trunk/cmake/TargetUtils.cmake
gearbox/trunk/cmake/internal/gearbox-config-internal.cmake
gearbox/trunk/doc/history.dox
gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp
Modified: gearbox/trunk/cmake/DependencyUtils.cmake
===================================================================
--- gearbox/trunk/cmake/DependencyUtils.cmake 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/cmake/DependencyUtils.cmake 2010-02-01 03:55:11 UTC (rev 484)
@@ -149,26 +149,22 @@
if( ${cumulative_var} )
GBX_UTIL_CHECK_MODULE_TYPE( ${module_type} is_exe is_lib )
-
+
if( ${ARGC} GREATER 5 )
set( reason ${ARGV6} )
else( ${ARGC} GREATER 5 )
set( reason "${target_name} is not being built" )
endif( ${ARGC} GREATER 5 )
-
- # TODO: should simply use this signature: if(NOT TARGET target_name)
- GET_TARGET_PROPERTY( target_location ${target_name} LOCATION )
-
- # must dereference both var and option names once (!) and IF will evaluate their values
- if( NOT target_location )
+
+ if( NOT TARGET ${target_name} )
set( ${cumulative_var} FALSE )
GBX_UTIL_MAKE_OPTION_NAME( option_name ${module_type} ${module_name} )
if( is_exe )
GBX_NOT_ADD_EXECUTABLE( ${module_name} ${reason} )
else( is_exe )
- GBX_NOT_add_library( ${module_name} ${reason} )
+ GBX_NOT_ADD_LIBRARY( ${module_name} ${reason} )
endif( is_exe )
- endif( NOT target_location )
+ endif( NOT TARGET ${target_name} )
endif( ${cumulative_var} )
Modified: gearbox/trunk/cmake/TargetUtils.cmake
===================================================================
--- gearbox/trunk/cmake/TargetUtils.cmake 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/cmake/TargetUtils.cmake 2010-02-01 03:55:11 UTC (rev 484)
@@ -108,9 +108,32 @@
set( libTypeDesc "static" )
endif( libType STREQUAL SHARED )
message( STATUS "Planning to build ${libTypeDesc} library: ${name}-${soversion}" )
-endmacro( GBX_ADD_LIBRARY name )
+endmacro( GBX_ADD_LIBRARY )
#
+# Special macro for handling dependencies on header-only libraries.
+# Internally creates a dummy target which other libraries can depend on.
+# This is helpful if the header-only library has external dependencies.
+# The header-only library can check its dependencies once and all libraries
+# can use internal dependency checking mechanism from then on.
+#
+macro( GBX_ADD_HEADER_ONLY_LIBRARY name )
+ if( COMMAND cmake_policy )
+ cmake_policy( SET CMP0003 NEW )
+ endif( COMMAND cmake_policy )
+
+# message( STATUS "DEBUG: adding header-only library '${name}'" )
+
+ add_custom_target( ${name} )
+
+ set( templist ${LIB_LIST} )
+ list( APPEND templist ${name} )
+ set( LIB_LIST ${templist} CACHE INTERNAL "Global list of libraries to build" FORCE )
+
+ message( STATUS "Considering header-only library as 'built': ${name}" )
+endmacro( GBX_ADD_HEADER_ONLY_LIBRARY )
+
+#
# GBX_ADD_HEADERS( install_subdir FILE0 [FILE1 FILE2 ...] )
#
# Specialization of install(FILES ...) to install header files.
Modified: gearbox/trunk/cmake/internal/gearbox-config-internal.cmake
===================================================================
--- gearbox/trunk/cmake/internal/gearbox-config-internal.cmake 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/cmake/internal/gearbox-config-internal.cmake 2010-02-01 03:55:11 UTC (rev 484)
@@ -11,5 +11,7 @@
set( GEARBOX_USE_FILE "${GEARBOX_CMAKE_DIR}/gearbox-use-file.cmake" )
-# this is where Gearbox libs will be installed
-set( GEARBOX_LINK_DIR ${CMAKE_INSTALL_PREFIX}/${GBX_LIB_INSTALL_DIR} )
+# this is where Gearbox libs will be installed.
+# we cannot use GBX_LIB_INSTALL_DIR here because this script is loaded from another
+# project (it's only used under special super-project configuration)
+set( GEARBOX_LINK_DIR ${CMAKE_INSTALL_PREFIX}/lib/gearbox )
Modified: gearbox/trunk/doc/history.dox
===================================================================
--- gearbox/trunk/doc/history.dox 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/doc/history.dox 2010-02-01 03:55:11 UTC (rev 484)
@@ -21,6 +21,15 @@
Developers: as you make substantial changes in the code (add features, fix bugs, etc.), add an item in the section on changes. When the next version is released, this list will be copied into the release notes.
+POST 9.11 CHANGES
+
+@par Project wide
+
+- Build system
+ - GBX_REQUIRE_LIB macro now uses a direct check for target existence.
+ - new GBX_ADD_HEADER_ONLY_LIBRARY macro for handling dependencies on header-only libraries.
+
+
@section gbx_doc_history_todo To-Do List for Next Release
None.
Modified: gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp 2010-02-01 03:55:11 UTC (rev 484)
@@ -25,6 +25,13 @@
void removeParsedData( std::vector<char> &buffer,
int numBytesParsed )
{
+ if ( numBytesParsed > (int)(buffer.size()) )
+ {
+ stringstream ss;
+ ss << "Huh? numBytesParsed("<<numBytesParsed<<") is bigger than buffer.size()("<<buffer.size()<<")";
+ throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
+ }
+
if ( numBytesParsed > 0 )
{
int numBytesBeyond = buffer.size()-numBytesParsed;
@@ -32,6 +39,7 @@
{
memmove( &(buffer[0]), &(buffer[numBytesParsed]), numBytesBeyond*sizeof(char) );
}
+ assert( numBytesBeyond >= 0 );
buffer.resize( numBytesBeyond );
}
}
@@ -228,7 +236,6 @@
tracer_.warning( ss.str() );
throw;
}
-
removeParsedData( buffer_, numBytesParsed );
if ( gotMessage )
Modified: gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp
===================================================================
--- gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp 2010-01-28 02:49:29 UTC (rev 483)
+++ gearbox/trunk/src/gbxsmartbatteryacfr/test/longtest.cpp 2010-02-01 03:55:11 UTC (rev 484)
@@ -16,6 +16,17 @@
}
+string checkMinToEmpty( const gbxsmartbatteryacfr::OceanServerSystem &data )
+{
+ if ( data.minToEmpty()<1 )
+ {
+ stringstream ss;
+ ss << "*********** YES: MinToEmpty is close to zero: " << data.minToEmpty();
+ return ss.str();
+ }
+ else
+ return "NO";
+}
int main( int argc, char **argv )
{
@@ -47,7 +58,7 @@
try
{
gbxsmartbatteryacfr::BatteryHealthWarningConfig config;
- config.expectedNumBatteries = 2;
+ config.expectedNumBatteries = 1;
config.numCyclesThreshhold = 300;
config.chargeTempThreshhold = 40.0;
config.dischargeTempThreshhold = 45.0;
@@ -58,7 +69,8 @@
while (true)
{
- cout << "Reading record " << numRecords << endl;
+ cout << "========================================================" << endl
+ << "Reading record " << numRecords << endl;
numRecords++;
gbxsmartbatteryacfr::OceanServerSystem data = oceanserver.getData();
@@ -67,9 +79,16 @@
cout << "Data was empty. No worries, keep trying to read." << endl;
continue;
}
+
+ cout << gbxsmartbatteryacfr::toString( data ) << endl
+ << "minToEmpty<1?: " << checkMinToEmpty(data) << endl
+ << "isSystemOnCharge?: " << isSystemOnCharge(data) << endl
+ << "raw record: " << endl
+ << toString(data.rawRecord()) << endl;
+
vector<string> shortWarning;
vector<string> verboseWarning;
- const bool printRawRecord = true;
+ const bool printRawRecord = false;
bool haveWarnings = conductAllHealthChecks( data, config, shortWarning, verboseWarning, printRawRecord );
if (haveWarnings)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2010-01-28 04:03:23
|
Revision: 482
http://gearbox.svn.sourceforge.net/gearbox/?rev=482&view=rev
Author: gbiggs
Date: 2010-01-28 02:41:45 +0000 (Thu, 28 Jan 2010)
Log Message:
-----------
Updated licensing information
Modified Paths:
--------------
openrtm/trunk/flexiport/Flexiport.cpp
openrtm/trunk/flexiport/Flexiport.h
openrtm/trunk/flexiport/README.Flexiport
openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.cpp
openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.h
openrtm/trunk/gbxgarminacfr/README.GbxGarminAcfr
openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.cpp
openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.h
openrtm/trunk/gbxnovatelacfr/README.GbxNovatelAcfr
openrtm/trunk/gbxsickacfr/README.gbxsickacfr
openrtm/trunk/gbxsickacfr/gbxsickacfr.cpp
openrtm/trunk/gbxsickacfr/gbxsickacfr.h
openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.cpp
openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.h
openrtm/trunk/gbxsmartbatteryacfr/README.GbxSmartBatteryAcfr
openrtm/trunk/hokuyo_aist/README.hokuyo_aist
openrtm/trunk/hokuyo_aist/hokuyo_aist.cpp
openrtm/trunk/hokuyo_aist/hokuyo_aist.h
Added Paths:
-----------
openrtm/trunk/flexiport/LICENSE.txt
openrtm/trunk/gbxgarminacfr/LICENSE.txt
openrtm/trunk/gbxnovatelacfr/LICENSE.txt
openrtm/trunk/gbxsickacfr/LICENSE.txt
openrtm/trunk/gbxsmartbatteryacfr/LICENSE.txt
openrtm/trunk/hokuyo_aist/LICENSE.txt
Modified: openrtm/trunk/flexiport/Flexiport.cpp
===================================================================
--- openrtm/trunk/flexiport/Flexiport.cpp 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/flexiport/Flexiport.cpp 2010-01-28 02:41:45 UTC (rev 482)
@@ -4,6 +4,19 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
+
#include "Flexiport.h"
#include <flexiport/flexiport.h>
Modified: openrtm/trunk/flexiport/Flexiport.h
===================================================================
--- openrtm/trunk/flexiport/Flexiport.h 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/flexiport/Flexiport.h 2010-01-28 02:41:45 UTC (rev 482)
@@ -4,6 +4,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial Science and Technology,
+ * Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See LICENSE.TXT.
+ *
+ */
+
#ifndef FLEXIPORT_H
#define FLEXIPORT_H
Added: openrtm/trunk/flexiport/LICENSE.txt
===================================================================
--- openrtm/trunk/flexiport/LICENSE.txt (rev 0)
+++ openrtm/trunk/flexiport/LICENSE.txt 2010-01-28 02:41:45 UTC (rev 482)
@@ -0,0 +1,223 @@
+Copyright (C) 2009-2010
+ Geoffrey Biggs
+ RT-Synthesis Research Group
+ Intelligent Systems Research Institute,
+ National Institute of Advanced Industrial Science and Technology (AIST),
+ Japan
+ All rights reserved.
+Licensed under the Eclipse Public License -v 1.0 (EPL)
+http://www.opensource.org/licenses/eclipse-1.0.txt
+
+Eclipse Public License -v 1.0
+
+THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+
+1. DEFINITIONS
+
+"Contribution" means:
+
+a) in the case of the initial Contributor, the initial code and documentation
+distributed under this Agreement, and
+
+b) in the case of each subsequent Contributor:
+
+i) changes to the Program, and
+
+ii) additions to the Program;
+
+where such changes and/or additions to the Program originate from and are
+distributed by that particular Contributor. A Contribution 'originates' from a
+Contributor if it was added to the Program by such Contributor itself or anyone
+acting on such Contributor's behalf. Contributions do not include additions to
+the Program which: (i) are separate modules of software distributed in
+conjunction with the Program under their own license agreement, and (ii) are
+not derivative works of the Program.
+
+"Contributor" means any person or entity that distributes the Program.
+
+"Licensed Patents " mean patent claims licensable by a Contributor which are
+necessarily infringed by the use or sale of its Contribution alone or when
+combined with the Program.
+
+"Program" means the Contributions distributed in accordance with this
+Agreement.
+
+"Recipient" means anyone who receives the Program under this Agreement,
+including all Contributors.
+
+2. GRANT OF RIGHTS
+
+a) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free copyright license to
+reproduce, prepare derivative works of, publicly display, publicly perform,
+distribute and sublicense the Contribution of such Contributor, if any, and
+such derivative works, in source code and object code form.
+
+b) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free patent license under
+Licensed Patents to make, use, sell, offer to sell, import and otherwise
+transfer the Contribution of such Contributor, if any, in source code and
+object code form. This patent license shall apply to the combination of the
+Contribution and the Program if, at the time the Contribution is added by the
+Contributor, such addition of the Contribution causes such combination to be
+covered by the Licensed Patents. The patent license shall not apply to any
+other combinations which include the Contribution. No hardware per se is
+licensed hereunder.
+
+c) Recipient understands that although each Contributor grants the licenses to
+its Contributions set forth herein, no assurances are provided by any
+Contributor that the Program does not infringe the patent or other intellectual
+property rights of any other entity. Each Contributor disclaims any liability
+to Recipient for claims brought by any other entity based on infringement of
+intellectual property rights or otherwise. As a condition to exercising the
+rights and licenses granted hereunder, each Recipient hereby assumes sole
+responsibility to secure any other intellectual property rights needed, if any.
+For example, if a third party patent license is required to allow Recipient to
+distribute the Program, it is Recipient's responsibility to acquire that
+license before distributing the Program.
+
+d) Each Contributor represents that to its knowledge it has sufficient
+copyright rights in its Contribution, if any, to grant the copyright license
+set forth in this Agreement.
+
+3. REQUIREMENTS
+
+A Contributor may choose to distribute the Program in object code form under
+its own license agreement, provided that:
+
+a) it complies with the terms and conditions of this Agreement; and
+
+b) its license agreement:
+
+i) effectively disclaims on behalf of all Contributors all warranties and
+conditions, express and implied, including warranties or conditions of title
+and non-infringement, and implied warranties or conditions of merchantability
+and fitness for a particular purpose;
+
+ii) effectively excludes on behalf of all Contributors all liability for
+damages, including direct, indirect, special, incidental and consequential
+damages, such as lost profits;
+
+iii) states that any provisions which differ from this Agreement are offered by
+that Contributor alone and not by any other party; and
+
+iv) states that source code for the Program is available from such Contributor,
+and informs licensees how to obtain it in a reasonable manner on or through a
+medium customarily used for software exchange.
+
+When the Program is made available in source code form:
+
+a) it must be made available under this Agreement; and
+
+b) a copy of this Agreement must be included with each copy of the Program.
+
+Contributors may not remove or alter any copyright notices contained within the
+Program.
+
+Each Contributor must identify itself as the originator of its Contribution, if
+any, in a manner that reasonably allows subsequent Recipients to identify the
+originator of the Contribution.
+
+4. COMMERCIAL DISTRIBUTION
+
+Commercial distributors of software may accept certain responsibilities with
+respect to end users, business partners and the like. While this license is
+intended to facilitate the commercial use of the Program, the Contributor who
+includes the Program in a commercial product offering should do so in a manner
+which does not create potential liability for other Contributors. Therefore, if
+a Contributor includes the Program in a commercial product offering, such
+Contributor ("Commercial Contributor") hereby agrees to defend and indemnify
+every other Contributor ("Indemnified Contributor") against any losses, damages
+and costs (collectively "Losses") arising from claims, lawsuits and other legal
+actions brought by a third party against the Indemnified Contributor to the
+extent caused by the acts or omissions of such Commercial Contributor in
+connection with its distribution of the Program in a commercial product
+offering. The obligations in this section do not apply to any claims or Losses
+relating to any actual or alleged intellectual property infringement. In order
+to qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+Contributor in writing of such claim, and b) allow the Commercial Contributor
+to control, and cooperate with the Commercial Contributor in, the defense and
+any related settlement negotiations. The Indemnified Contributor may
+participate in any such claim at its own expense.
+
+For example, a Contributor might include the Program in a commercial product
+offering, Product X. That Contributor is then a Commercial Contributor. If that
+Commercial Contributor then makes performance claims, or offers warranties
+related to Product X, those performance claims and warranties are such
+Commercial Contributor's responsibility alone. Under this section, the
+Commercial Contributor would have to defend claims against the other
+Contributors related to those performance claims and warranties, and if a court
+requires any other Contributor to pay any damages as a result, the Commercial
+Contributor must pay those damages.
+
+5. NO WARRANTY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON AN
+"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR
+IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE,
+NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each
+Recipient is solely responsible for determining the appropriateness of using
+and distributing the Program and assumes all risks associated with its exercise
+of rights under this Agreement , including but not limited to the risks and
+costs of program errors, compliance with applicable laws, damage to or loss of
+data, programs or equipment, and unavailability or interruption of operations.
+
+6. DISCLAIMER OF LIABILITY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST
+PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS
+GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+7. GENERAL
+
+If any provision of this Agreement is invalid or unenforceable under applicable
+law, it shall not affect the validity or enforceability of the remainder of the
+terms of this Agreement, and without further action by the parties hereto, such
+provision shall be reformed to the minimum extent necessary to make such
+provision valid and enforceable.
+
+If Recipient institutes patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+(excluding combinations of the Program with other software or hardware)
+infringes such Recipient's patent(s), then such Recipient's rights granted
+under Section 2(b) shall terminate as of the date such litigation is filed.
+
+All Recipient's rights under this Agreement shall terminate if it fails to
+comply with any of the material terms or conditions of this Agreement and does
+not cure such failure in a reasonable period of time after becoming aware of
+such noncompliance. If all Recipient's rights under this Agreement terminate,
+Recipient agrees to cease use and distribution of the Program as soon as
+reasonably practicable. However, Recipient's obligations under this Agreement
+and any licenses granted by Recipient relating to the Program shall continue
+and survive.
+
+Everyone is permitted to copy and distribute copies of this Agreement, but in
+order to avoid inconsistency the Agreement is copyrighted and may only be
+modified in the following manner. The Agreement Steward reserves the right to
+publish new versions (including revisions) of this Agreement from time to time.
+No one other than the Agreement Steward has the right to modify this Agreement.
+The Eclipse Foundation is the initial Agreement Steward. The Eclipse Foundation
+may assign the responsibility to serve as the Agreement Steward to a suitable
+separate entity. Each new version of the Agreement will be given a
+distinguishing version number. The Program (including Contributions) may always
+be distributed subject to the version of the Agreement under which it was
+received. In addition, after a new version of the Agreement is published,
+Contributor may elect to distribute the Program (including its Contributions)
+under the new version. Except as expressly stated in Sections 2(a) and 2(b)
+above, Recipient receives no rights or licenses to the intellectual property of
+any Contributor under this Agreement, whether expressly, by implication,
+estoppel or otherwise. All rights in the Program not expressly granted under
+this Agreement are reserved.
+
+This Agreement is governed by the laws of the State of New York and the
+intellectual property laws of the United States of America. No party to this
+Agreement will bring a legal action under this Agreement more than one year
+after the cause of action arose. Each party waives its rights to a jury trial
+in any resulting litigation.
+
Modified: openrtm/trunk/flexiport/README.Flexiport
===================================================================
--- openrtm/trunk/flexiport/README.Flexiport 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/flexiport/README.Flexiport 2010-01-28 02:41:45 UTC (rev 482)
@@ -1,3 +1,15 @@
+#
+# GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+# http://gearbox.sf.net/
+# Copyright (c) 2008-2010 Geoffrey Biggs
+#
+# This work is a product of the National Institute of Advanced Industrial
+# Science and Technology, Japan. Registration number: H22PRO-1085.
+#
+# This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+# LICENSE.TXT.
+#
+
#======================================================================
# RTComponent: Flexiport specificatioin
#
Modified: openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.cpp
===================================================================
--- openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.cpp 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.cpp 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#include "GbxGarminAcfr.h"
#include <gbxgarminacfr/driver.h>
Modified: openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.h
===================================================================
--- openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.h 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxgarminacfr/GbxGarminAcfr.h 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#ifndef GBXGARMINACFR_H
#define GBXGARMINACFR_H
Added: openrtm/trunk/gbxgarminacfr/LICENSE.txt
===================================================================
--- openrtm/trunk/gbxgarminacfr/LICENSE.txt (rev 0)
+++ openrtm/trunk/gbxgarminacfr/LICENSE.txt 2010-01-28 02:41:45 UTC (rev 482)
@@ -0,0 +1,223 @@
+Copyright (C) 2009-2010
+ Geoffrey Biggs
+ RT-Synthesis Research Group
+ Intelligent Systems Research Institute,
+ National Institute of Advanced Industrial Science and Technology (AIST),
+ Japan
+ All rights reserved.
+Licensed under the Eclipse Public License -v 1.0 (EPL)
+http://www.opensource.org/licenses/eclipse-1.0.txt
+
+Eclipse Public License -v 1.0
+
+THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+
+1. DEFINITIONS
+
+"Contribution" means:
+
+a) in the case of the initial Contributor, the initial code and documentation
+distributed under this Agreement, and
+
+b) in the case of each subsequent Contributor:
+
+i) changes to the Program, and
+
+ii) additions to the Program;
+
+where such changes and/or additions to the Program originate from and are
+distributed by that particular Contributor. A Contribution 'originates' from a
+Contributor if it was added to the Program by such Contributor itself or anyone
+acting on such Contributor's behalf. Contributions do not include additions to
+the Program which: (i) are separate modules of software distributed in
+conjunction with the Program under their own license agreement, and (ii) are
+not derivative works of the Program.
+
+"Contributor" means any person or entity that distributes the Program.
+
+"Licensed Patents " mean patent claims licensable by a Contributor which are
+necessarily infringed by the use or sale of its Contribution alone or when
+combined with the Program.
+
+"Program" means the Contributions distributed in accordance with this
+Agreement.
+
+"Recipient" means anyone who receives the Program under this Agreement,
+including all Contributors.
+
+2. GRANT OF RIGHTS
+
+a) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free copyright license to
+reproduce, prepare derivative works of, publicly display, publicly perform,
+distribute and sublicense the Contribution of such Contributor, if any, and
+such derivative works, in source code and object code form.
+
+b) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free patent license under
+Licensed Patents to make, use, sell, offer to sell, import and otherwise
+transfer the Contribution of such Contributor, if any, in source code and
+object code form. This patent license shall apply to the combination of the
+Contribution and the Program if, at the time the Contribution is added by the
+Contributor, such addition of the Contribution causes such combination to be
+covered by the Licensed Patents. The patent license shall not apply to any
+other combinations which include the Contribution. No hardware per se is
+licensed hereunder.
+
+c) Recipient understands that although each Contributor grants the licenses to
+its Contributions set forth herein, no assurances are provided by any
+Contributor that the Program does not infringe the patent or other intellectual
+property rights of any other entity. Each Contributor disclaims any liability
+to Recipient for claims brought by any other entity based on infringement of
+intellectual property rights or otherwise. As a condition to exercising the
+rights and licenses granted hereunder, each Recipient hereby assumes sole
+responsibility to secure any other intellectual property rights needed, if any.
+For example, if a third party patent license is required to allow Recipient to
+distribute the Program, it is Recipient's responsibility to acquire that
+license before distributing the Program.
+
+d) Each Contributor represents that to its knowledge it has sufficient
+copyright rights in its Contribution, if any, to grant the copyright license
+set forth in this Agreement.
+
+3. REQUIREMENTS
+
+A Contributor may choose to distribute the Program in object code form under
+its own license agreement, provided that:
+
+a) it complies with the terms and conditions of this Agreement; and
+
+b) its license agreement:
+
+i) effectively disclaims on behalf of all Contributors all warranties and
+conditions, express and implied, including warranties or conditions of title
+and non-infringement, and implied warranties or conditions of merchantability
+and fitness for a particular purpose;
+
+ii) effectively excludes on behalf of all Contributors all liability for
+damages, including direct, indirect, special, incidental and consequential
+damages, such as lost profits;
+
+iii) states that any provisions which differ from this Agreement are offered by
+that Contributor alone and not by any other party; and
+
+iv) states that source code for the Program is available from such Contributor,
+and informs licensees how to obtain it in a reasonable manner on or through a
+medium customarily used for software exchange.
+
+When the Program is made available in source code form:
+
+a) it must be made available under this Agreement; and
+
+b) a copy of this Agreement must be included with each copy of the Program.
+
+Contributors may not remove or alter any copyright notices contained within the
+Program.
+
+Each Contributor must identify itself as the originator of its Contribution, if
+any, in a manner that reasonably allows subsequent Recipients to identify the
+originator of the Contribution.
+
+4. COMMERCIAL DISTRIBUTION
+
+Commercial distributors of software may accept certain responsibilities with
+respect to end users, business partners and the like. While this license is
+intended to facilitate the commercial use of the Program, the Contributor who
+includes the Program in a commercial product offering should do so in a manner
+which does not create potential liability for other Contributors. Therefore, if
+a Contributor includes the Program in a commercial product offering, such
+Contributor ("Commercial Contributor") hereby agrees to defend and indemnify
+every other Contributor ("Indemnified Contributor") against any losses, damages
+and costs (collectively "Losses") arising from claims, lawsuits and other legal
+actions brought by a third party against the Indemnified Contributor to the
+extent caused by the acts or omissions of such Commercial Contributor in
+connection with its distribution of the Program in a commercial product
+offering. The obligations in this section do not apply to any claims or Losses
+relating to any actual or alleged intellectual property infringement. In order
+to qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+Contributor in writing of such claim, and b) allow the Commercial Contributor
+to control, and cooperate with the Commercial Contributor in, the defense and
+any related settlement negotiations. The Indemnified Contributor may
+participate in any such claim at its own expense.
+
+For example, a Contributor might include the Program in a commercial product
+offering, Product X. That Contributor is then a Commercial Contributor. If that
+Commercial Contributor then makes performance claims, or offers warranties
+related to Product X, those performance claims and warranties are such
+Commercial Contributor's responsibility alone. Under this section, the
+Commercial Contributor would have to defend claims against the other
+Contributors related to those performance claims and warranties, and if a court
+requires any other Contributor to pay any damages as a result, the Commercial
+Contributor must pay those damages.
+
+5. NO WARRANTY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON AN
+"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR
+IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE,
+NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each
+Recipient is solely responsible for determining the appropriateness of using
+and distributing the Program and assumes all risks associated with its exercise
+of rights under this Agreement , including but not limited to the risks and
+costs of program errors, compliance with applicable laws, damage to or loss of
+data, programs or equipment, and unavailability or interruption of operations.
+
+6. DISCLAIMER OF LIABILITY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST
+PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS
+GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+7. GENERAL
+
+If any provision of this Agreement is invalid or unenforceable under applicable
+law, it shall not affect the validity or enforceability of the remainder of the
+terms of this Agreement, and without further action by the parties hereto, such
+provision shall be reformed to the minimum extent necessary to make such
+provision valid and enforceable.
+
+If Recipient institutes patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+(excluding combinations of the Program with other software or hardware)
+infringes such Recipient's patent(s), then such Recipient's rights granted
+under Section 2(b) shall terminate as of the date such litigation is filed.
+
+All Recipient's rights under this Agreement shall terminate if it fails to
+comply with any of the material terms or conditions of this Agreement and does
+not cure such failure in a reasonable period of time after becoming aware of
+such noncompliance. If all Recipient's rights under this Agreement terminate,
+Recipient agrees to cease use and distribution of the Program as soon as
+reasonably practicable. However, Recipient's obligations under this Agreement
+and any licenses granted by Recipient relating to the Program shall continue
+and survive.
+
+Everyone is permitted to copy and distribute copies of this Agreement, but in
+order to avoid inconsistency the Agreement is copyrighted and may only be
+modified in the following manner. The Agreement Steward reserves the right to
+publish new versions (including revisions) of this Agreement from time to time.
+No one other than the Agreement Steward has the right to modify this Agreement.
+The Eclipse Foundation is the initial Agreement Steward. The Eclipse Foundation
+may assign the responsibility to serve as the Agreement Steward to a suitable
+separate entity. Each new version of the Agreement will be given a
+distinguishing version number. The Program (including Contributions) may always
+be distributed subject to the version of the Agreement under which it was
+received. In addition, after a new version of the Agreement is published,
+Contributor may elect to distribute the Program (including its Contributions)
+under the new version. Except as expressly stated in Sections 2(a) and 2(b)
+above, Recipient receives no rights or licenses to the intellectual property of
+any Contributor under this Agreement, whether expressly, by implication,
+estoppel or otherwise. All rights in the Program not expressly granted under
+this Agreement are reserved.
+
+This Agreement is governed by the laws of the State of New York and the
+intellectual property laws of the United States of America. No party to this
+Agreement will bring a legal action under this Agreement more than one year
+after the cause of action arose. Each party waives its rights to a jury trial
+in any resulting litigation.
+
Modified: openrtm/trunk/gbxgarminacfr/README.GbxGarminAcfr
===================================================================
--- openrtm/trunk/gbxgarminacfr/README.GbxGarminAcfr 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxgarminacfr/README.GbxGarminAcfr 2010-01-28 02:41:45 UTC (rev 482)
@@ -1,3 +1,15 @@
+#
+# GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+# http://gearbox.sf.net/
+# Copyright (c) 2008-2010 Geoffrey Biggs
+#
+# This work is a product of the National Institute of Advanced Industrial
+# Science and Technology, Japan. Registration number: H22PRO-1085.
+#
+# This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+# LICENSE.TXT.
+#
+
======================================================================
RTComponent: GbxGarminAcfr specificatioin
Modified: openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.cpp
===================================================================
--- openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.cpp 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.cpp 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#include "GbxNovatelAcfr.h"
#include <gbxnovatelacfr/driver.h>
Modified: openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.h
===================================================================
--- openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.h 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxnovatelacfr/GbxNovatelAcfr.h 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#ifndef GBXNOVATELACFR_H
#define GBXNOVATELACFR_H
Added: openrtm/trunk/gbxnovatelacfr/LICENSE.txt
===================================================================
--- openrtm/trunk/gbxnovatelacfr/LICENSE.txt (rev 0)
+++ openrtm/trunk/gbxnovatelacfr/LICENSE.txt 2010-01-28 02:41:45 UTC (rev 482)
@@ -0,0 +1,223 @@
+Copyright (C) 2009-2010
+ Geoffrey Biggs
+ RT-Synthesis Research Group
+ Intelligent Systems Research Institute,
+ National Institute of Advanced Industrial Science and Technology (AIST),
+ Japan
+ All rights reserved.
+Licensed under the Eclipse Public License -v 1.0 (EPL)
+http://www.opensource.org/licenses/eclipse-1.0.txt
+
+Eclipse Public License -v 1.0
+
+THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+
+1. DEFINITIONS
+
+"Contribution" means:
+
+a) in the case of the initial Contributor, the initial code and documentation
+distributed under this Agreement, and
+
+b) in the case of each subsequent Contributor:
+
+i) changes to the Program, and
+
+ii) additions to the Program;
+
+where such changes and/or additions to the Program originate from and are
+distributed by that particular Contributor. A Contribution 'originates' from a
+Contributor if it was added to the Program by such Contributor itself or anyone
+acting on such Contributor's behalf. Contributions do not include additions to
+the Program which: (i) are separate modules of software distributed in
+conjunction with the Program under their own license agreement, and (ii) are
+not derivative works of the Program.
+
+"Contributor" means any person or entity that distributes the Program.
+
+"Licensed Patents " mean patent claims licensable by a Contributor which are
+necessarily infringed by the use or sale of its Contribution alone or when
+combined with the Program.
+
+"Program" means the Contributions distributed in accordance with this
+Agreement.
+
+"Recipient" means anyone who receives the Program under this Agreement,
+including all Contributors.
+
+2. GRANT OF RIGHTS
+
+a) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free copyright license to
+reproduce, prepare derivative works of, publicly display, publicly perform,
+distribute and sublicense the Contribution of such Contributor, if any, and
+such derivative works, in source code and object code form.
+
+b) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free patent license under
+Licensed Patents to make, use, sell, offer to sell, import and otherwise
+transfer the Contribution of such Contributor, if any, in source code and
+object code form. This patent license shall apply to the combination of the
+Contribution and the Program if, at the time the Contribution is added by the
+Contributor, such addition of the Contribution causes such combination to be
+covered by the Licensed Patents. The patent license shall not apply to any
+other combinations which include the Contribution. No hardware per se is
+licensed hereunder.
+
+c) Recipient understands that although each Contributor grants the licenses to
+its Contributions set forth herein, no assurances are provided by any
+Contributor that the Program does not infringe the patent or other intellectual
+property rights of any other entity. Each Contributor disclaims any liability
+to Recipient for claims brought by any other entity based on infringement of
+intellectual property rights or otherwise. As a condition to exercising the
+rights and licenses granted hereunder, each Recipient hereby assumes sole
+responsibility to secure any other intellectual property rights needed, if any.
+For example, if a third party patent license is required to allow Recipient to
+distribute the Program, it is Recipient's responsibility to acquire that
+license before distributing the Program.
+
+d) Each Contributor represents that to its knowledge it has sufficient
+copyright rights in its Contribution, if any, to grant the copyright license
+set forth in this Agreement.
+
+3. REQUIREMENTS
+
+A Contributor may choose to distribute the Program in object code form under
+its own license agreement, provided that:
+
+a) it complies with the terms and conditions of this Agreement; and
+
+b) its license agreement:
+
+i) effectively disclaims on behalf of all Contributors all warranties and
+conditions, express and implied, including warranties or conditions of title
+and non-infringement, and implied warranties or conditions of merchantability
+and fitness for a particular purpose;
+
+ii) effectively excludes on behalf of all Contributors all liability for
+damages, including direct, indirect, special, incidental and consequential
+damages, such as lost profits;
+
+iii) states that any provisions which differ from this Agreement are offered by
+that Contributor alone and not by any other party; and
+
+iv) states that source code for the Program is available from such Contributor,
+and informs licensees how to obtain it in a reasonable manner on or through a
+medium customarily used for software exchange.
+
+When the Program is made available in source code form:
+
+a) it must be made available under this Agreement; and
+
+b) a copy of this Agreement must be included with each copy of the Program.
+
+Contributors may not remove or alter any copyright notices contained within the
+Program.
+
+Each Contributor must identify itself as the originator of its Contribution, if
+any, in a manner that reasonably allows subsequent Recipients to identify the
+originator of the Contribution.
+
+4. COMMERCIAL DISTRIBUTION
+
+Commercial distributors of software may accept certain responsibilities with
+respect to end users, business partners and the like. While this license is
+intended to facilitate the commercial use of the Program, the Contributor who
+includes the Program in a commercial product offering should do so in a manner
+which does not create potential liability for other Contributors. Therefore, if
+a Contributor includes the Program in a commercial product offering, such
+Contributor ("Commercial Contributor") hereby agrees to defend and indemnify
+every other Contributor ("Indemnified Contributor") against any losses, damages
+and costs (collectively "Losses") arising from claims, lawsuits and other legal
+actions brought by a third party against the Indemnified Contributor to the
+extent caused by the acts or omissions of such Commercial Contributor in
+connection with its distribution of the Program in a commercial product
+offering. The obligations in this section do not apply to any claims or Losses
+relating to any actual or alleged intellectual property infringement. In order
+to qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+Contributor in writing of such claim, and b) allow the Commercial Contributor
+to control, and cooperate with the Commercial Contributor in, the defense and
+any related settlement negotiations. The Indemnified Contributor may
+participate in any such claim at its own expense.
+
+For example, a Contributor might include the Program in a commercial product
+offering, Product X. That Contributor is then a Commercial Contributor. If that
+Commercial Contributor then makes performance claims, or offers warranties
+related to Product X, those performance claims and warranties are such
+Commercial Contributor's responsibility alone. Under this section, the
+Commercial Contributor would have to defend claims against the other
+Contributors related to those performance claims and warranties, and if a court
+requires any other Contributor to pay any damages as a result, the Commercial
+Contributor must pay those damages.
+
+5. NO WARRANTY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON AN
+"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR
+IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE,
+NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each
+Recipient is solely responsible for determining the appropriateness of using
+and distributing the Program and assumes all risks associated with its exercise
+of rights under this Agreement , including but not limited to the risks and
+costs of program errors, compliance with applicable laws, damage to or loss of
+data, programs or equipment, and unavailability or interruption of operations.
+
+6. DISCLAIMER OF LIABILITY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST
+PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS
+GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+7. GENERAL
+
+If any provision of this Agreement is invalid or unenforceable under applicable
+law, it shall not affect the validity or enforceability of the remainder of the
+terms of this Agreement, and without further action by the parties hereto, such
+provision shall be reformed to the minimum extent necessary to make such
+provision valid and enforceable.
+
+If Recipient institutes patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+(excluding combinations of the Program with other software or hardware)
+infringes such Recipient's patent(s), then such Recipient's rights granted
+under Section 2(b) shall terminate as of the date such litigation is filed.
+
+All Recipient's rights under this Agreement shall terminate if it fails to
+comply with any of the material terms or conditions of this Agreement and does
+not cure such failure in a reasonable period of time after becoming aware of
+such noncompliance. If all Recipient's rights under this Agreement terminate,
+Recipient agrees to cease use and distribution of the Program as soon as
+reasonably practicable. However, Recipient's obligations under this Agreement
+and any licenses granted by Recipient relating to the Program shall continue
+and survive.
+
+Everyone is permitted to copy and distribute copies of this Agreement, but in
+order to avoid inconsistency the Agreement is copyrighted and may only be
+modified in the following manner. The Agreement Steward reserves the right to
+publish new versions (including revisions) of this Agreement from time to time.
+No one other than the Agreement Steward has the right to modify this Agreement.
+The Eclipse Foundation is the initial Agreement Steward. The Eclipse Foundation
+may assign the responsibility to serve as the Agreement Steward to a suitable
+separate entity. Each new version of the Agreement will be given a
+distinguishing version number. The Program (including Contributions) may always
+be distributed subject to the version of the Agreement under which it was
+received. In addition, after a new version of the Agreement is published,
+Contributor may elect to distribute the Program (including its Contributions)
+under the new version. Except as expressly stated in Sections 2(a) and 2(b)
+above, Recipient receives no rights or licenses to the intellectual property of
+any Contributor under this Agreement, whether expressly, by implication,
+estoppel or otherwise. All rights in the Program not expressly granted under
+this Agreement are reserved.
+
+This Agreement is governed by the laws of the State of New York and the
+intellectual property laws of the United States of America. No party to this
+Agreement will bring a legal action under this Agreement more than one year
+after the cause of action arose. Each party waives its rights to a jury trial
+in any resulting litigation.
+
Modified: openrtm/trunk/gbxnovatelacfr/README.GbxNovatelAcfr
===================================================================
--- openrtm/trunk/gbxnovatelacfr/README.GbxNovatelAcfr 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxnovatelacfr/README.GbxNovatelAcfr 2010-01-28 02:41:45 UTC (rev 482)
@@ -1,3 +1,15 @@
+#
+# GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+# http://gearbox.sf.net/
+# Copyright (c) 2008-2010 Geoffrey Biggs
+#
+# This work is a product of the National Institute of Advanced Industrial
+# Science and Technology, Japan. Registration number: H22PRO-1085.
+#
+# This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+# LICENSE.TXT.
+#
+
======================================================================
RTComponent: GbxNovatelAcfr specificatioin
Added: openrtm/trunk/gbxsickacfr/LICENSE.txt
===================================================================
--- openrtm/trunk/gbxsickacfr/LICENSE.txt (rev 0)
+++ openrtm/trunk/gbxsickacfr/LICENSE.txt 2010-01-28 02:41:45 UTC (rev 482)
@@ -0,0 +1,223 @@
+Copyright (C) 2009-2010
+ Geoffrey Biggs
+ RT-Synthesis Research Group
+ Intelligent Systems Research Institute,
+ National Institute of Advanced Industrial Science and Technology (AIST),
+ Japan
+ All rights reserved.
+Licensed under the Eclipse Public License -v 1.0 (EPL)
+http://www.opensource.org/licenses/eclipse-1.0.txt
+
+Eclipse Public License -v 1.0
+
+THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+
+1. DEFINITIONS
+
+"Contribution" means:
+
+a) in the case of the initial Contributor, the initial code and documentation
+distributed under this Agreement, and
+
+b) in the case of each subsequent Contributor:
+
+i) changes to the Program, and
+
+ii) additions to the Program;
+
+where such changes and/or additions to the Program originate from and are
+distributed by that particular Contributor. A Contribution 'originates' from a
+Contributor if it was added to the Program by such Contributor itself or anyone
+acting on such Contributor's behalf. Contributions do not include additions to
+the Program which: (i) are separate modules of software distributed in
+conjunction with the Program under their own license agreement, and (ii) are
+not derivative works of the Program.
+
+"Contributor" means any person or entity that distributes the Program.
+
+"Licensed Patents " mean patent claims licensable by a Contributor which are
+necessarily infringed by the use or sale of its Contribution alone or when
+combined with the Program.
+
+"Program" means the Contributions distributed in accordance with this
+Agreement.
+
+"Recipient" means anyone who receives the Program under this Agreement,
+including all Contributors.
+
+2. GRANT OF RIGHTS
+
+a) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free copyright license to
+reproduce, prepare derivative works of, publicly display, publicly perform,
+distribute and sublicense the Contribution of such Contributor, if any, and
+such derivative works, in source code and object code form.
+
+b) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free patent license under
+Licensed Patents to make, use, sell, offer to sell, import and otherwise
+transfer the Contribution of such Contributor, if any, in source code and
+object code form. This patent license shall apply to the combination of the
+Contribution and the Program if, at the time the Contribution is added by the
+Contributor, such addition of the Contribution causes such combination to be
+covered by the Licensed Patents. The patent license shall not apply to any
+other combinations which include the Contribution. No hardware per se is
+licensed hereunder.
+
+c) Recipient understands that although each Contributor grants the licenses to
+its Contributions set forth herein, no assurances are provided by any
+Contributor that the Program does not infringe the patent or other intellectual
+property rights of any other entity. Each Contributor disclaims any liability
+to Recipient for claims brought by any other entity based on infringement of
+intellectual property rights or otherwise. As a condition to exercising the
+rights and licenses granted hereunder, each Recipient hereby assumes sole
+responsibility to secure any other intellectual property rights needed, if any.
+For example, if a third party patent license is required to allow Recipient to
+distribute the Program, it is Recipient's responsibility to acquire that
+license before distributing the Program.
+
+d) Each Contributor represents that to its knowledge it has sufficient
+copyright rights in its Contribution, if any, to grant the copyright license
+set forth in this Agreement.
+
+3. REQUIREMENTS
+
+A Contributor may choose to distribute the Program in object code form under
+its own license agreement, provided that:
+
+a) it complies with the terms and conditions of this Agreement; and
+
+b) its license agreement:
+
+i) effectively disclaims on behalf of all Contributors all warranties and
+conditions, express and implied, including warranties or conditions of title
+and non-infringement, and implied warranties or conditions of merchantability
+and fitness for a particular purpose;
+
+ii) effectively excludes on behalf of all Contributors all liability for
+damages, including direct, indirect, special, incidental and consequential
+damages, such as lost profits;
+
+iii) states that any provisions which differ from this Agreement are offered by
+that Contributor alone and not by any other party; and
+
+iv) states that source code for the Program is available from such Contributor,
+and informs licensees how to obtain it in a reasonable manner on or through a
+medium customarily used for software exchange.
+
+When the Program is made available in source code form:
+
+a) it must be made available under this Agreement; and
+
+b) a copy of this Agreement must be included with each copy of the Program.
+
+Contributors may not remove or alter any copyright notices contained within the
+Program.
+
+Each Contributor must identify itself as the originator of its Contribution, if
+any, in a manner that reasonably allows subsequent Recipients to identify the
+originator of the Contribution.
+
+4. COMMERCIAL DISTRIBUTION
+
+Commercial distributors of software may accept certain responsibilities with
+respect to end users, business partners and the like. While this license is
+intended to facilitate the commercial use of the Program, the Contributor who
+includes the Program in a commercial product offering should do so in a manner
+which does not create potential liability for other Contributors. Therefore, if
+a Contributor includes the Program in a commercial product offering, such
+Contributor ("Commercial Contributor") hereby agrees to defend and indemnify
+every other Contributor ("Indemnified Contributor") against any losses, damages
+and costs (collectively "Losses") arising from claims, lawsuits and other legal
+actions brought by a third party against the Indemnified Contributor to the
+extent caused by the acts or omissions of such Commercial Contributor in
+connection with its distribution of the Program in a commercial product
+offering. The obligations in this section do not apply to any claims or Losses
+relating to any actual or alleged intellectual property infringement. In order
+to qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+Contributor in writing of such claim, and b) allow the Commercial Contributor
+to control, and cooperate with the Commercial Contributor in, the defense and
+any related settlement negotiations. The Indemnified Contributor may
+participate in any such claim at its own expense.
+
+For example, a Contributor might include the Program in a commercial product
+offering, Product X. That Contributor is then a Commercial Contributor. If that
+Commercial Contributor then makes performance claims, or offers warranties
+related to Product X, those performance claims and warranties are such
+Commercial Contributor's responsibility alone. Under this section, the
+Commercial Contributor would have to defend claims against the other
+Contributors related to those performance claims and warranties, and if a court
+requires any other Contributor to pay any damages as a result, the Commercial
+Contributor must pay those damages.
+
+5. NO WARRANTY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON AN
+"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR
+IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE,
+NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each
+Recipient is solely responsible for determining the appropriateness of using
+and distributing the Program and assumes all risks associated with its exercise
+of rights under this Agreement , including but not limited to the risks and
+costs of program errors, compliance with applicable laws, damage to or loss of
+data, programs or equipment, and unavailability or interruption of operations.
+
+6. DISCLAIMER OF LIABILITY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST
+PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS
+GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+7. GENERAL
+
+If any provision of this Agreement is invalid or unenforceable under applicable
+law, it shall not affect the validity or enforceability of the remainder of the
+terms of this Agreement, and without further action by the parties hereto, such
+provision shall be reformed to the minimum extent necessary to make such
+provision valid and enforceable.
+
+If Recipient institutes patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+(excluding combinations of the Program with other software or hardware)
+infringes such Recipient's patent(s), then such Recipient's rights granted
+under Section 2(b) shall terminate as of the date such litigation is filed.
+
+All Recipient's rights under this Agreement shall terminate if it fails to
+comply with any of the material terms or conditions of this Agreement and does
+not cure such failure in a reasonable period of time after becoming aware of
+such noncompliance. If all Recipient's rights under this Agreement terminate,
+Recipient agrees to cease use and distribution of the Program as soon as
+reasonably practicable. However, Recipient's obligations under this Agreement
+and any licenses granted by Recipient relating to the Program shall continue
+and survive.
+
+Everyone is permitted to copy and distribute copies of this Agreement, but in
+order to avoid inconsistency the Agreement is copyrighted and may only be
+modified in the following manner. The Agreement Steward reserves the right to
+publish new versions (including revisions) of this Agreement from time to time.
+No one other than the Agreement Steward has the right to modify this Agreement.
+The Eclipse Foundation is the initial Agreement Steward. The Eclipse Foundation
+may assign the responsibility to serve as the Agreement Steward to a suitable
+separate entity. Each new version of the Agreement will be given a
+distinguishing version number. The Program (including Contributions) may always
+be distributed subject to the version of the Agreement under which it was
+received. In addition, after a new version of the Agreement is published,
+Contributor may elect to distribute the Program (including its Contributions)
+under the new version. Except as expressly stated in Sections 2(a) and 2(b)
+above, Recipient receives no rights or licenses to the intellectual property of
+any Contributor under this Agreement, whether expressly, by implication,
+estoppel or otherwise. All rights in the Program not expressly granted under
+this Agreement are reserved.
+
+This Agreement is governed by the laws of the State of New York and the
+intellectual property laws of the United States of America. No party to this
+Agreement will bring a legal action under this Agreement more than one year
+after the cause of action arose. Each party waives its rights to a jury trial
+in any resulting litigation.
+
Modified: openrtm/trunk/gbxsickacfr/README.gbxsickacfr
===================================================================
--- openrtm/trunk/gbxsickacfr/README.gbxsickacfr 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxsickacfr/README.gbxsickacfr 2010-01-28 02:41:45 UTC (rev 482)
@@ -1,3 +1,15 @@
+#
+# GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+# http://gearbox.sf.net/
+# Copyright (c) 2008-2010 Geoffrey Biggs
+#
+# This work is a product of the National Institute of Advanced Industrial
+# Science and Technology, Japan. Registration number: H22PRO-1085.
+#
+# This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+# LICENSE.TXT.
+#
+
======================================================================
RTComponent: gbxsickacfr specificatioin
Modified: openrtm/trunk/gbxsickacfr/gbxsickacfr.cpp
===================================================================
--- openrtm/trunk/gbxsickacfr/gbxsickacfr.cpp 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxsickacfr/gbxsickacfr.cpp 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#include "gbxsickacfr.h"
#include <gbxutilacfr/trivialtracer.h>
Modified: openrtm/trunk/gbxsickacfr/gbxsickacfr.h
===================================================================
--- openrtm/trunk/gbxsickacfr/gbxsickacfr.h 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxsickacfr/gbxsickacfr.h 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#ifndef GBXSICKACFR_H
#define GBXSICKACFR_H
Modified: openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.cpp
===================================================================
--- openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.cpp 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.cpp 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008-2010 Geoffrey Biggs
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1085.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * LICENSE.TXT.
+ *
+ */
#include "GbxSmartBatteryAcfr.h"
#include <gbxutilacfr/trivialtracer.h>
Modified: openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.h
===================================================================
--- openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.h 2010-01-28 02:29:51 UTC (rev 481)
+++ openrtm/trunk/gbxsmartbatteryacfr/GbxSmartBatteryAcfr.h 2010-01-28 02:41:45 UTC (rev 482)
@@ -6,6 +6,18 @@
*
* $Id$
*/
+/*
+ * GearBox...
[truncated message content] |