Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[499] gearbox/trunk/src/hokuyo_aist
|
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] |