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