gearbox

Revision: 500
          http://gearbox.svn.sourceforge.net/gearbox/?rev=500&view=rev
Author:   gbiggs
Date:     2010-08-04 00:21:24 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Re-worked error handling

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
    gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
    gearbox/trunk/src/hokuyo_aist/test/example.cpp

Added Paths:
-----------
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h

Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-04 00:21:24 UTC (rev 500)
@@ -12,8 +12,8 @@
 if (build)
 	include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
 
-	set (hdrs hokuyo_aist.h)
-	set (srcs hokuyo_aist.cpp)
+	set (hdrs hokuyo_aist.h hokuyo_errors.h)
+	set (srcs hokuyo_aist.cpp hokuyo_errors.cpp)
 
 	if (WIN32)
 		if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)

Modified: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp	2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp	2010-08-04 00:21:24 UTC (rev 500)
@@ -23,6 +23,7 @@
 using namespace std;
 
 #include <hokuyo_aist/hokuyo_aist.h>
+#include <hokuyo_aist/hokuyo_errors.h>
 
 int main(int argc, char **argv)
 {
@@ -76,9 +77,9 @@
 		// Close the laser
 		laser.close ();
 	}
-	catch (hokuyo_aist::BaseError e)
+	catch (hokuyo_aist::BaseError &e)
 	{
-		cerr << "Caught exception: (" << e.code () << ") " << e.what () << endl;
+		cerr << "Caught exception: " << e.what () << endl;
 		return 1;
 	}
 

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-04 00:21:24 UTC (rev 500)
@@ -18,6 +18,7 @@
  */
 
 #include "hokuyo_aist.h"
+#include "hokuyo_errors.h"
 using namespace hokuyo_aist;
 
 #include <flexiport/flexiport.h>
@@ -189,242 +190,6 @@
 // Utility functions
 ///////////////////////////////////////////////////////////////////////////////
 
-std::string scip1_error_to_string(char error, char cmd)
-{
-    return std::string("No error descriptions available");
-}
-
-// error must be null-terminated
-std::string scip2_error_to_string(const char *error, const char *cmd)
-{
-    std::stringstream ss;
-
-    // Check for non-errors
-    if (error[0] == '0' && error[1] == '0')
-        return "Status OK - 00";
-    else if (error[0] == '9' && error[1] == '9')
-        return "Status OK - 99";
-
-    // Check for universal errors
-    if (error[0] == '0' && error[1] == 'A')
-        return "Unable to create transmission data or reply command internally";
-    else if (error[0] == '0' && error[1] == 'B')
-        return "Buffer shortage or command repeated that is already processed";
-    else if (error[0] == '0' && error[1] == 'C')
-        return "Command with insufficient parameters 1";
-    else if (error[0] == '0' && error[1] == 'D')
-        return "Undefined command 1";
-    else if (error[0] == '0' && error[1] == 'E')
-        return "Undefined command 2";
-    else if (error[0] == '0' && error[1] == 'F')
-        return "Command with insufficient parameters 2";
-    else if (error[0] == '0' && error[1] == 'G')
-        return "String character in command exceeds 16 letters";
-    else if (error[0] == '0' && error[1] == 'H')
-        return "String character has invalid letters";
-    else if (error[0] == '0' && error[1] == 'I')
-        return "Sensor is now in firmware update mode";
-
-    int error_code = atoi(error);
-
-    if (cmd[0] == 'B' && cmd[1] == 'M')
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Unable to control due to laser malfunction";
-            case 2:
-                return "Laser is already on";
-        }
-    }
-// No info in the manual for this.
-//    else if (cmd[0] == 'Q' && cmd[1] == 'T')
-//    {
-//        switch (error_code)
-//        {
-//            default:
-//                std::stringstream ss;
-//                ss << "Unknown error code " << error_code <<
-//                      " for command " << cmd[0] << cmd[1];
-//                return ss.str ();
-//        }
-//    }
-    else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
-             (cmd[0] == 'G' && cmd[1] == 'S'))
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Starting step has non-numeric value";
-            case 2:
-                return "Ending step has non-numeric value";
-            case 3:
-                return "Cluster count has non-numeric value";
-            case 4:
-                return "Ending step is out of range";
-            case 5:
-                return "Ending step is smaller than start step";
-            case 10:
-                return "Laser is off";
-            default:
-                if (error_code >= 50)
-                    ss << "Hardware error: " << error_code;
-                else
-                    ss << "Unknown error code " << error_code <<
-                        " for command " << cmd[0] << cmd[1];
-
-                return ss.str();
-        }
-    }
-    else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
-             (cmd[0] == 'M' && cmd[1] == 'S'))
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Starting step has non-numeric value";
-            case 2:
-                return "Ending step has non-numeric value";
-            case 3:
-                return "Cluster count has non-numeric value";
-            case 4:
-                return "Ending step is out of range";
-            case 5:
-                return "Ending step is smaller than start step";
-            case 6:
-                return "Scan interval has non-numeric value";
-            case 7:
-                return "Number of scans is non-numeric";
-            default:
-                if (error_code >= 21 && error_code <= 49)
-                {
-                    return "Processing stopped to verify error. "
-                        "This function is not yet supported by hokuyo_aist.";
-                }
-                else if (error_code >= 50 && error_code <= 97)
-                    ss << "Hardware error: " << error_code;
-                else if (error_code == 98)
-                {
-                    return "Resumption of processing after confirming normal "
-                        "laser opteration. This function is not yet supported "
-                        "by hokuyo_aist.";
-                }
-                else
-                    ss << "Unknown error code " << error_code <<
-                        " for command " << cmd[0] << cmd[1];
-
-                return ss.str();
-        }
-    }
-    else if (cmd[0] == 'T' && cmd[1] == 'M')
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Invalid control code";
-            case 2:
-                return "Adjust mode on command received when sensor's adjust "
-                    "mode is already on";
-            case 3:
-                return "Adjust mode off command received when sensor's adjust "
-                    "mode is already off";
-            case 4:
-                return "Adjust mode is off when requested time";
-        }
-    }
-    else if (cmd[0] == 'S' && cmd[1] == 'S')
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Baud rate has non-numeric value";
-            case 2:
-                return "Invalid baud rate";
-            case 3:
-                return "Sensor is already running at that baud rate";
-            case 4:
-                return "Not compatible with the sensor model";
-        }
-    }
-    else if (cmd[0] == 'C' && cmd[1] == 'R')
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Invalid speed";
-            case 2:
-                return "Speed is out of range";
-            case 3:
-                return "Motor is already running at that speed";
-            case 4:
-                return "Not compatible with the sensor model";
-        }
-    }
-    else if (cmd[0] == 'H' && cmd[1] == 'S')
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "Parameter error";
-            case 2:
-                return "Already running in the set mode";
-            case 3:
-                return "Not compatible with the sensor model";
-        }
-    }
-// No info in the manual for this.
-//    else if (cmd[0] == 'R' && cmd[1] == 'S')
-//    {
-//        switch (error_code)
-//        {
-//            case :
-//                return "";
-//            default:
-//                std::stringstream ss;
-//                ss << "Unknown error code " << error_code <<
-//                    " for command " << cmd[0] << cmd[1];
-//                return ss.str ();
-//        }
-//    }
-// No info in the manual for this.
-//    else if (cmd[0] == 'V' && cmd[1] == 'V')
-//    {
-//        switch (error_code)
-//        {
-//            case :
-//                return "";
-//        }
-//    }
-// No info in the manual for this.
-//    else if (cmd[0] == 'P' && cmd[1] == 'P')
-//    {
-//        switch (error_code)
-//        {
-//            case :
-//                return "";
-//        }
-//    }
-// No info in the manual for this.
-//    else if (cmd[0] == 'I' && cmd[1] == 'I')
-//    {
-//        switch (error_code)
-//        {
-//            case :
-//                return "";
-//        }
-//    }
-    else
-    {
-        ss << "Unknown command: " << cmd[0] << cmd[1];
-        return ss.str();
-    }
-
-    // Known commands with unknown error codes fall through to here
-    ss << "Unknown error code " << error_code << " for command " << cmd[0] <<
-        cmd[1];
-    return ss.str();
-}
-
 unsigned int decode_2_byte_value(char *data)
 {
     unsigned int byte1, byte2;
@@ -471,46 +236,6 @@
 }
 
 ///////////////////////////////////////////////////////////////////////////////
-// BaseError class
-///////////////////////////////////////////////////////////////////////////////
-
-std::string BaseError::as_string() const throw()
-{
-    switch (_error_code)
-    {
-        case HOKUYO_ERR_READ:
-            return "HOKUYO_ERR_READ";
-        case HOKUYO_ERR_WRITE:
-            return "HOKUYO_ERR_WRITE";
-        case HOKUYO_ERR_PROTOCOL:
-            return "HOKUYO_ERR_PROTOCOL";
-        case HOKUYO_ERR_CHANGEBAUD:
-            return "HOKUYO_ERR_CHANGEBAUD";
-        case HOKUYO_ERR_CONNECT_FAILED:
-            return "HOKUYO_ERR_CONNECT_FAILED";
-        case HOKUYO_ERR_CLOSE_FAILED:
-            return "HOKUYO_ERR_CLOSE_FAILED";
-        case HOKUYO_ERR_NODESTINATION:
-            return "HOKUYO_ERR_NODESTINATION";
-        case HOKUYO_ERR_BADFIRMWARE:
-            return "HOKUYO_ERR_BADFIRMWARE";
-        case HOKUYO_ERR_SCIPVERSION:
-            return "HOKUYO_ERR_SCIPVERSION";
-        case HOKUYO_ERR_MEMORY:
-            return "HOKUYO_ERR_MEMORY";
-        case HOKUYO_ERR_UNSUPPORTED:
-            return "HOKUYO_ERR_UNSUPPORTED";
-        case HOKUYO_ERR_BADARG:
-            return "HOKUYO_ERR_BADARG";
-        case HOKUYO_ERR_NODATA:
-            return "HOKUYO_ERR_NODATA";
-        case HOKUYO_ERR_NOTSERIAL:
-            return "HOKUYO_ERR_NOTSERIAL";
-    }
-    return "UNKNOWN_ERROR";
-}
-
-///////////////////////////////////////////////////////////////////////////////
 // SensorInfo class
 ///////////////////////////////////////////////////////////////////////////////
 
@@ -824,7 +549,7 @@
 uint32_t ScanData::operator[](unsigned int index)
 {
     if (index >= _length)
-        throw BaseError(HOKUYO_ERR_BADARG, "Invalid data index.");
+        throw IndexError();
     return _ranges[index];
 }
 
@@ -1070,7 +795,7 @@
 void Sensor::close()
 {
     if (!_port)
-        throw BaseError(HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
+        throw CloseError();
     if (_verbose)
         std::cerr << "Sensor::" << __func__ << "() Closing connection." <<
             std::endl;
@@ -1124,7 +849,7 @@
         _skip_lines(1);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 // This function assumes that both the port and the laser scanner are already
@@ -1133,8 +858,7 @@
 {
     if (_port->GetPortType() != "serial")
     {
-        throw BaseError(HOKUYO_ERR_NOTSERIAL,
-            "Cannot change baud rate of non-serial connection.");
+        throw NotSerialError();
     }
 
     char newBaud[13];
@@ -1143,9 +867,7 @@
     if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
         baud != 250000 && baud != 500000 && baud != 750000)
     {
-        std::stringstream ss;
-        ss << "Bad baud rate: " << baud << std::endl;
-        throw BaseError(HOKUYO_ERR_BADARG, ss.str());
+        throw BaudrateError(baud);
     }
     number_to_string(baud, newBaud, 6);
 
@@ -1166,16 +888,13 @@
         reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 void Sensor::reset()
 {
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-            "SCIP version 1 does not support the reset command.");
-    }
+        throw UnsupportedError(7);
     else if (_scip_version == 2)
     {
         if (_verbose)
@@ -1185,21 +904,18 @@
         _skip_lines(1);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 void Sensor::set_motor_speed(unsigned int speed)
 {
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-            "SCIP version 1 does not support the set motor speed command.");
-    }
+        throw UnsupportedError(8);
     else if (_scip_version == 2)
     {
         // Sanity check the value
         if (speed > 10 && speed != 99)
-            throw BaseError(HOKUYO_ERR_BADARG, "Invalid motor speed.");
+            throw MotorSpeedError();
         char buffer[3];
         if (speed == 0)
         {
@@ -1236,16 +952,13 @@
         _skip_lines(1);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 void Sensor::set_high_sensitivity(bool on)
 {
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-            "SCIP version 1 does not support the high sensitivity command.");
-    }
+        throw UnsupportedError(10);
     else if (_scip_version == 2)
     {
         if (on)
@@ -1267,14 +980,13 @@
         _skip_lines(1);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 void Sensor::get_sensor_info(SensorInfo *info)
 {
     if (info == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No info object provided.");
+        throw NoDestinationError();
 
     if (_scip_version == 1)
     {
@@ -1431,16 +1143,13 @@
         }
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 }
 
 unsigned int Sensor::get_time()
 {
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-                "SCIP version 1 does not support the get time command.");
-    }
+        throw UnsupportedError(12);
     else if (_scip_version == 2)
     {
         if (_verbose)
@@ -1456,7 +1165,7 @@
         return decode_4_byte_value(buffer);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 
     return 0;
 }
@@ -1465,8 +1174,7 @@
         int end_step, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDestinationError();
 
     char buffer[11];
     memset(buffer, 0, sizeof(char) * 11);
@@ -1508,15 +1216,14 @@
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
         if (_read_line_with_check(buffer) == 0)
-            throw BaseError(HOKUYO_ERR_NODATA,
-                    "No data received. Check data error code.");
+            throw NoDataError();
         data->_time = decode_4_byte_value(buffer);
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the GD command
         _read_3_byte_range_data(data, num_steps);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 
     return data->_length;
 }
@@ -1525,8 +1232,7 @@
         double end_angle, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDataError();
 
     // Calculate the given angles in steps, rounding towards _front_step
     int start_step, end_step;
@@ -1535,9 +1241,9 @@
 
     // Check the steps are within the allowable range
     if (start_step < _first_step || start_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+        throw StartStepError();
     if (end_step < _first_step || end_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+        throw EndStepError();
 
     if (_verbose)
     {
@@ -1554,14 +1260,10 @@
         int end_step, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDestinationError();
 
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-                "SCIP version 1 does not support the get new ranges command.");
-    }
+        throw UnsupportedError(16);
     else if (_scip_version == 2)
     {
         char buffer[14];
@@ -1596,19 +1298,11 @@
         _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
         // Check the echo is correct
         if (response[0] != 'M' || response[1] != 'D')
-        {
-            std::stringstream ss;
-            ss << "Incorrect data prefix: MD" << " != " << response[0] <<
-                response[1];
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw CommandEchoError("MD", response);
         // Then compare the parameters
         buffer[12] = '0'; // There will be zero scans remaining after this one
         if (memcmp(&response[2], buffer, 13) != 0)
-        {
-            throw BaseError(HOKUYO_ERR_PROTOCOL,
-                    "Incorrect paramaters prefix for MD data.");
-        }
+            throw ParamEchoError("MD");
         // The next line should be the status line
         _read_line_with_check(response, 4);
         if (_verbose)
@@ -1623,10 +1317,7 @@
             // There is an extra line feed after an error status (signalling
             // end of message)
             _skip_lines(1);
-            std::stringstream ss;
-            ss << "Bad status for MD data: " << response[0] << response[1] <<
-                " " << scip2_error_to_string(response, "MD");
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+            throw ResponseError(response, "MD");
         }
 
         // Now the actual data will arrive
@@ -1634,15 +1325,14 @@
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
         if (_read_line_with_check(buffer) == 0)
-            throw BaseError(HOKUYO_ERR_NODATA,
-                    "No data received. Check data error code.");
+            throw NoDataError();
         data->_time = decode_4_byte_value(buffer);
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the MD command
         _read_3_byte_range_data(data, num_steps);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 
     return data->_length;
 }
@@ -1651,13 +1341,9 @@
         double start_angle, double end_angle, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDestinationError();
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-                "SCIP version 1 does not support the get new ranges command.");
-    }
+        throw UnsupportedError(16);
 
     // Calculate the given angles in steps, rounding towards _front_step
     int start_step, end_step;
@@ -1666,9 +1352,9 @@
 
     // Check the steps are within the allowable range
     if (start_step < _first_step || start_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+        throw StartStepError();
     if (end_step < _first_step || end_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+        throw EndStepError();
 
     if (_verbose)
     {
@@ -1685,15 +1371,10 @@
         int start_step, int end_step, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDestinationError();
 
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-            "SCIP version 1 does not support the get new ranges and "
-            "intensities command.");
-    }
+        throw UnsupportedError(17);
     else if (_scip_version == 2)
     {
         char buffer[14];
@@ -1729,18 +1410,10 @@
         _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
         // Check the echo is correct
         if (response[0] != 'M' || response[1] != 'E')
-        {
-            std::stringstream ss;
-            ss << "Incorrect data prefix: ME" << " != " << response[0] <<
-                response[1];
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw CommandEchoError("ME", response);
         // Then compare the parameters
         if (memcmp(&response[2], buffer, 13) != 0)
-        {
-            throw BaseError(HOKUYO_ERR_PROTOCOL,
-                    "Incorrect paramaters prefix for ME data.");
-        }
+            throw ParamEchoError("ME");
         // The next line should be the status line
         _read_line_with_check(response, 4);
         if (_verbose)
@@ -1755,10 +1428,7 @@
             // There is an extra line feed after an error status (signalling
             // end of message)
             _skip_lines(1);
-            std::stringstream ss;
-            ss << "Bad status for ME data: " << response[0] << response[1] <<
-                " " << scip2_error_to_string(response, "MD");
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+            throw ResponseError(response, "ME");
         }
 
         // Now the actual data will arrive
@@ -1766,15 +1436,14 @@
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
         if (_read_line_with_check(buffer) == 0)
-            throw BaseError(HOKUYO_ERR_NODATA,
-                    "No data received. Check data error code.");
+            throw NoDataError();
         data->_time = decode_4_byte_value(buffer);
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the ME command
         _read_3_byte_range_and_intensity_data(data, num_steps);
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 
     return data->_length;
 }
@@ -1783,14 +1452,9 @@
         double start_angle, double end_angle, unsigned int cluster_count)
 {
     if (data == NULL)
-        throw BaseError(HOKUYO_ERR_NODESTINATION,
-                "No data destination provided.");
+        throw NoDestinationError();
     if (_scip_version == 1)
-    {
-        throw BaseError(HOKUYO_ERR_UNSUPPORTED,
-            "SCIP version 1 does not support the get new ranges and "
-            "intensities command.");
-    }
+        throw UnsupportedError(17);
 
     // Calculate the given angles in steps, rounding towards _front_step
     int start_step, end_step;
@@ -1799,9 +1463,9 @@
 
     // Check the steps are within the allowable range
     if (start_step < _first_step || start_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "Start step is out of range.");
+        throw StartStepError();
     if (end_step < _first_step || end_step > _last_step)
-        throw BaseError(HOKUYO_ERR_BADARG, "End step is out of range.");
+        throw EndStepError();
 
     if (_verbose)
     {
@@ -1876,11 +1540,9 @@
         }
         // We need to get at least 1 byte in a line: the line feed.
         if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
-            throw BaseError(HOKUYO_ERR_READ,
-                    "Timed out trying to read a line.");
+            throw ReadError(0);
         else if (linelength == 0)
-            throw BaseError(HOKUYO_ERR_READ,
-                    "No data received when trying to read a line.");
+            throw ReadError(1);
         // Replace the line feed with a NULL
         buffer[linelength - 1] = '\0';
     }
@@ -1894,19 +1556,11 @@
         }
         // expected_length+1 for the NULL
         if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
-            throw BaseError(HOKUYO_ERR_READ,
-                    "Timed out trying to read a line.");
+            throw ReadError(0);
         else if (linelength == 0)
-            throw BaseError(HOKUYO_ERR_READ,
-                    "No data received when trying to read a line.");
+            throw ReadError(1);
         else if (linelength < expected_length)
-        {
-            std::stringstream ss;
-            ss << "Sensor::" << __func__ <<
-                "() Got an incorrect line length: " << linelength << " != " <<
-                expected_length;
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw LineLengthError(linelength, expected_length);
         // Replace the line feed with a NULL
         buffer[linelength - 1] = '\0';
     }
@@ -1964,13 +1618,7 @@
             linelength << " bytes." << std::endl;
     }
     if (bytesToConsider < 1)
-    {
-        std::stringstream ss;
-        ss << "Not enough bytes to calculate checksum with: " <<
-            bytesToConsider << " bytes (line length is " << linelength <<
-            " bytes).";
-        throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-    }
+        throw InsufficientBytesError(bytesToConsider, linelength);
 
     int checkSum = 0;
     try
@@ -1978,10 +1626,9 @@
         checkSum = _confirm_checksum(buffer, bytesToConsider,
                 static_cast<int>(buffer[checksumIndex]));
     }
-    catch (BaseError &e)
+    catch (ProtocolError &e)
     {
-        if (e.code() == HOKUYO_ERR_PROTOCOL && _sensorIsUTM30LX &&
-                _enable_checksum_workaround)
+        if (_sensorIsUTM30LX && _enable_checksum_workaround)
         {
             // Here comes the UTM-30LX workaround
             char* hasComment = strstr(buffer, "<-");
@@ -1997,11 +1644,8 @@
                 }
                 if (newBytesToConsider < 1)
                 {
-                    std::stringstream ss;
-                    ss << "Not enough bytes to calculate checksum with: " <<
-                        newBytesToConsider << " bytes (line length is " <<
-                        linelength << " bytes).";
-                    throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                    throw InsufficientBytesError(newBytesToConsider,
+                            linelength);
                 }
 
                 checkSum = _confirm_checksum(buffer, newBytesToConsider,
@@ -2029,7 +1673,7 @@
         std::cerr << "Sensor::" << __func__ << "() Skipping " << count <<
             " lines." << std::endl;
     if (_port->SkipUntil(0x0A, count) < 0)
-        throw BaseError(HOKUYO_ERR_READ, "Timed out while skipping.");
+        throw ReadError(18);
 }
 
 // Sends a command with optional parameters and checks that the echo of the
@@ -2060,17 +1704,14 @@
         }
         // Write the command
         if (_port->Write(cmd, 1) < 1)
-            throw BaseError(HOKUYO_ERR_WRITE,
-                    "Failed to write command byte.");
+            throw WriteError(19);
         if (param_length > 0)
         {
             if (_port->Write(param, param_length) < param_length)
-                throw BaseError(HOKUYO_ERR_WRITE,
-                        "Failed to write command parameters.");
+                throw WriteError(20);
         }
         if (_port->Write("\n", 1) < 1)
-            throw BaseError(HOKUYO_ERR_WRITE,
-                    "Failed to write termination character.");
+            throw WriteError(21);
 
         // Read back the response (should get at least 4 bytes , possibly up to
         // 16 including \n's depending on the parameters): cmd[0] params \n
@@ -2081,18 +1722,23 @@
         // First make sure that the echoed command matches
         if (response[0] != cmd[0])
         {
-            throw BaseError(HOKUYO_ERR_PROTOCOL,
-                    std::string("Incorrect command echo: ") + cmd[0] +
-                    std::string(" != ") + response[0]);
+            char temp_cmd[2];
+            temp_cmd[0] = cmd[0];
+            temp_cmd[1] = '\0';
+            char temp_echo[2];
+            temp_echo[0] = response[0];
+            temp_echo[1] = '\0';
+            throw CommandEchoError(temp_cmd, temp_echo);
         }
         // Then compare the parameters
         if (param_length > 0)
         {
             if (memcmp(&response[1], param, param_length) != 0)
             {
-                throw BaseError(HOKUYO_ERR_PROTOCOL,
-                    std::string("Incorrect paramaters echo for command ") +
-                    cmd[0]);
+                char temp_cmd[2];
+                temp_cmd[0] = cmd[0];
+                temp_cmd[1] = '\0';
+                throw ParamEchoError(temp_cmd);
             }
         }
         // Next up, check the status byte
@@ -2111,11 +1757,7 @@
                     // There is an extra line feed after an error status
                     // (signalling end of message)
                     _skip_lines(1);
-                    std::stringstream ss;
-                    ss << "Bad response to " << cmd[0] << " command: " <<
-                        " " << scip1_error_to_string(response[statusIndex],
-                                cmd[0]);
-                    throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                    throw Scip1ResponseError(response[statusIndex], cmd[0]);
                 }
             }
             else
@@ -2123,11 +1765,7 @@
                 // There is an extra line feed after an error status
                 // (signalling end of message)
                 _skip_lines(1);
-                std::stringstream ss;
-                ss << "Bad response to " << cmd[0] << " command: " <<
-                    response[statusIndex] << " " <<
-                    scip1_error_to_string(response[statusIndex], cmd[0]);
-                throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                throw Scip1ResponseError(response[statusIndex], cmd[0]);
             }
         }
         statusCode = atoi(&response[statusIndex]);
@@ -2143,36 +1781,25 @@
         }
         // Write the command
         if (_port->Write(cmd, 2) < 2)
-            throw BaseError(HOKUYO_ERR_WRITE, "Failed to write command byte.");
+            throw WriteError(19);
         if (param_length > 0)
         {
             if (_port->Write(param, param_length) < param_length)
-                throw BaseError(HOKUYO_ERR_WRITE,
-                        "Failed to write command parameters.");
+                throw WriteError(20);
         }
         if (_port->Write("\n", 1) < 1)
-            throw BaseError(HOKUYO_ERR_WRITE,
-                    "Failed to write termination character.");
+            throw WriteError(21);
 
         // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
         _read_line(response, 3 + param_length);
         // Check the echo is correct
         if (response[0] != cmd[0] || response[1] != cmd[1])
-        {
-            std::stringstream ss;
-            ss << "Incorrect command echo: " << cmd << " != " << response[0] <<
-                response[1];
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw CommandEchoError(cmd, response);
         // Then compare the parameters
         if (param_length > 0)
         {
             if (memcmp(&response[2], param, param_length) != 0)
-            {
-                throw BaseError(HOKUYO_ERR_PROTOCOL,
-                        std::string("Incorrect paramaters echo for command ") +
-                        cmd);
-            }
+                throw ParamEchoError(cmd);
         }
 
         // The next line should be the status line
@@ -2195,11 +1822,7 @@
                     // There is an extra line feed after an error status
                     // (signalling end of message)
                     _skip_lines(1);
-                    std::stringstream ss;
-                    ss << "Bad response to " << cmd << " command: " <<
-                        response[0] << response[1] << " " <<
-                        scip2_error_to_string(response, cmd);
-                    throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                    throw ResponseError(response, cmd);
                 }
             }
             else
@@ -2207,18 +1830,14 @@
                 // There is an extra line feed after an error status
                 // (signalling end of message)
                 _skip_lines(1);
-                std::stringstream ss;
-                ss << "Bad response to " << cmd << " command: " <<
-                    response[0] << response[1] << " " <<
-                    scip2_error_to_string(response, cmd);
-                throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                throw ResponseError(response, cmd);
             }
         }
         statusCode = atoi(response);
         // All OK, data starts at beginning of port's buffer
     }
     else
-        throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+        throw UnknownScipVersionError();
 
     return statusCode;
 }
@@ -2258,8 +1877,7 @@
         }
         catch (BaseError)
         {
-            throw BaseError(HOKUYO_ERR_SCIPVERSION,
-                    "SCIP versions 1 and 2 failed.");
+            throw ScipVersionError();
         }
         // Skip the vendor and product info
         _skip_lines(2);
@@ -2269,10 +1887,7 @@
         _read_line(buffer);
 
         if (strncmp(buffer, "FIRM:", 5) != 0)
-        {
-            throw BaseError(HOKUYO_ERR_PROTOCOL,
-                "'FIRM:' was not found when checking firmware version.");
-        }
+            throw MissingFirmSpecError();
         // Pull out the major version number
         // Note that although lasers such as the UTM-30LX appear to use a
         // different firmware version format, that doesn't matter because they
@@ -2280,8 +1895,7 @@
         // they do, it's an uncaught error.
         int majorVer = strtol(&buffer[5], NULL, 10);
         if (errno == ERANGE)
-            throw BaseError(HOKUYO_ERR_BADFIRMWARE,
-                    "Out-of-range firmware version.");
+            throw FirmwareError();
         if (_verbose)
         {
             std::cerr << "Sensor::" << __func__ <<
@@ -2348,7 +1962,7 @@
     }
 
     // Fallback case if didn't find a good SCIP version and return above
-    throw BaseError(HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
+    throw ScipVersionError();
 }
 
 void Sensor::_get_defaults()
@@ -2402,11 +2016,7 @@
     else if (strncmp(buffer, "SERI", 4) == 0)
         info->serial = &buffer[5]; // Serial number
     else if (!_ignore_unknowns)
-    {
-        std::stringstream ss;
-        ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
-        throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-    }
+        throw UnknownLineError(buffer);
 }
 
 void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
@@ -2429,11 +2039,7 @@
     else if (strncmp(buffer, "SCAN", 4) == 0)
         info->standard_speed = atoi(&buffer[5]);
     else if (!_ignore_unknowns)
-    {
-        std::stringstream ss;
-        ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
-        throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-    }
+        throw UnknownLineError(buffer);
 }
 
 void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
@@ -2455,9 +2061,7 @@
             // Unchanged motor speed
             if (sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
             {
-                std::stringstream ss;
-                ss << "Motor speed line parse failed: " << buffer;
-                throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                throw ParseError(buffer, "Motor speed");
             }
             info->speed_level = 0;
         }
@@ -2468,9 +2072,7 @@
             if (sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
                         &info->speed) != 2)
             {
-                std::stringstream ss;
-                ss << "Motor speed line parse failed: " << buffer;
-                throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
+                throw ParseError(buffer, "Motor speed");
             }
         }
     }
@@ -2485,29 +2087,17 @@
             info->baud = 0;
         }
         else if (sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
-        {
-            std::stringstream ss;
-            ss << "Baud rate line parse failed: " << buffer;
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw ParseError(buffer, "Baud rate");
     }
     else if (strncmp(buffer, "TIME", 4) == 0)
     {
         if (sscanf(buffer, "TIME:%x", &info->time) != 1)
-        {
-            std::stringstream ss;
-            ss << "Timestamp line parse failed: " << buffer;
-            throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-        }
+            throw ParseError(buffer, "Timestamp");
     }
     else if (strncmp(buffer, "STAT", 4) == 0)
         info->sensor_diagnostic = &buffer[5];
     else if (!_ignore_unknowns)
-    {
-        std::stringstream ss;
-        ss << "Sensor::" << __func__ << "() Unknown line: " << buffer;
-        throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-    }
+        throw UnknownLineError(buffer);
 }
 
 void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
@@ -2539,8 +2129,7 @@
             if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a data block? Why?
-                throw BaseError(HOKUYO_ERR_PROTOCOL,
-                        "Found line feed in a data block.");
+                throw MisplacedLineFeedError();
             }
             data->_ranges[currentStep] = decode_2_byte_value(&buffer[ii]);
             if (data->_ranges[currentStep] < 20)
@@ -2553,8 +2142,7 @@
         std::cerr << "Sensor::" << __func__ << "() Read " <<
             currentStep << " ranges." << std::endl;
     if (currentStep != num_steps)
-        throw BaseError(HOKUYO_ERR_PROTOCOL,
-                "Read less range readings than were asked for.");
+        throw DataCountError();
 }
 
 void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
@@ -2589,8 +2177,7 @@
             if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a line? Why?
-                throw BaseError(HOKUYO_ERR_PROTOCOL,
-                        "Found line feed in a data block.");
+                throw MisplacedLineFeedError();
             }
             if (ii == numBytesInLine - 2)       // Short 1 byte
             {
@@ -2654,10 +2241,7 @@
         std::cerr << "Sensor::" << __func__ << "() Read " <<
             currentStep << " ranges." << std::endl;
     if (currentStep != num_steps)
-    {
-        throw BaseError(HOKUYO_ERR_PROTOCOL,
-            "Read a different number of range readings than were asked for.");
-    }
+        throw DataCountError();
 }
 
 void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
@@ -2695,8 +2279,7 @@
             if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a line? Why?
-                throw BaseError(HOKUYO_ERR_PROTOCOL,
-                        "Found line feed in a data block.");
+                throw MisplacedLineFeedError();
             }
             if (ii == numBytesInLine - 2)       // Short 1 byte
             {
@@ -2782,43 +2365,34 @@
             " intensities (expected " << num_steps << ")." << std::endl;
     }
     if (currentRange != num_steps || currentIntensity != num_steps)
-    {
-        throw BaseError(HOKUYO_ERR_PROTOCOL,
-            "Read a different number of range or intensity readings than "
-            "were asked for.");
-    }
+        throw DataCountError();
 }
 
 int Sensor::_confirm_checksum(const char *buffer, int length,
         int expected_sum)
 {
-    int checkSum = 0;
+    int checksum = 0;
     // Start by adding the byte values
     for (int ii = 0; ii < length; ii++)
-        checkSum += buffer[ii];
+        checksum += buffer[ii];
     // Take the lowest 6 bits
-    checkSum &= 0x3F;
+    checksum &= 0x3F;
     // Add 0x30
-    checkSum += 0x30;
+    checksum += 0x30;
 
     if (_verbose)
     {
         std::cerr << "Sensor::" << __func__ <<
-            "() Calculated checksum = " << checkSum << " (" <<
-            static_cast<char>(checkSum) << "), given checksum = " <<
+            "() Calculated checksum = " << checksum << " (" <<
+            static_cast<char>(checksum) << "), given checksum = " <<
             expected_sum << " (" << static_cast<char> (expected_sum) <<
             ")" << std::endl;
     }
-    if (checkSum != expected_sum)
-    {
-        std::stringstream ss;
-        ss << "Invalid checksum -  given: " << expected_sum <<
-            ", calculated: " << checkSum;
-        throw BaseError(HOKUYO_ERR_PROTOCOL, ss.str());
-    }
+    if (checksum != expected_sum)
+        throw ChecksumError(expected_sum, checksum);
 
-    return checkSum;
+    return checksum;
 }
 
-} // namespace hokuyo_aist
+}; // namespace hokuyo_aist
 

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 00:21:24 UTC (rev 500)
@@ -17,8 +17,8 @@
  * http://www.opensource.org/licenses/eclipse-1.0.txt
  */
 
-#ifndef __HOKUYO_AIST_H
-#define __HOKUYO_AIST_H
+#ifndef HOKUYO_AIST_H__
+#define HOKUYO_AIST_H__
 
 #include <flexiport/port.h>
 #include <string>
@@ -45,35 +45,6 @@
 namespace hokuyo_aist
 {
 
-/// Hokuyo general error class.
-class HOKUYO_AIST_EXPORT BaseError : public std::exception
-{
-    public:
-        /** @brief Hokuyo error constructor.
-
-        @param code Error code of the error.
-        @param desc Description of the error. */
-        BaseError(unsigned int code, std::string desc)
-            : _error_code(code), _error_desc(desc)
-        {}
-        virtual ~BaseError() throw() {};
-
-        virtual unsigned int code() const throw()
-            { return _error_code; }
-
-        virtual const char* what() const throw()
-            { return _error_desc.c_str(); }
-
-        virtual std::string as_string() const throw();
-
-    private:
-        /** Error code. */
-        unsigned int _error_code;
-        /** Error description. */
-        std::string _error_desc;
-}; //class BaseError
-
-
 #if defined(WIN32)
 // Exporting data members on Windows is a bloody pain (they have to be
 // initialised somewhere else), so we'll use #define's on Win32 and lose the
@@ -569,4 +540,5 @@
 
 /** @} */
 
-#endif // _ _HOKUYO_AIST_H
+#endif // HOKUYO_AIST_H__
+

Added: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	                        (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 00:21:24 UTC (rev 500)
@@ -0,0 +1,450 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ *               http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * hokuyo_aist Hokuyo laser scanner driver.
+ *
+ * This distribution is licensed to you under the terms described in the
+ * LICENSE file included in this distribution.
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1086.
+ *
+ * This file is part of hokuyo_aist.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * http://www.opensource.org/licenses/eclipse-1.0.txt
+ */
+
+#include "hokuyo_errors.h"
+
+#include <cstdlib>
+#include <cstring>
+
+namespace hokuyo_aist
+{
+
+// error must be null-terminated
+std::string scip2_error_to_string(char const* const error,
+        char const* const cmd)
+{
+    std::stringstream ss;
+
+    // Check for non-errors
+    if (error[0] == '0' && error[1] == '0')
+        return "Status OK - 00";
+    else if (error[0] == '9' && error[1] == '9')
+        return "Status OK - 99";
+
+    // Check for universal errors
+    if (error[0] == '0' && error[1] == 'A')
+        return "Unable to create transmission data or reply command internally";
+    else if (error[0] == '0' && error[1] == 'B')
+        return "Buffer shortage or command repeated that is already processed";
+    else if (error[0] == '0' && error[1] == 'C')
+        return "Command with insufficient parameters 1";
+    else if (error[0] == '0' && error[1] == 'D')
+        return "Undefined command 1";
+    else if (error[0] == '0' && error[1] == 'E')
+        return "Undefined command 2";
+    else if (error[0] == '0' && error[1] == 'F')
+        return "Command with insufficient parameters 2";
+    else if (error[0] == '0' && error[1] == 'G')
+        return "String character in command exceeds 16 letters";
+    else if (error[0] == '0' && error[1] == 'H')
+        return "String character has invalid letters";
+    else if (error[0] == '0' && error[1] == 'I')
+        return "Sensor is now in firmware update mode";
+
+    int error_code = atoi(error);
+
+    if (cmd[0] == 'B' && cmd[1] == 'M')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Unable to control due to laser malfunction";
+            case 2:
+                return "Laser is already on";
+        }
+    }
+// No info in the manual for this.
+//    else if (cmd[0] == 'Q' && cmd[1] == 'T')
+//    {
+//        switch (error_code)
+//        {
+//            default:
+//                std::stringstream ss;
+//                ss << "Unknown error code " << error_code <<
+//                      " for command " << cmd[0] << cmd[1];
+//                return ss.str ();
+//        }
+//    }
+    else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+             (cmd[0] == 'G' && cmd[1] == 'S'))
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Starting step has non-numeric value";
+            case 2:
+                return "Ending step has non-numeric value";
+            case 3:
+                return "Cluster count has non-numeric value";
+            case 4:
+                return "Ending step is out of range";
+            case 5:
+                return "Ending step is smaller than start step";
+            case 10:
+                return "Laser is off";
+            default:
+                if (error_code >= 50)
+                    ss << "Hardware error: " << error_code;
+                else
+                    ss << "Unknown error code " << error_code <<
+                        " for command " << cmd[0] << cmd[1];
+
+                return ss.str();
+        }
+    }
+    else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+             (cmd[0] == 'M' && cmd[1] == 'S'))
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Starting step has non-numeric value";
+            case 2:
+                return "Ending step has non-numeric value";
+            case 3:
+                return "Cluster count has non-numeric value";
+            case 4:
+                return "Ending step is out of range";
+            case 5:
+                return "Ending step is smaller than start step";
+            case 6:
+                return "Scan interval has non-numeric value";
+            case 7:
+                return "Number of scans is non-numeric";
+            default:
+                if (error_code >= 21 && error_code <= 49)
+                {
+                    return "Processing stopped to verify error. "
+                        "This function is not yet supported by hokuyo_aist.";
+                }
+                else if (error_code >= 50 && error_code <= 97)
+                    ss << "Hardware error: " << error_code;
+                else if (error_code == 98)
+                {
+                    return "Resumption of processing after confirming normal "
+                        "laser opteration. This function is not yet supported "
+                        "by hokuyo_aist.";
+                }
+                else
+                    ss << "Unknown error code " << error_code <<
+                        " for command " << cmd[0] << cmd[1];
+
+                return ss.str();
+        }
+    }
+    else if (cmd[0] == 'T' && cmd[1] == 'M')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Invalid control code";
+            case 2:
+                return "Adjust mode on command received when sensor's adjust "
+                    "mode is already on";
+            case 3:
+                return "Adjust mode off command received when sensor's adjust "
+                    "mode is already off";
+            case 4:
+                return "Adjust mode is off when requested time";
+        }
+    }
+    else if (cmd[0] == 'S' && cmd[1] == 'S')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Baud rate has non-numeric value";
+            case 2:
+                return "Invalid baud rate";
+            case 3:
+                return "Sensor is already running at that baud rate";
+            case 4:
+                return "Not compatible with the sensor model";
+        }
+    }
+    else if (cmd[0] == 'C' && cmd[1] == 'R')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Invalid speed";
+            case 2:
+                return "Speed is out of range";
+            case 3:
+                return "Motor is already running at that speed";
+            case 4:
+                return "Not compatible with the sensor model";
+        }
+    }
+    else if (cmd[0] == 'H' && cmd[1] == 'S')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Parameter error";
+            case 2:
+                return "Already running in the set mode";
+            case 3:
+                return "Not compatible with the sensor model";
+        }
+    }
+// No info in the manual for this.
+//    else if (cmd[0] == 'R' && cmd[1] == 'S')
+//    {
+//        switch (error_code)
+//        {
+//            case :
+//                return "";
+//            default:
+//                std::stringstream ss;
+//                ss << "Unknown error code " << error_code <<
+//                    " for command " << cmd[0] << cmd[1];
+//                return ss.str ();
+//        }
+//    }
+// No info in the manual for this.
+//    else if (cmd[0] == 'V' && cmd[1] == 'V')
+//    {
+//        switch (error_code)
+//        {
+//            case :
+//                return "";
+//        }
+//    }
+// No info in the manual for this.
+//    else if (cmd[0] == 'P' && cmd[1] == 'P')
+//    {
+//        switch (error_code)
+//        {
+//            case :
+//                return "";
+//        }
+//    }
+// No info in the manual for this.
+//    else if (cmd[0] == 'I' && cmd[1] == 'I')
+//    {
+//        switch (error_code)
+//        {
+//            case :
+//                return "";
+//        }
+//    }
+    else
+    {
+        ss << "Unknown command: " << cmd[0] << cmd[1];
+        return ss.str();
+    }
+
+    // Known commands with unknown error codes fall through to here
+    ss << "Unknown error code " << error_code << " for command " << cmd[0] <<
+        cmd[1];
+    return ss.str();
+}
+
+
+std::string desc_code_to_string(unsigned int code)
+{
+    static char const* const descriptions[] = {
+        "Timed out trying to read a line",
+        "No data received when trying to read a line.",
+        "Invalid data index.",
+        "Port is not open.",
+        "Unknown SCIP version.",
+        "Cannot change baud rate of non-serial connection.",
+        "Bad baud rate: ",
+        "SCIP version 1 does not support the reset command.",
+        "SCIP version 1 does not support the set motor speed command.",
+        "Invalid motor speed.",
+        "SCIP version 1 does not support the high sensitivity command.",
+        "No info object provided.",
+        "SCIP version 1 does not support the get time command.",
+        "No data received. Check data error code.",
+        "Start step is out of range.",
+        "End step is out of range.",
+        "SCIP version 1 does not support the get new ranges command.",
+        "SCIP version 1 does not support the get new ranges and intensities command.",
+        "Timed out while skipping.",
+        "Failed to write command byte.",
+        "Failed to write command parameters.",
+        "Failed to write termination character.",
+        "SCIP versions 1 and 2 failed.",
+        "Out-of-range firmware version.",
+        "Invalid checksum: ",
+        "Read a different number of range or intensity readings than were asked for.",
+        "Found line feed in a data block.",
+        "Unknown line: ",
+        "Parse error: ",
+        "'FIRM:' was not found when checking firmware version.",
+        "Bad response.",
+        "Incorrect command echo.",
+        "Incorrect parameters echo for command.",
+        "Not enough bytes to calculate checksum.",
+        "Incorrect line length received.",
+    };
+
+    return std::string(descriptions[code]);
+}
+
+
+const char* BaseError::what() throw()
+{
+    ss << _error_type << " (" << _desc_code << "): " <<
+        desc_code_to_string(_desc_code);
+    return ss.str().c_str();
+}
+
+
+const char* BaudrateError::what() throw()
+{
+    RuntimeError::what();
+    ss << _baud;
+    return ss.str().c_str();
+}
+
+
+const char* ChecksumError::what() throw()
+{
+    ProtocolError::what();
+    ss << "expected " << _expected << ", calculated " << _calculated;
+    return ss.str().c_str();
+}
+
+
+UnknownLineError::UnknownLineError(char const* const line)
+    : ProtocolError(27)
+{
+    strncpy(_line, line, 128);
+}
+
+
+const char* UnknownLineError::what() throw()
+{
+    ProtocolError::what();
+    ss << _line;
+    return ss.str().c_str();
+}
+
+
+ParseError::ParseError(char const* const line, char const* const type)
+    : ProtocolError(28)
+{
+    strncpy(_line, line, 128);
+    strncpy(_type, type, 16);
+}
+
+
+const char* ParseError::what() throw()
+{
+    ProtocolError::what();
+    ss << "Line type: " << _type << ". Line: " << _line;
+    return ss.str().c_str();
+}
+
+
+const char* ResponseError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Command: " << _cmd[0] << _cmd[1];
+    ss << " Error : (" << _error[0] << _error[1] << ") " <<
+        scip2_error_to_string(_error, _cmd);
+    return ss.str().c_str();
+}
+
+
+const char* Scip1ResponseError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Command: " << _cmd;
+    ss << " Error : " << _error;
+    return ss.str().c_str();
+}
+
+
+const char* CommandEchoError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Command: " << _cmd[0] << _cmd[1];
+    ss << " Received echo: " << _echo[0] << _echo[1];
+    return ss.str().c_str();
+}
+
+
+const char* ParamEchoError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Command: " << _cmd[0] << _cmd[1];
+    return ss.str().c_str();
+}
+
+
+const char* InsufficientBytesError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Number of bytes: " << _num;
+    ss << " Line length: " << _line_length;
+    return ss.str().c_str();
+}
+
+
+const char* LineLengthError::what() throw()
+{
+    ProtocolError::what();
+    ss << " Received length: " << _length;
+    ss << " Expected line length: " << _expected;
+    return ss.str().c_str();
+}
+
+
+// Static member initialisation
+
+char const* BaseError::_error_type = "BaseError";
+char const* LogicError::_error_type = "LogicError";
+char const* RuntimeError::_error_type = "RuntimeError";
+char const* ReadError::_error_type = "ReadError";
+char const* WriteError::_error_type = "WriteError";
+char const* BaudrateError::_error_type = "BaudrateError";
+char const* CloseError::_error_type = "CloseError";
+char const* NoDestinationError::_error_type = "NoDestinationError";
+char const* FirmwareError::_error_type = "FirmwareError";
+char const* ScipVersionError::_error_type = "ScipVersionError";
+char const* UnknownScipVersionError::_error_type = "UnknownScipVersionError";
+char const* UnsupportedError::_error_type = "UnsupportedError";
+char const* ArgError::_error_type = "ArgError";
+char const* NoDataError::_error_type = "NoDataError";
+char const* NotSerialError::_error_type = "NotSerialError";
+char const* IndexError::_error_type = "IndexError";
+char const* MotorSpeedError::_error_type = "MotorSpeedError";
+char const* StartStepError::_error_type = "StartStepError";
+char const* EndStepError::_error_type = "EndStepError";
+char const* ProtocolError::_error_type = "ProtocolError";
+char const* ChecksumError::_error_type = "ChecksumError";
+char const* DataCountError::_error_type = "DataCountError";
+char const* MisplacedLineFeedError::_error_type = "MisplacedLineFeedError";
+char const* UnknownLineError::_error_type = "UnknownLineError";
+char const* ParseError::_error_type = "ParseError";
+char const* MissingFirmSpecError::_error_type = "MissingFirmSpecError";
+char const* ResponseError::_error_type = "ResponseError";
+char const* Scip1ResponseError::_error_type = "Scip1ResponseError";
+char const* CommandEchoError::_error_type = "CommandEchoError";
+char const* ParamEchoError::_error_type = "ParamEchoError";
+char const* InsufficientBytesError::_error_type = "InsufficientBytesError";
+char const* LineLengthError::_error_type = "LineLengthError";
+
+}; // namespace hokuyo_aist
+

Added: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	                        (rev 0)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 00:21:24 UTC (rev 500)
@@ -0,0 +1,701 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ *               http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * hokuyo_aist Hokuyo laser scanner driver.
+ *
+ * This distribution is licensed to you under the terms described in the
+ * LICENSE file included in this distribution.
+ *
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1086.
+ *
+ * This file is part of hokuyo_aist.
+ *
+ * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
+ * http://www.opensource.org/licenses/eclipse-1.0.txt
+ */
+
+#ifndef HOKUYO_ERRORS_H__
+#define HOKUYO_ERRORS_H__
+
+#include <sstream>
+
+#if defined(WIN32)
+    typedef unsigned char           uint8_t;
+    typedef unsigned int            uint32_t;
+    #if defined(HOKUYO_AIST_STATIC)
+        #define HOKUYO_AIST_EXPORT
+    #elif defined(HOKUYO_AIST_EXPORTS)
+        #define HOKUYO_AIST_EXPORT       __declspec(dllexport)
+    #else
+        #define HOKUYO_AIST_EXPORT       __declspec(dllimport)
+    #endif
+#else
+    #include <stdint.h>
+    #define HOKUYO_AIST_EXPORT
+#endif
+
+/** @ingroup gbx_library_hokuyo_aist
+@{
+*/
+
+namespace hokuyo_aist
+{
+
+/// Translates a SCIP2 error code into a string.
+std::string scip2_error_to_string(char const* const error,
+        char const* const cmd);
+
+/// Translates an error description code into a string.
+std::string desc_code_to_string(unsigned int code);
+
+
+/// General error class.
+class HOKUYO_AIST_EXPORT BaseError : public std::exception
+{
+    public:
+        /** @brief Hokuyo error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        BaseError(unsigned int desc_code)
+            : _desc_code(desc_code)
+        {}
+        BaseError(BaseError const& rhs)
+            : _desc_code(rhs.desc_code())
+        {}
+        virtual ~BaseError() throw() {};
+
+        virtual unsigned int desc_code() const throw()
+            { return _desc_code; }
+
+        virtual const char* what() throw();
+
+    protected:
+        /** Description code for use with the error string table. */
+        unsigned int _desc_code;
+
+        /** Formatted description of the error. */
+        std::stringstream ss;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; //class BaseError
+
+
+/// Logic error class
+class HOKUYO_AIST_EXPORT LogicError : public BaseError
+{
+    public:
+        /** @brief Logic error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        LogicError(unsigned int desc_code)
+            : BaseError(desc_code)
+        {}
+        virtual ~LogicError() throw() {};
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class LogicError
+
+
+/// Runtime error class
+class HOKUYO_AIST_EXPORT RuntimeError : public BaseError
+{
+    public:
+        /** @brief Runtime error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        RuntimeError(unsigned int desc_code)
+            : BaseError(desc_code)
+        {}
+        virtual ~RuntimeError() throw() {};
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class RuntimeError
+
+
+/// Read error class
+class HOKUYO_AIST_EXPORT ReadError: public RuntimeError
+{
+    public:
+        /** @brief Read error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        ReadError(unsigned int desc_code)
+            : RuntimeError(desc_code)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ReadError
+
+
+/// Write error class
+class HOKUYO_AIST_EXPORT WriteError: public RuntimeError
+{
+    public:
+        /** @brief Write error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        WriteError(unsigned int desc_code)
+            : RuntimeError(desc_code)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class WriteError
+
+
+/// Baudrate error class
+class HOKUYO_AIST_EXPORT BaudrateError: public RuntimeError
+{
+    public:
+        /** @brief Baud rate error constructor.
+
+        @param baud The bad baud rate. */
+        BaudrateError(unsigned int baud)
+            : RuntimeError(6), _baud(baud)
+        {}
+
+        unsigned int baud() const throw()
+            { return _baud; }
+
+        const char* what() throw();
+
+    protected:
+        /** Baud rate that caused the error. */
+        unsigned int _baud;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class BaudrateError
+
+
+/// Close error class
+class HOKUYO_AIST_EXPORT CloseError: public RuntimeError
+{
+    public:
+        CloseError()
+            : RuntimeError(3)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class CloseError
+
+
+/// No destination error class
+class HOKUYO_AIST_EXPORT NoDestinationError: public RuntimeError
+{
+    public:
+        NoDestinationError()
+            : RuntimeError(11)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class NoDestinationError
+
+
+/// Bad firmware error class
+class HOKUYO_AIST_EXPORT FirmwareError: public RuntimeError
+{
+    public:
+        FirmwareError()
+            : RuntimeError(23)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class FirmwareError
+
+
+/// SCIP version error class
+class HOKUYO_AIST_EXPORT ScipVersionError: public RuntimeError
+{
+    public:
+        ScipVersionError()
+            : RuntimeError(22)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ScipVersionError
+
+
+/// Unknown SCIP version error class
+class HOKUYO_AIST_EXPORT UnknownScipVersionError: public RuntimeError
+{
+    public:
+        UnknownScipVersionError()
+            : RuntimeError(4)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class UnknownScipVersionError
+
+
+/// Unsupported feature error class
+class HOKUYO_AIST_EXPORT UnsupportedError: public RuntimeError
+{
+    public:
+        /** @brief Unsupported error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        UnsupportedError(unsigned int desc_code)
+            : RuntimeError(desc_code)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class UnsupportedError
+
+
+/// Bad argument error class
+class HOKUYO_AIST_EXPORT ArgError: public RuntimeError
+{
+    public:
+        /** @brief Argument error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        ArgError(unsigned int desc_code)
+            : RuntimeError(desc_code)
+        {}
+        virtual ~ArgError() throw() {};
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ArgError
+
+
+/// No data error class
+class HOKUYO_AIST_EXPORT NoDataError: public RuntimeError
+{
+    public:
+        NoDataError()
+            : RuntimeError(13)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class NoDataError
+
+
+/// Not a serial connection error class
+class HOKUYO_AIST_EXPORT NotSerialError: public RuntimeError
+{
+    public:
+        NotSerialError()
+            : RuntimeError(5)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class NotSerialError
+
+
+/// Bad index error class
+class HOKUYO_AIST_EXPORT IndexError: public RuntimeError
+{
+    public:
+        IndexError()
+            : RuntimeError(2)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class IndexError
+
+
+/// Invalid motor speed error class
+class HOKUYO_AIST_EXPORT MotorSpeedError: public ArgError
+{
+    public:
+        MotorSpeedError()
+            : ArgError(9)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class MotorSpeedError
+
+
+/// Bad start step error class
+class HOKUYO_AIST_EXPORT StartStepError: public ArgError
+{
+    public:
+        StartStepError()
+            : ArgError(14)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class StartStepError
+
+
+/// Bad end step error class
+class HOKUYO_AIST_EXPORT EndStepError: public ArgError
+{
+    public:
+        EndStepError()
+            : ArgError(15)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class EndStepError
+
+
+/// Base protocol error
+class HOKUYO_AIST_EXPORT ProtocolError: public RuntimeError
+{
+    public:
+        /** @brief Protocol error constructor.
+
+        @param desc_code Index into the error descriptions string table. */
+        ProtocolError(unsigned int desc_code)
+            : RuntimeError(desc_code)
+        {}
+        virtual ~ProtocolError() throw() {}
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ProtocolError
+
+
+/// Bad checksum error
+class HOKUYO_AIST_EXPORT ChecksumError: public ProtocolError
+{
+    public:
+        /** @brief Checksum error constructor.
+
+        @param expected The expected checksum.
+        @param calculated The calculated checksum. */
+        ChecksumError(int expected, int calculated)
+            : ProtocolError(24), _expected(expected), _calculated(calculated)
+        {}
+
+        virtual int expected() const throw()
+            { return _expected; }
+
+        virtual int calculated() const throw()
+            { return _calculated; }
+
+        const char* what() throw();
+
+    protected:
+        /** Expected checksum value. */
+        int _expected;
+        /** Calculated checksum value. */
+        int _calculated;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ProtocolError
+
+
+/// Incorrect number of data sets read error
+class HOKUYO_AIST_EXPORT DataCountError: public ProtocolError
+{
+    public:
+        DataCountError()
+            : ProtocolError(25)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class DataCountError
+
+
+/// Misplaced line feed error
+class HOKUYO_AIST_EXPORT MisplacedLineFeedError: public ProtocolError
+{
+    public:
+        MisplacedLineFeedError()
+            : ProtocolError(26)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class MisplacedLineFeedError
+
+
+/// UnknownLine error
+class HOKUYO_AIST_EXPORT UnknownLineError: public ProtocolError
+{
+    public:
+        /** @brief Unknown line error constructor.
+
+        @param line The mystery line that was not understood. */
+        UnknownLineError(char const* const line);
+
+        virtual char const* const line() const throw()
+            { return _line; }
+
+        const char* what() throw();
+
+    protected:
+        /** The mystery line. */
+        char _line[128];
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class UnknownLineError
+
+
+/// Parse error
+class HOKUYO_AIST_EXPORT ParseError: public ProtocolError
+{
+    public:
+        /** @brief Parse error constructor.
+
+        @param line The line that could not be parsed.
+        @param type The type of line that was expected. */
+        ParseError(char const* const line, char const* const type);
+
+        virtual char const* const line() const throw()
+            { return _line; }
+
+        virtual char const* const type() const throw()
+            { return _type; }
+
+        const char* what() throw();
+
+    protected:
+        /** The bad line. */
+        char _line[128];
+        /** The type of line. */
+        char _type[16];
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ParseError
+
+
+/// Missing firmware specification error
+class HOKUYO_AIST_EXPORT MissingFirmSpecError: public ProtocolError
+{
+    public:
+        MissingFirmSpecError()
+            : ProtocolError(29)
+        {}
+
+    protected:
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class MissingFirmSpecError
+
+
+/// Bad response error
+class HOKUYO_AIST_EXPORT ResponseError: public ProtocolError
+{
+    public:
+        /** @brief Response error constructor.
+
+        @param line The two-byte error code received.
+        @param type The command that caused the error. */
+        ResponseError(char const* const error, char const* const cmd)
+            : ProtocolError(30)
+        {
+            _error[0] = error[0]; _error[1] = error[1];
+            _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
+        }
+
+        /// Get the two-byte error code as a non-null-terminated array.
+        virtual char const* const error_code() const throw()
+            { return _error; }
+
+        /// Get the two-byte command code as a non-null-terminated array.
+        virtual char const* const cmd_code() const throw()
+            { return _cmd; }
+
+        const char* what() throw();
+
+    protected:
+        /** Error code as defined in SCIP2 (two bytes). */
+        char _error[2];
+        /** Command that triggered the error, from SCIP2 (two bytes). */
+        char _cmd[2];
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ResponseError
+
+
+/// Bad response error (SCIP1 version)
+class HOKUYO_AIST_EXPORT Scip1ResponseError: public ProtocolError
+{
+    public:
+        /** @brief Response error constructor.
+
+        @param line The two-byte error code received.
+        @param type The command that caused the error. */
+        Scip1ResponseError(char error, char cmd)
+            : ProtocolError(30), _error(error), _cmd(cmd)
+        {}
+
+        /// Get the one-byte error code.
+        virtual char error_code() const throw()
+            { return _error; }
+
+        /// Get the one-byte command code.
+        virtual char cmd_code() const throw()
+            { return _cmd; }
+
+        const char* what() throw();
+
+    protected:
+        /** Error code as defined in SCIP2 (two bytes). */
+        char _error;
+        /** Command that triggered the error, from SCIP2 (two bytes). */
+        char _cmd;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class Scip1ResponseError
+
+
+/// Command echo error
+class HOKUYO_AIST_EXPORT CommandEchoError: public ProtocolError
+{
+    public:
+        /** @brief Command echo error constructor.
+
+        @param line The two-byte command code expected.
+        @param type The two-byte command echo received. */
+        CommandEchoError(char const* const cmd, char const* const echo)
+            : ProtocolError(31)
+        {
+            _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
+            _echo[0] = echo[0]; _echo[1] = echo[1];
+        }
+
+        /// Get the two-byte command code as a non-null-terminated array.
+        virtual char const* const cmd_code() const throw()
+            { return _cmd; }
+
+        /// Get the two-byte command echo as a non-null-terminated array.
+        virtual char const* const cmd_echo() const throw()
+            { return _echo; }
+
+        const char* what() throw();
+
+    protected:
+        /** Command that triggered the error, from SCIP2 (two bytes). */
+        char _cmd[2];
+        /** Received echo. */
+        char _echo[2];
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class CommandEchoError
+
+
+/// Parameter echo error
+class HOKUYO_AIST_EXPORT ParamEchoError: public ProtocolError
+{
+    public:
+        /** @brief Parameter echo error constructor.
+
+        @param type The two-byte command code sent. */
+        ParamEchoError(char const* const cmd)
+            : ProtocolError(32)
+        {
+            _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
+        }
+
+        /// Get the two-byte command code as a non-null-terminated array.
+        virtual char const* const cmd_code() const throw()
+            { return _cmd; }
+
+        const char* what() throw();
+
+    protected:
+        /** Command that triggered the error, from SCIP2 (two bytes). */
+        char _cmd[2];
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class ParamEchoError
+
+
+/// Insufficient bytes to calculate checksum error
+class HOKUYO_AIST_EXPORT InsufficientBytesError: public ProtocolError
+{
+    public:
+        /** @brief Insufficient bytes error constructor.
+
+        @param num The number of bytes received.
+        @param line_length The length of the line. */
+        InsufficientBytesError(int num, int line_length)
+            : ProtocolError(33), _num(num), _line_length(line_length)
+        {}
+
+        virtual int num() const throw()
+            { return _num; }
+
+        virtual int line_length() const throw()
+            { return _line_length; }
+
+        const char* what() throw();
+
+    protected:
+        /** Number of bytes available. */
+        int _num;
+        /** Length of the line. */
+        int _line_length;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class InsufficientBytesError
+
+
+/// Incorrect line length error
+class HOKUYO_AIST_EXPORT LineLengthError: public ProtocolError
+{
+    public:
+        /** @brief Line length error constructor.
+
+        @param num The number of bytes received.
+        @param line_length The length of the line. */
+        LineLengthError(int length, int expected)
+            : ProtocolError(34), _length(length), _expected(expected)
+        {}
+
+        virtual int length() const throw()
+            { return _length; }
+
+        virtual int expected() const throw()
+            { return _expected; }
+
+        const char* what() throw();
+
+    protected:
+        /** The received line length. */
+        int _length;
+        /** The expected line length. */
+        int _expected;
+        /** Static string representation of the error. */
+        static char const* _error_type;
+}; // class LineLengthError
+
+}; // namespace hokuyo_aist
+
+/** @} */
+
+#endif // HOKUYO_ERRORS_H__
+

Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-02 12:06:28 UTC (rev 499)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-04 00:21:24 UTC (rev 500)
@@ -23,6 +23,7 @@
 using namespace std;
 
 #include <hokuyo_aist/hokuyo_aist.h>
+#include <hokuyo_aist/hokuyo_errors.h>
 
 int main(int argc, char **argv)
 {
@@ -110,18 +111,18 @@
 		{
 			laser.set_baud (baud);
 		}
-		catch (hokuyo_aist::BaseError e)
+		catch (hokuyo_aist::BaudrateError &e)
 		{
-			cerr << "Failed to change baud rate: (" << e.code () << ") " << e.what () << endl;
+			cerr << "Failed to change baud rate: " << e.what () << endl;
 		}
 		// Set the motor speed
 		try
 		{
 			laser.set_motor_speed (speed);
 		}
-		catch (hokuyo_aist::BaseError e)
+		catch (hokuyo_aist::MotorSpeedError &e)
 		{
-			cerr << "Failed to set motor speed: (" << e.code () << ") " << e.what () << endl;
+			cerr << "Failed to set motor speed: " << e.what () << endl;
 		}
 
 		// Get some laser info
@@ -170,9 +171,9 @@
 		// Close the laser
 		laser.close ();
 	}
-	catch (hokuyo_aist::BaseError e)
+	catch (hokuyo_aist::BaseError &e)
 	{
-		cerr << "Caught exception: (" << e.code () << ") " << e.what () << endl;
+		cerr << "Caught exception: " << e.what () << endl;
 		return 1;
 	}
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 501
          http://gearbox.svn.sourceforge.net/gearbox/?rev=501&view=rev
Author:   gbiggs
Date:     2010-08-04 02:51:43 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Added ability to redirect error output

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-04 00:21:24 UTC (rev 500)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-04 02:51:43 UTC (rev 501)
@@ -274,30 +274,30 @@
 {
     std::stringstream ss;
 
-    ss << "Vendor: " << vendor << std::endl;
-    ss << "Product: " << product << std::endl;
-    ss << "Firmware: " << firmware << std::endl;
-    ss << "Protocol: " << protocol << std::endl;
-    ss << "Serial: " << serial << std::endl;
-    ss << "Model: " << model << std::endl;
+    ss << "Vendor: " << vendor << '\n';
+    ss << "Product: " << product << '\n';
+    ss << "Firmware: " << firmware << '\n';
+    ss << "Protocol: " << protocol << '\n';
+    ss << "Serial: " << serial << '\n';
+    ss << "Model: " << model << '\n';
 
     ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range <<
-        "mm" << std::endl;
+        "mm\n";
     ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
-        scanable_steps << std::endl;
+        scanable_steps << '\n';
     ss << "First step: " << first_step << "\tFront step: " << front_step <<
-        "\tLast step: " << last_step << std::endl;
-    ss << "Resolution: " << resolution << " radians/step" << std::endl;
+        "\tLast step: " << last_step << '\n';
+    ss << "Resolution: " << resolution << " radians/step\n";
     ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
-        max_angle << " radians" << std::endl;
-    ss << "Standard motor speed: " << standard_speed << "rpm" << std::endl;
+        max_angle << " radians\n";
+    ss << "Standard motor speed: " << standard_speed << "rpm\n";
 
     ss << "Power status: " << (power ? "On" : "Off") <<
-        "\tMeasurement state: " << measure_state << std::endl;
+        "\tMeasurement state: " << measure_state << '\n';
     ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
-        ")\tBaud rate: " << baud << "bps" << std::endl;
-    ss << "Time stamp: " << time << "ms" << std::endl;
-    ss << "Sensor diagnostic: " << sensor_diagnostic << std::endl;
+        ")\tBaud rate: " << baud << "bps\n";
+    ss << "Time stamp: " << time << "ms\n";
+    ss << "Sensor diagnostic: " << sensor_diagnostic << '\n';
 
     return ss.str();
 }
@@ -557,29 +557,29 @@
 {
     std::stringstream ss;
 
-    ss << _length << " readings:" << std::endl;
+    ss << _length << " readings:\n";
     for (unsigned int ii = 0; ii < _length; ii++)
         ss << _ranges[ii] << "\t";
     if (_intensities != NULL)
     {
-        ss << std::endl << _length << " intensities:" << std::endl;
+        ss << std::endl << _length << " intensities:\n";
         for (unsigned int ii = 0; ii < _length; ii++)
             ss << _intensities[ii] << "\t";
     }
-    ss << std::endl;
+    ss << '\n';
     if (_error)
     {
-        ss << "Detected data errors:" << std::endl;
+        ss << "Detected data errors:\n";
         for (unsigned int ii = 0; ii < _length; ii++)
         {
             if (_ranges[ii] < 20)
-                ss << ii << ": " << error_code_to_string(_ranges[ii]) <<
-                    std::endl;
+                ss << ii << ": " <<
+                    error_code_to_string(_ranges[ii]) << '\n';
         }
     }
     else
-        ss << "No data errors." << std::endl;
-    ss << "Time stamp: " << _time << std::endl;
+        ss << "No data errors.\n";
+    ss << "Time stamp: " << _time << '\n';
 
     return ss.str();
 }
@@ -662,13 +662,21 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 Sensor::Sensor()
-    : _port(NULL), _scip_version(2), _verbose(false), _sensorIsUTM30LX(false),
-    _enable_checksum_workaround(false), _ignore_unknowns(false), _min_angle(0.0),
-    _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
-    _front_step(0)
+    : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
+    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
 {
 }
 
+Sensor::Sensor(std::ostream& err_output)
+    : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
+    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
+{
+}
+
 Sensor::~Sensor()
 {
     if (_port != NULL)
@@ -679,18 +687,18 @@
 {
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ <<
-            "() Creating and opening port using options: " << port_options <<
-            std::endl;
+        _err_output << "Sensor::" << __func__ <<
+            "() Creating and opening port using options: " <<
+            port_options << '\n';
     }
     _port = flexiport::CreatePort(port_options);
     _port->Open();
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection." << std::endl;
-        std::cerr << _port->GetStatus();
+        _err_output << "Sensor::" << __func__ << "() Connected using " <<
+            _port->GetPortType() << " connection.\n";
+        _err_output << _port->GetStatus();
     }
     _port->Flush();
 
@@ -705,18 +713,18 @@
 {
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ <<
+        _err_output << "Sensor::" << __func__ <<
             "() Creating and opening port using options: " << port_options <<
-            std::endl;
+            '\n';
     }
     _port = flexiport::CreatePort(port_options);
     _port->Open();
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection." << std::endl;
-        std::cerr << _port->GetStatus();
+        _err_output << "Sensor::" << __func__ << "() Connected using " <<
+            _port->GetPortType() << " connection.\n";
+        _err_output << _port->GetStatus();
     }
     _port->Flush();
 
@@ -732,8 +740,8 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
-                "() Failed to connect at the default baud rate." << std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Failed to connect at the default baud rate.\n";
         }
         if (_port->GetPortType() == "serial")
         {
@@ -753,8 +761,8 @@
                     // loop and be happy
                     if (_verbose)
                     {
-                        std::cerr << "Sensor::" << __func__ <<
-                            "() Connected at " << bauds[ii] << std::endl;
+                        _err_output << "Sensor::" << __func__ <<
+                            "() Connected at " << bauds[ii] << '\n';
                     }
                     return bauds[ii];
                 }
@@ -765,9 +773,8 @@
                         // Last baud rate, give up and rethrow
                         if (_verbose)
                         {
-                            std::cerr << "Sensor::" << __func__ <<
-                                "() Failed to connect at any baud rate." <<
-                                std::endl;
+                            _err_output << "Sensor::" << __func__ <<
+                                "() Failed to connect at any baud rate.\n";
                         }
                         throw;
                     }
@@ -779,8 +786,8 @@
         {
             if (_verbose)
             {
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Port is not serial, cannot probe." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Port is not serial, cannot probe.\n";
             }
             throw;
         }
@@ -797,8 +804,7 @@
     if (!_port)
         throw CloseError();
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Closing connection." <<
-            std::endl;
+        _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
     delete _port;
     _port = NULL;
 }
@@ -817,15 +823,15 @@
         if (on)
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Turning laser on." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Turning laser on.\n";
             _send_command("L", "1", 1, NULL);
         }
         else
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Turning laser off." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Turning laser off.\n";
             _send_command("L", "0", 1, NULL);
         }
         _skip_lines(1);
@@ -835,15 +841,15 @@
         if (on)
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Turning laser on." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Turning laser on.\n";
             _send_command("BM", NULL, 0, "02");
         }
         else
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Turning laser off." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Turning laser off.\n";
             _send_command("QT", NULL, 0, "02");
         }
         _skip_lines(1);
@@ -898,8 +904,8 @@
     else if (_scip_version == 2)
     {
         if (_verbose)
-            std::cerr << "Sensor::" << __func__ <<
-                "() Resetting laser." << std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Resetting laser.\n";
         _send_command("RS", NULL, 0, NULL);
         _skip_lines(1);
     }
@@ -921,8 +927,8 @@
         {
             if (_verbose)
             {
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Reseting motor speed to default." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Reseting motor speed to default.\n";
             }
             buffer[0] = '0';
             buffer[1] = '0';
@@ -932,8 +938,8 @@
         {
             if (_verbose)
             {
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Reseting motor speed to default." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Reseting motor speed to default.\n";
             }
             buffer[0] = '9';
             buffer[1] = '9';
@@ -943,8 +949,8 @@
         {
             if (_verbose)
             {
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Setting motor speed to ratio " << speed << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Setting motor speed to ratio " << speed << '\n';
             }
             number_to_string(speed, buffer, 2);
         }
@@ -964,16 +970,16 @@
         if (on)
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Switching to high sensitivity." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Switching to high sensitivity.\n";
             _send_command("HS", "1", 1, "02");
         }
         else
         {
             if (_verbose)
             {
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Switching to normal sensitivity." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Switching to normal sensitivity.\n";
             }
             _send_command("HS", "0", 1, "02");
         }
@@ -992,9 +998,8 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
-                "() Getting sensor information using SCIP version 1." <<
-                std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Getting sensor information using SCIP version 1.\n";
         }
 
         info->set_defaults();
@@ -1035,8 +1040,8 @@
         if (atoi(info->firmware.c_str()) >= 3)
         {
             if (_verbose)
-                std::cerr << "SCIP1 Firmware line for parsing: " <<
-                    info->firmware << std::endl;
+                _err_output << "SCIP1 Firmware line for parsing: " <<
+                    info->firmware << '\n';
             // Now the fun part: parsing the line. It would be nice if we could
             // use the POSIX regex functions, but since MS doesn't believe in
             // POSIX we get to do it the hard way.
@@ -1062,9 +1067,9 @@
                 info->calculate_values();
                 if (_verbose)
                 {
-                    std::cerr << "Retrieved sensor info (hard-coded, not "
-                        "enough values):" << std::endl;
-                    std::cerr << info->as_string();
+                    _err_output << "Retrieved sensor info (hard-coded, not "
+                        "enough values):\n";
+                    _err_output << info->as_string();
                 }
             }
             else
@@ -1084,9 +1089,8 @@
 
                 if (_verbose)
                 {
-                    std::cerr << "Retrieved sensor info (from FIRM line):" <<
-                        std::endl;
-                    std::cerr << info->as_string();
+                    _err_output << "Retrieved sensor info (from FIRM line):\n";
+                    _err_output << info->as_string();
                 }
             }
         }
@@ -1097,9 +1101,8 @@
             info->calculate_values();
             if (_verbose)
             {
-                std::cerr << "Retrieved sensor info (hard-coded):" <<
-                    std::endl;
-                std::cerr << info->as_string();
+                _err_output << "Retrieved sensor info (hard-coded):\n";
+                _err_output << info->as_string();
             }
         }
     }
@@ -1107,9 +1110,8 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
-                "() Getting sensor information using SCIP version 2." <<
-                std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Getting sensor information using SCIP version 2.\n";
         }
 
         info->set_defaults();
@@ -1138,8 +1140,8 @@
         info->calculate_values();
         if (_verbose)
         {
-            std::cerr << "Retrieved sensor info:" << std::endl;
-            std::cerr << info->as_string();
+            _err_output << "Retrieved sensor info:\n";
+            _err_output << info->as_string();
         }
     }
     else
@@ -1153,8 +1155,8 @@
     else if (_scip_version == 2)
     {
         if (_verbose)
-            std::cerr << "Sensor::" << __func__ <<
-                "() Retrieving time from laser." << std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Retrieving time from laser.\n";
         _send_command("TM", "0", 1, NULL);
         _send_command("TM", "1", 1, NULL);
         char buffer[7];
@@ -1187,10 +1189,10 @@
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Reading " <<
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges between " << start_step << " and " <<
             end_step << " with a cluster count of " << cluster_count <<
-            std::endl;
+            '\n';
     }
 
     if (_scip_version == 1)
@@ -1247,9 +1249,9 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+        _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << std::endl;
+            end_angle << " is step " << end_step << '\n';
     }
 
     // Get the data
@@ -1277,10 +1279,10 @@
         unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ << "() Reading " <<
+            _err_output << "Sensor::" << __func__ << "() Reading " <<
                 num_steps << " new ranges between " << start_step << " and " <<
                 end_step << " with a cluster count of " << cluster_count <<
-                std::endl;
+                '\n';
         }
 
         // Send the command to ask for the most recent range data from start_step to end_step
@@ -1307,9 +1309,9 @@
         _read_line_with_check(response, 4);
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() MD data prefix status: " << response[0] << response[1] <<
-                std::endl;
+                '\n';
         }
         // Check the status code is OK - should only get 99 here
         if (response[0] != '9' || response[1] != '9')
@@ -1358,9 +1360,9 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+        _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << std::endl;
+            end_angle << " is step " << end_step << '\n';
     }
 
     // Get the data
@@ -1388,10 +1390,10 @@
         unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ << "() Reading " <<
+            _err_output << "Sensor::" << __func__ << "() Reading " <<
                 num_steps << " new ranges between " << start_step << " and " <<
                 end_step << " with a cluster count of " << cluster_count <<
-                std::endl;
+                '\n';
         }
 
         // Send the command to ask for the most recent range data with
@@ -1418,9 +1420,9 @@
         _read_line_with_check(response, 4);
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() ME data prefix status: " << response[0] << response[1] <<
-                std::endl;
+                '\n';
         }
         // Check the status code is OK - should only get 99 here
         if (response[0] != '9' || response[1] != '9')
@@ -1469,9 +1471,9 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Start angle " <<
+        _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << std::endl;
+            end_angle << " is step " << end_step << '\n';
     }
 
     // Get the data
@@ -1535,8 +1537,8 @@
             SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ << "() Reading up to " <<
-                maxlength << " bytes." << std::endl;
+            _err_output << "Sensor::" << __func__ << "() Reading up to " <<
+                maxlength << " bytes.\n";
         }
         // We need to get at least 1 byte in a line: the line feed.
         if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
@@ -1550,9 +1552,8 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
-                "() Reading exactly " << expected_length << " bytes." <<
-                std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Reading exactly " << expected_length << " bytes.\n";
         }
         // expected_length+1 for the NULL
         if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
@@ -1567,10 +1568,10 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Read " << linelength <<
-            " bytes." << std::endl;
-        std::cerr << "Sensor::" << __func__ << "() Line is " << buffer <<
-            std::endl;
+        _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
+            " bytes.\n";
+        _err_output << "Sensor::" << __func__ << "() Line is " << buffer <<
+            '\n';
     }
     return linelength - 1; // Line feed not included
 }
@@ -1613,9 +1614,9 @@
     int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Considering " <<
+        _err_output << "Sensor::" << __func__ << "() Considering " <<
             bytesToConsider << " bytes for checksum from a line length of " <<
-            linelength << " bytes." << std::endl;
+            linelength << " bytes.\n";
     }
     if (bytesToConsider < 1)
         throw InsufficientBytesError(bytesToConsider, linelength);
@@ -1637,10 +1638,10 @@
                 int newBytesToConsider = hasComment - buffer;
                 if (_verbose)
                 {
-                    std::cerr << "Sensor::" << __func__ <<
+                    _err_output << "Sensor::" << __func__ <<
                         "() Performing UTM-30LX II response "
                         "checksum workaround: trying with " <<
-                        newBytesToConsider << " bytes." << std::endl;
+                        newBytesToConsider << " bytes.\n";
                 }
                 if (newBytesToConsider < 1)
                 {
@@ -1670,8 +1671,8 @@
 void Sensor::_skip_lines(int count)
 {
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Skipping " << count <<
-            " lines." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
+            " lines.\n";
     if (_port->SkipUntil(0x0A, count) < 0)
         throw ReadError(18);
 }
@@ -1698,9 +1699,9 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() Writing in SCIP1 mode. Command is " << cmd[0] <<
-                ", parameters length is " << param_length << std::endl;
+                ", parameters length is " << param_length << '\n';
         }
         // Write the command
         if (_port->Write(cmd, 1) < 1)
@@ -1744,9 +1745,9 @@
         // Next up, check the status byte
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() Command response status: " << response[statusIndex] <<
-                std::endl;
+                '\n';
         }
         if (response[statusIndex] != '0')
         {
@@ -1775,9 +1776,9 @@
     {
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() Writing in SCIP2 mode. Command is " << cmd <<
-                ", parameters length is " << param_length << std::endl;
+                ", parameters length is " << param_length << '\n';
         }
         // Write the command
         if (_port->Write(cmd, 2) < 2)
@@ -1806,9 +1807,9 @@
         _read_line_with_check(response, 4);
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() Command response status: " << response[0] << response[1] <<
-                std::endl;
+                '\n';
         }
         // Check the status code is OK
         response[2] = '\0';
@@ -1848,8 +1849,8 @@
 
 
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ <<
-            "() Testing SCIP protocol version." << std::endl;
+        _err_output << "Sensor::" << __func__ <<
+            "() Testing SCIP protocol version.\n";
     // Try SCIP version 2 first by sending an info command
     try
     {
@@ -1859,8 +1860,8 @@
     {
         // That didn't work too well...
         if (_verbose)
-            std::cerr << "Sensor::" << __func__ <<
-                "() Initial SCIP version 2 test failed." << std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Initial SCIP version 2 test failed.\n";
         scip2Failed = true;
     }
 
@@ -1898,9 +1899,9 @@
             throw FirmwareError();
         if (_verbose)
         {
-            std::cerr << "Sensor::" << __func__ <<
+            _err_output << "Sensor::" << __func__ <<
                 "() Firmware major version is " << majorVer <<
-                std::endl;
+                '\n';
         }
         // Dump the rest of the V command result (one of these will be the
         // empty last line)
@@ -1911,9 +1912,9 @@
         if (majorVer < 3)
         {
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
+                _err_output << "Sensor::" << __func__ <<
                     "() Firmware does not support SCIP version 2; using SCIP "
-                    "version 1." << std::endl;
+                    "version 1.\n";
             return;
         }
         // Otherwise we can try SCIP version 2
@@ -1932,9 +1933,9 @@
             catch (BaseError)
             {
                 if (_verbose)
-                    std::cerr << "Sensor::" << __func__ <<
+                    _err_output << "Sensor::" << __func__ <<
                         "() Could not change to SCIP version 2; using SCIP "
-                        "version 1." << std::endl;
+                        "version 1.\n";
                 return;
             }
             // There'll be a trailing line on the end
@@ -1942,8 +1943,8 @@
 
             // Changed to SCIP version 2
             if (_verbose)
-                std::cerr << "Sensor::" << __func__ <<
-                    "() Using SCIP version 2." << std::endl;
+                _err_output << "Sensor::" << __func__ <<
+                    "() Using SCIP version 2.\n";
             _scip_version = 2;
             return;
         }
@@ -1956,8 +1957,8 @@
         // Dump the rest of the result
         _skip_lines(6);
         if (_verbose)
-            std::cerr << "Sensor::" << __func__ <<
-                "() Using SCIP version 2." << std::endl;
+            _err_output << "Sensor::" << __func__ <<
+                "() Using SCIP version 2.\n";
         return;
     }
 
@@ -1968,8 +1969,8 @@
 void Sensor::_get_defaults()
 {
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ <<
-            "() Getting default values." << std::endl;
+        _err_output << "Sensor::" << __func__ <<
+            "() Getting default values.\n";
 
     // Get the laser's info
     SensorInfo info;
@@ -1984,10 +1985,10 @@
     _max_range = info.max_range;
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ <<
+        _err_output << "Sensor::" << __func__ <<
             "() Got default values: " << _min_angle << " " << _max_angle <<
             " " << _resolution << " " << _first_step << " " << _last_step <<
-            " " << _front_step << " " << _max_range << std::endl;
+            " " << _front_step << " " << _max_range << '\n';
     }
 }
 
@@ -2103,8 +2104,8 @@
 void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
 {
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " ranges.\n";
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
@@ -2139,8 +2140,8 @@
     }
 
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Read " <<
+            currentStep << " ranges.\n";
     if (currentStep != num_steps)
         throw DataCountError();
 }
@@ -2148,8 +2149,8 @@
 void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
 {
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " ranges.\n";
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
@@ -2217,16 +2218,16 @@
                 }
                 if (data->_ranges[currentStep] > _max_range)
                 {
-                    std::cerr << "WARNING: Sensor::" << __func__ <<
+                    _err_output << "WARNING: Sensor::" << __func__ <<
                         "() Value at step " << currentStep <<
                         " beyond maximum range: " <<
                         data->_ranges[currentStep] << " (raw bytes: ";
                     if (splitCount != 0)
-                        std::cerr << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")" << std::endl;
+                        _err_output << splitValue[0] << splitValue[1] <<
+                            splitValue[2] << ")\n";
                     else
-                        std::cerr << buffer[0] << buffer[1] << buffer[2] <<
-                            ")" << std::endl;
+                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
+                            ")\n";
                 }
                 else if (data->_ranges[currentStep] < 20)
                     data->_error = true;
@@ -2238,8 +2239,8 @@
     }
 
     if (_verbose)
-        std::cerr << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Read " <<
+            currentStep << " ranges.\n";
     if (currentStep != num_steps)
         throw DataCountError();
 }
@@ -2249,8 +2250,8 @@
 {
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges and intensities." << std::endl;
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " ranges and intensities.\n";
     }
 
     // This will automatically take care of whether it actually needs to
@@ -2332,16 +2333,16 @@
                 if (data->_ranges[currentRange] > _max_range &&
                         !nextIsIntensity)
                 {
-                    std::cerr << "WARNING: Sensor::" << __func__ <<
+                    _err_output << "WARNING: Sensor::" << __func__ <<
                         "() Value at step " << currentRange <<
                         " beyond maximum range: " <<
                         data->_ranges[currentRange] << " (raw bytes: ";
                     if (splitCount != 0)
-                        std::cerr << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")" << std::endl;
+                        _err_output << splitValue[0] << splitValue[1] <<
+                            splitValue[2] << ")\n";
                     else
-                        std::cerr << buffer[0] << buffer[1] << buffer[2] <<
-                            ")" << std::endl;
+                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
+                            ")\n";
                 }
                 else if (data->_ranges[currentRange] < 20)
                     data->_error = true;
@@ -2360,9 +2361,9 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ << "() Read " <<
+        _err_output << "Sensor::" << __func__ << "() Read " <<
             currentRange << " ranges and " << currentIntensity <<
-            " intensities (expected " << num_steps << ")." << std::endl;
+            " intensities (expected " << num_steps << ").\n";
     }
     if (currentRange != num_steps || currentIntensity != num_steps)
         throw DataCountError();
@@ -2382,11 +2383,11 @@
 
     if (_verbose)
     {
-        std::cerr << "Sensor::" << __func__ <<
+        _err_output << "Sensor::" << __func__ <<
             "() Calculated checksum = " << checksum << " (" <<
             static_cast<char>(checksum) << "), given checksum = " <<
             expected_sum << " (" << static_cast<char> (expected_sum) <<
-            ")" << std::endl;
+            ")\n";
     }
     if (checksum != expected_sum)
         throw ChecksumError(expected_sum, checksum);

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 00:21:24 UTC (rev 500)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 02:51:43 UTC (rev 501)
@@ -280,6 +280,7 @@
 {
     public:
         Sensor();
+        Sensor(std::ostream& err_output);
         ~Sensor();
 
         /// @brief Open the laser scanner and begin scanning.
@@ -506,6 +507,7 @@
 
     private:
         flexiport::Port *_port;
+        std::ostream& _err_output;
 
         uint8_t _scip_version;
         bool _verbose, _sensorIsUTM30LX, _enable_checksum_workaround,


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 502
          http://gearbox.svn.sourceforge.net/gearbox/?rev=502&view=rev
Author:   gbiggs
Date:     2010-08-04 03:20:45 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Fixed polymorphism in exceptions

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 02:51:43 UTC (rev 501)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 03:20:45 UTC (rev 502)
@@ -302,6 +302,20 @@
 }
 
 
+BaseError::BaseError(unsigned int desc_code, char const* error_type)
+    : _desc_code(desc_code)
+{
+    strncpy(_error_type, error_type, 32);
+}
+
+
+BaseError::BaseError(BaseError const& rhs)
+    : _desc_code(rhs.desc_code())
+{
+    strncpy(_error_type, rhs.error_type(), 32);
+}
+
+
 const char* BaseError::what() throw()
 {
     ss << _error_type << " (" << _desc_code << "): " <<
@@ -327,12 +341,19 @@
 
 
 UnknownLineError::UnknownLineError(char const* const line)
-    : ProtocolError(27)
+    : ProtocolError(27, "UnknownLineError")
 {
     strncpy(_line, line, 128);
 }
 
 
+UnknownLineError::UnknownLineError(UnknownLineError const& rhs)
+    : ProtocolError(rhs)
+{
+    strncpy(_line, rhs.line(), 128);
+}
+
+
 const char* UnknownLineError::what() throw()
 {
     ProtocolError::what();
@@ -342,13 +363,21 @@
 
 
 ParseError::ParseError(char const* const line, char const* const type)
-    : ProtocolError(28)
+    : ProtocolError(28, "ParseError")
 {
     strncpy(_line, line, 128);
     strncpy(_type, type, 16);
 }
 
 
+ParseError::ParseError(ParseError const& rhs)
+    : ProtocolError(rhs)
+{
+    strncpy(_line, rhs.line(), 128);
+    strncpy(_type, rhs.type(), 16);
+}
+
+
 const char* ParseError::what() throw()
 {
     ProtocolError::what();
@@ -410,41 +439,5 @@
     return ss.str().c_str();
 }
 
-
-// Static member initialisation
-
-char const* BaseError::_error_type = "BaseError";
-char const* LogicError::_error_type = "LogicError";
-char const* RuntimeError::_error_type = "RuntimeError";
-char const* ReadError::_error_type = "ReadError";
-char const* WriteError::_error_type = "WriteError";
-char const* BaudrateError::_error_type = "BaudrateError";
-char const* CloseError::_error_type = "CloseError";
-char const* NoDestinationError::_error_type = "NoDestinationError";
-char const* FirmwareError::_error_type = "FirmwareError";
-char const* ScipVersionError::_error_type = "ScipVersionError";
-char const* UnknownScipVersionError::_error_type = "UnknownScipVersionError";
-char const* UnsupportedError::_error_type = "UnsupportedError";
-char const* ArgError::_error_type = "ArgError";
-char const* NoDataError::_error_type = "NoDataError";
-char const* NotSerialError::_error_type = "NotSerialError";
-char const* IndexError::_error_type = "IndexError";
-char const* MotorSpeedError::_error_type = "MotorSpeedError";
-char const* StartStepError::_error_type = "StartStepError";
-char const* EndStepError::_error_type = "EndStepError";
-char const* ProtocolError::_error_type = "ProtocolError";
-char const* ChecksumError::_error_type = "ChecksumError";
-char const* DataCountError::_error_type = "DataCountError";
-char const* MisplacedLineFeedError::_error_type = "MisplacedLineFeedError";
-char const* UnknownLineError::_error_type = "UnknownLineError";
-char const* ParseError::_error_type = "ParseError";
-char const* MissingFirmSpecError::_error_type = "MissingFirmSpecError";
-char const* ResponseError::_error_type = "ResponseError";
-char const* Scip1ResponseError::_error_type = "Scip1ResponseError";
-char const* CommandEchoError::_error_type = "CommandEchoError";
-char const* ParamEchoError::_error_type = "ParamEchoError";
-char const* InsufficientBytesError::_error_type = "InsufficientBytesError";
-char const* LineLengthError::_error_type = "LineLengthError";
-
 }; // namespace hokuyo_aist
 

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 02:51:43 UTC (rev 501)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 03:20:45 UTC (rev 502)
@@ -59,17 +59,16 @@
         /** @brief Hokuyo error constructor.
 
         @param desc_code Index into the error descriptions string table. */
-        BaseError(unsigned int desc_code)
-            : _desc_code(desc_code)
-        {}
-        BaseError(BaseError const& rhs)
-            : _desc_code(rhs.desc_code())
-        {}
+        BaseError(unsigned int desc_code, char const* error_type);
+        BaseError(BaseError const& rhs);
         virtual ~BaseError() throw() {};
 
         virtual unsigned int desc_code() const throw()
             { return _desc_code; }
 
+        virtual char const* error_type() const throw()
+            { return _error_type; }
+
         virtual const char* what() throw();
 
     protected:
@@ -79,7 +78,7 @@
         /** Formatted description of the error. */
         std::stringstream ss;
         /** Static string representation of the error. */
-        static char const* _error_type;
+        char _error_type[32];
 }; //class BaseError
 
 
@@ -91,13 +90,12 @@
 
         @param desc_code Index into the error descriptions string table. */
         LogicError(unsigned int desc_code)
-            : BaseError(desc_code)
+            : BaseError(desc_code, "LogicError")
         {}
+        LogicError(unsigned int desc_code, char const* error_type)
+            : BaseError(desc_code, error_type)
+        {}
         virtual ~LogicError() throw() {};
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class LogicError
 
 
@@ -109,8 +107,11 @@
 
         @param desc_code Index into the error descriptions string table. */
         RuntimeError(unsigned int desc_code)
-            : BaseError(desc_code)
+            : BaseError(desc_code, "RuntimeError")
         {}
+        RuntimeError(unsigned int desc_code, char const* error_type)
+            : BaseError(desc_code, error_type)
+        {}
         virtual ~RuntimeError() throw() {};
 
     protected:
@@ -127,12 +128,8 @@
 
         @param desc_code Index into the error descriptions string table. */
         ReadError(unsigned int desc_code)
-            : RuntimeError(desc_code)
+            : RuntimeError(desc_code, "ReadError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ReadError
 
 
@@ -144,12 +141,8 @@
 
         @param desc_code Index into the error descriptions string table. */
         WriteError(unsigned int desc_code)
-            : RuntimeError(desc_code)
+            : RuntimeError(desc_code, "WriteError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class WriteError
 
 
@@ -161,8 +154,11 @@
 
         @param baud The bad baud rate. */
         BaudrateError(unsigned int baud)
-            : RuntimeError(6), _baud(baud)
+            : RuntimeError(6, "BaudrateError"), _baud(baud)
         {}
+        BaudrateError(BaudrateError const& rhs)
+            : RuntimeError(rhs), _baud(rhs.baud())
+        {}
 
         unsigned int baud() const throw()
             { return _baud; }
@@ -172,8 +168,6 @@
     protected:
         /** Baud rate that caused the error. */
         unsigned int _baud;
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class BaudrateError
 
 
@@ -182,12 +176,8 @@
 {
     public:
         CloseError()
-            : RuntimeError(3)
+            : RuntimeError(3, "CloseError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class CloseError
 
 
@@ -196,12 +186,8 @@
 {
     public:
         NoDestinationError()
-            : RuntimeError(11)
+            : RuntimeError(11, "NoDestinationError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class NoDestinationError
 
 
@@ -210,12 +196,8 @@
 {
     public:
         FirmwareError()
-            : RuntimeError(23)
+            : RuntimeError(23, "FirmwareError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class FirmwareError
 
 
@@ -224,12 +206,8 @@
 {
     public:
         ScipVersionError()
-            : RuntimeError(22)
+            : RuntimeError(22, "ScipVersionError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ScipVersionError
 
 
@@ -238,12 +216,8 @@
 {
     public:
         UnknownScipVersionError()
-            : RuntimeError(4)
+            : RuntimeError(4, "UnknownScipVersionError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class UnknownScipVersionError
 
 
@@ -255,12 +229,8 @@
 
         @param desc_code Index into the error descriptions string table. */
         UnsupportedError(unsigned int desc_code)
-            : RuntimeError(desc_code)
+            : RuntimeError(desc_code, "UnsupportedError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class UnsupportedError
 
 
@@ -272,13 +242,12 @@
 
         @param desc_code Index into the error descriptions string table. */
         ArgError(unsigned int desc_code)
-            : RuntimeError(desc_code)
+            : RuntimeError(desc_code, "ArgError")
         {}
+        ArgError(unsigned int desc_code, char const* error_type)
+            : RuntimeError(desc_code, error_type)
+        {}
         virtual ~ArgError() throw() {};
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ArgError
 
 
@@ -287,12 +256,8 @@
 {
     public:
         NoDataError()
-            : RuntimeError(13)
+            : RuntimeError(13, "NoDataError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class NoDataError
 
 
@@ -301,12 +266,8 @@
 {
     public:
         NotSerialError()
-            : RuntimeError(5)
+            : RuntimeError(5, "NotSerialError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class NotSerialError
 
 
@@ -315,12 +276,8 @@
 {
     public:
         IndexError()
-            : RuntimeError(2)
+            : RuntimeError(2, "IndexError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class IndexError
 
 
@@ -329,12 +286,8 @@
 {
     public:
         MotorSpeedError()
-            : ArgError(9)
+            : ArgError(9, "MotorSpeedError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class MotorSpeedError
 
 
@@ -343,12 +296,8 @@
 {
     public:
         StartStepError()
-            : ArgError(14)
+            : ArgError(14, "StartStepError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class StartStepError
 
 
@@ -357,12 +306,8 @@
 {
     public:
         EndStepError()
-            : ArgError(15)
+            : ArgError(15, "EndStepError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class EndStepError
 
 
@@ -374,12 +319,12 @@
 
         @param desc_code Index into the error descriptions string table. */
         ProtocolError(unsigned int desc_code)
-            : RuntimeError(desc_code)
+            : RuntimeError(desc_code, "ProtocolError")
         {}
+        ProtocolError(unsigned int desc_code, char const* error_type)
+            : RuntimeError(desc_code, error_type)
+        {}
         virtual ~ProtocolError() throw() {}
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ProtocolError
 
 
@@ -392,8 +337,13 @@
         @param expected The expected checksum.
         @param calculated The calculated checksum. */
         ChecksumError(int expected, int calculated)
-            : ProtocolError(24), _expected(expected), _calculated(calculated)
+            : ProtocolError(24, "ChecksumError"), _expected(expected),
+            _calculated(calculated)
         {}
+        ChecksumError(ChecksumError const& rhs)
+            : ProtocolError(rhs), _expected(rhs.expected()),
+            _calculated(rhs.calculated())
+        {}
 
         virtual int expected() const throw()
             { return _expected; }
@@ -408,8 +358,6 @@
         int _expected;
         /** Calculated checksum value. */
         int _calculated;
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ProtocolError
 
 
@@ -418,12 +366,8 @@
 {
     public:
         DataCountError()
-            : ProtocolError(25)
+            : ProtocolError(25, "DataCountError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class DataCountError
 
 
@@ -432,12 +376,8 @@
 {
     public:
         MisplacedLineFeedError()
-            : ProtocolError(26)
+            : ProtocolError(26, "MisplacedLineFeedError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class MisplacedLineFeedError
 
 
@@ -449,6 +389,7 @@
 
         @param line The mystery line that was not understood. */
         UnknownLineError(char const* const line);
+        UnknownLineError(UnknownLineError const& rhs);
 
         virtual char const* const line() const throw()
             { return _line; }
@@ -458,8 +399,6 @@
     protected:
         /** The mystery line. */
         char _line[128];
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class UnknownLineError
 
 
@@ -472,6 +411,7 @@
         @param line The line that could not be parsed.
         @param type The type of line that was expected. */
         ParseError(char const* const line, char const* const type);
+        ParseError(ParseError const& rhs);
 
         virtual char const* const line() const throw()
             { return _line; }
@@ -486,8 +426,6 @@
         char _line[128];
         /** The type of line. */
         char _type[16];
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ParseError
 
 
@@ -496,12 +434,8 @@
 {
     public:
         MissingFirmSpecError()
-            : ProtocolError(29)
+            : ProtocolError(29, "MissingFirmSpecError")
         {}
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class MissingFirmSpecError
 
 
@@ -514,11 +448,19 @@
         @param line The two-byte error code received.
         @param type The command that caused the error. */
         ResponseError(char const* const error, char const* const cmd)
-            : ProtocolError(30)
+            : ProtocolError(30, "ResponseError")
         {
             _error[0] = error[0]; _error[1] = error[1];
             _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
         }
+        ResponseError(ResponseError const& rhs)
+            : ProtocolError(rhs)
+        {
+            _error[0] = rhs.error_code()[0];
+            _error[1] = rhs.error_code()[1];
+            _cmd[0] = rhs.cmd_code()[0];
+            _cmd[1] = rhs.cmd_code()[1];
+        }
 
         /// Get the two-byte error code as a non-null-terminated array.
         virtual char const* const error_code() const throw()
@@ -535,8 +477,6 @@
         char _error[2];
         /** Command that triggered the error, from SCIP2 (two bytes). */
         char _cmd[2];
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ResponseError
 
 
@@ -549,8 +489,13 @@
         @param line The two-byte error code received.
         @param type The command that caused the error. */
         Scip1ResponseError(char error, char cmd)
-            : ProtocolError(30), _error(error), _cmd(cmd)
+            : ProtocolError(30, "Scip1ResponseError"),
+            _error(error), _cmd(cmd)
         {}
+        Scip1ResponseError(Scip1ResponseError const& rhs)
+            : ProtocolError(rhs), _error(rhs.error_code()),
+            _cmd(rhs.cmd_code())
+        {}
 
         /// Get the one-byte error code.
         virtual char error_code() const throw()
@@ -567,8 +512,6 @@
         char _error;
         /** Command that triggered the error, from SCIP2 (two bytes). */
         char _cmd;
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class Scip1ResponseError
 
 
@@ -581,11 +524,19 @@
         @param line The two-byte command code expected.
         @param type The two-byte command echo received. */
         CommandEchoError(char const* const cmd, char const* const echo)
-            : ProtocolError(31)
+            : ProtocolError(31, "CommandEchoError")
         {
             _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
             _echo[0] = echo[0]; _echo[1] = echo[1];
         }
+        CommandEchoError(CommandEchoError const& rhs)
+            : ProtocolError(rhs)
+        {
+            _cmd[0] = rhs.cmd_code()[0];
+            _cmd[1] = rhs.cmd_code()[1];
+            _echo[0] = rhs.cmd_echo()[0];
+            _echo[1] = rhs.cmd_echo()[1];
+        }
 
         /// Get the two-byte command code as a non-null-terminated array.
         virtual char const* const cmd_code() const throw()
@@ -602,8 +553,6 @@
         char _cmd[2];
         /** Received echo. */
         char _echo[2];
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class CommandEchoError
 
 
@@ -615,10 +564,16 @@
 
         @param type The two-byte command code sent. */
         ParamEchoError(char const* const cmd)
-            : ProtocolError(32)
+            : ProtocolError(32, "ParamEchoError")
         {
             _cmd[0] = cmd[0]; _cmd[1] = cmd[1];
         }
+        ParamEchoError(ParamEchoError const& rhs)
+            : ProtocolError(rhs)
+        {
+            _cmd[0] = rhs.cmd_code()[0];
+            _cmd[1] = rhs.cmd_code()[1];
+        }
 
         /// Get the two-byte command code as a non-null-terminated array.
         virtual char const* const cmd_code() const throw()
@@ -629,8 +584,6 @@
     protected:
         /** Command that triggered the error, from SCIP2 (two bytes). */
         char _cmd[2];
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class ParamEchoError
 
 
@@ -643,8 +596,13 @@
         @param num The number of bytes received.
         @param line_length The length of the line. */
         InsufficientBytesError(int num, int line_length)
-            : ProtocolError(33), _num(num), _line_length(line_length)
+            : ProtocolError(33, "InsufficientBytesError"),
+            _num(num), _line_length(line_length)
         {}
+        InsufficientBytesError(InsufficientBytesError const& rhs)
+            : ProtocolError(rhs), _num(rhs.num()),
+            _line_length(rhs.line_length())
+        {}
 
         virtual int num() const throw()
             { return _num; }
@@ -659,8 +617,6 @@
         int _num;
         /** Length of the line. */
         int _line_length;
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class InsufficientBytesError
 
 
@@ -673,8 +629,13 @@
         @param num The number of bytes received.
         @param line_length The length of the line. */
         LineLengthError(int length, int expected)
-            : ProtocolError(34), _length(length), _expected(expected)
+            : ProtocolError(34, "LineLengthError"),
+            _length(length), _expected(expected)
         {}
+        LineLengthError(LineLengthError const& rhs)
+            : ProtocolError(rhs), _length(rhs.length()),
+            _expected(rhs.expected())
+        {}
 
         virtual int length() const throw()
             { return _length; }
@@ -689,8 +650,6 @@
         int _length;
         /** The expected line length. */
         int _expected;
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class LineLengthError
 
 }; // namespace hokuyo_aist


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 503
          http://gearbox.svn.sourceforge.net/gearbox/?rev=503&view=rev
Author:   gbiggs
Date:     2010-08-04 04:25:13 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Reorganising code

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h

Added Paths:
-----------
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h
    gearbox/trunk/src/hokuyo_aist/utils.cpp
    gearbox/trunk/src/hokuyo_aist/utils.h

Removed Paths:
-------------
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp

Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-04 03:20:45 UTC (rev 502)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-04 04:25:13 UTC (rev 503)
@@ -10,31 +10,40 @@
 GBX_REQUIRE_LIBS (build LIB ${libName} ${reqLibs})
 
 if (build)
-	include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
+    include (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
 
-	set (hdrs hokuyo_aist.h hokuyo_errors.h)
-	set (srcs hokuyo_aist.cpp hokuyo_errors.cpp)
+    set (hdrs hokuyo_aist.h
+              hokuyo_errors.h
+              sensor_info.h
+              scan_data.h
+              sensor.h
+              utils.h)
+    set (srcs hokuyo_errors.cpp
+              sensor_info.cpp
+              scan_data.cpp
+              sensor.cpp
+              utils.cpp)
 
-	if (WIN32)
-		if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
-			add_definitions (-DHOKUYO_AIST_EXPORTS)
-		else (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
-			add_definitions (-DHOKUYO_AIST_STATIC -DFLEXIPORT_STATIC)
-		endif (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
-	endif (WIN32)
-	GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
-	target_link_libraries (${libName} ${reqLibs})
-	GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
+    if (WIN32)
+        if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+            add_definitions (-DHOKUYO_AIST_EXPORTS)
+        else (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+            add_definitions (-DHOKUYO_AIST_STATIC -DFLEXIPORT_STATIC)
+        endif (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
+    endif (WIN32)
+    GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
+    target_link_libraries (${libName} ${reqLibs})
+    GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
 
-	GBX_ADD_HEADERS (${libName} ${hdrs})
+    GBX_ADD_HEADERS (${libName} ${hdrs})
 
-	add_subdirectory (getid)
-	if (GBX_BUILD_TESTS)
-		add_subdirectory (test)
-	endif (GBX_BUILD_TESTS)
+    add_subdirectory (getid)
+    if (GBX_BUILD_TESTS)
+        add_subdirectory (test)
+    endif (GBX_BUILD_TESTS)
 
-	option (HOKUYO_AIST_BUILD_BINDINGS "Build the Python bindings for Hokuyo_aist" ON)
-	if (HOKUYO_AIST_BUILD_BINDINGS)
-		add_subdirectory (python)
-	endif (HOKUYO_AIST_BUILD_BINDINGS)
+    option (HOKUYO_AIST_BUILD_BINDINGS "Build the Python bindings for Hokuyo_aist" ON)
+    if (HOKUYO_AIST_BUILD_BINDINGS)
+        add_subdirectory (python)
+    endif (HOKUYO_AIST_BUILD_BINDINGS)
 endif (build)

Deleted: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-04 03:20:45 UTC (rev 502)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2010-08-04 04:25:13 UTC (rev 503)
@@ -1,2399 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
- *
- * hokuyo_aist Hokuyo laser scanner driver.
- *
- * This distribution is licensed to you under the terms described in the
- * LICENSE file included in this distribution.
- *
- * This work is a product of the National Institute of Advanced Industrial
- * Science and Technology, Japan. Registration number: H22PRO-1086.
- *
- * This file is part of hokuyo_aist.
- *
- * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
- * http://www.opensource.org/licenses/eclipse-1.0.txt
- */
-
-#include "hokuyo_aist.h"
-#include "hokuyo_errors.h"
-using namespace hokuyo_aist;
-
-#include <flexiport/flexiport.h>
-#include <flexiport/port.h>
-#include <flexiport/serialport.h>
-
-#include <cstring>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <errno.h>
-#include <math.h>
-#include <sstream>
-#include <iostream>
-
-using namespace flexiport;
-
-#if defined(WIN32)
-    #define __func__    __FUNCTION__
-#endif
-
-namespace hokuyo_aist
-{
-
-#ifndef M_PI
-    const double M_PI = 3.14159265358979323846;
-#endif
-// Convert radians to degrees
-#ifndef RTOD
-    inline double RTOD(double rad)
-    {
-        return rad * 180.0 / M_PI;
-    }
-#endif
-// Convert degrees to radians
-#ifndef DTOR
-    inline double DTOR(double deg)
-    {
-        return deg * M_PI / 180.0;
-    }
-#endif
-
-// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
-// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
-
-///////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 1 notes
-///////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
-(x) = x byte block
- - No checksum
- - Host to sensor: Command | Parameters... | LF
- - Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF
- - Where a block of data would take more than 64 bytes, a line feed is inserted
-   every 64 bytes.
- - Status 0 is OK, anything else is an error.
-
-L  Power
-   L|Control code|LF
-   L|Control code|LF|Status|LF|LF
-   3 byte command block
-G  Get data
-   G|Start(3)|End(3)|Cluster(2)|LF
-   G|Start(3)|End(3)|Cluster(2)|LF|Status|LF|Data...|LF|LF
-   10 byte command block
-S  Set baud rate
-   S|Baud rate(6)|Reserved(7)|LF
-   S|Baud rate(6)|Reserved(7)|Status|LF|LF|
-   16 byte command block
-V  Version info
-   V|LF
-   V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|
-        Serial...|LF|LF
-   2 byte command block
-*/
-
-///////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 2 notes
-///////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes),
-(x) = x byte block
- - We don't use the string block (which can be up to 16 bytes) so it's marked
-   as size 0 and ignored in the command definitions below.
- - Host to sensor: Command(2) | Parameters... | String(0) | LF
- - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) |
-        Sum | LF
- - Each data row: Data (max 64) | Sum | LF
- - Data rows are broken after a maximum of 64 bytes, each one having a checksum
-   and a line feed.
- - Status codes 00 and 99 are OK, anything else is an error.
- - Checksum is calculated by... well, see the code.
- - BIG NOTE: The URG-30LX has a probable bug in its response to the II code
-   whereby it does not include anything after and including "<-" in the
-   checksum calculation.
-
-VV    Version info
-      V|V|LF
-      V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|
-        Firmware...|;|Sum|LF|Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
-      3 byte command block
-PP    Specification info
-      P|P|LF
-      P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|
-        MinRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
-        LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF
-      3 byte command block
-II    Status info
-      I|I|LF
-      I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|
-        MotorSpeed...|;|Sum|LF|Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|
-        LF|Diagnostic...|;|Sum|LF|LF
-      3 byte command block
-BM    Power on
-      B|M|LF
-      B|M|LF|Status(2)|Sum|LF|LF
-      3 byte command block
-QT    Power off
-      Q|T|LF
-      Q|T|LF|Status(2)|Sum|LF|LF
-      3 byte command block
-SS    Set baud rate
-      S|S|Baud(6)|LF
-      S|S|Baud(6)|LF|Status(2)|Sum|LF|LF
-      9 byte command block
-MDMS  Get new data
-      M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
-      M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|
-        LF|Data...|LF|LF
-      16 byte command block
-ME    Get new data, including intensity data
-      M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
-      M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|
-        Data...|LF|LF
-      16 byte command block
-      Featured only on UTM-30LX for getting intensity data.
-GDGS  Get latest data
-      G|D/S|Start(4)|End(4)|Cluster(2)|LF
-      G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
-      12 byte command block
-CR    Set motor speed
-      C|R|Speed(2)|LF
-      C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
-      5 byte command block
-TM    Get sensor time
-      T|M|Code|LF
-      T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
-      4 byte command block
-      Optional part only comes back for control code 1.
-RS    Reset
-      R|S|LF
-      R|S|LF|Status(2)|Sum|LF|LF
-      3 byte command block
-GE
-ND
-NE
-HD
-HE
-HS
-$IP
-RT
-DB
-*/
-
-///////////////////////////////////////////////////////////////////////////////
-// Utility functions
-///////////////////////////////////////////////////////////////////////////////
-
-unsigned int decode_2_byte_value(char *data)
-{
-    unsigned int byte1, byte2;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-
-    return (byte1 << 6) + (byte2);
-}
-
-unsigned int decode_3_byte_value(char *data)
-{
-    unsigned int byte1, byte2, byte3;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-    byte3 = data[2] - 0x30;
-
-    return (byte1 << 12) + (byte2 << 6) + (byte3);
-}
-
-unsigned int decode_4_byte_value(char *data)
-{
-    unsigned int byte1, byte2, byte3, byte4;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-    byte3 = data[2] - 0x30;
-    byte4 = data[3] - 0x30;
-
-    return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
-}
-
-void number_to_string(unsigned int num, char *dest, int length)
-{
-#if defined(WIN32)
-    _snprintf(dest, length + 1, "%*d", length, num);
-#else
-    snprintf(dest, length + 1, "%*d", length, num);
-#endif
-    // Replace all leading spaces with '0'
-    for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
-        dest[ii] = '0';
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// SensorInfo class
-///////////////////////////////////////////////////////////////////////////////
-
-SensorInfo::SensorInfo()
-    : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
-    front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
-    baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
-    scanable_steps(0)
-{
-}
-
-// Set various known values based on what the manual says
-void SensorInfo::set_defaults()
-{
-    min_range = 20;
-    max_range = 4095;
-    steps = 1024;
-    first_step = 44;
-    last_step = 725;
-    front_step = 384;
-}
-
-void SensorInfo::calculate_values()
-{
-    resolution = DTOR(360.0) / steps;
-    // If any of the steps are beyond INT_MAX, we have problems.
-    // We also have an incredibly high-resolution sensor.
-    min_angle = (static_cast<int>(first_step) - static_cast<int>(front_step)) *
-        resolution;
-    max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) *
-        resolution;
-    scanable_steps = last_step - first_step + 1;
-}
-
-std::string SensorInfo::as_string()
-{
-    std::stringstream ss;
-
-    ss << "Vendor: " << vendor << '\n';
-    ss << "Product: " << product << '\n';
-    ss << "Firmware: " << firmware << '\n';
-    ss << "Protocol: " << protocol << '\n';
-    ss << "Serial: " << serial << '\n';
-    ss << "Model: " << model << '\n';
-
-    ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range <<
-        "mm\n";
-    ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
-        scanable_steps << '\n';
-    ss << "First step: " << first_step << "\tFront step: " << front_step <<
-        "\tLast step: " << last_step << '\n';
-    ss << "Resolution: " << resolution << " radians/step\n";
-    ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
-        max_angle << " radians\n";
-    ss << "Standard motor speed: " << standard_speed << "rpm\n";
-
-    ss << "Power status: " << (power ? "On" : "Off") <<
-        "\tMeasurement state: " << measure_state << '\n';
-    ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
-        ")\tBaud rate: " << baud << "bps\n";
-    ss << "Time stamp: " << time << "ms\n";
-    ss << "Sensor diagnostic: " << sensor_diagnostic << '\n';
-
-    return ss.str();
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// ScanData class
-///////////////////////////////////////////////////////////////////////////////
-
-ScanData::ScanData()
-    : _ranges(NULL), _intensities(NULL), _length(0),
-    _error(false), _time(0)
-{
-}
-
-ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
-        unsigned int time)
-    : _error(error), _time(time)
-{
-    _length = length;
-    if (_length == 0)
-    {
-        _ranges = NULL;
-        _intensities = NULL;
-    }
-    else
-    {
-        try
-        {
-            _ranges = new uint32_t[_length];
-        }
-        catch (std::bad_alloc &e)
-        {
-            _length = 0;
-            throw;
-        }
-        memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
-        _intensities = NULL;    // No intensity data to copy
-    }
-}
-
-ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
-        unsigned int length, bool error, unsigned int time)
-    : _error(error), _time(time)
-{
-    _length = length;
-    if (_length == 0)
-    {
-        _ranges = NULL;
-        _intensities = NULL;
-    }
-    else
-    {
-        try
-        {
-            _ranges = new uint32_t[_length];
-        }
-        catch (std::bad_alloc &e)
-        {
-            _length = 0;
-            throw;
-        }
-        memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
-        _intensities = new uint32_t[_length];
-        memcpy(_intensities, intensities, sizeof(uint32_t) * _length);
-    }
-}
-
-ScanData::ScanData(const ScanData &rhs)
-{
-    _length = rhs.length();
-    if (_length == 0)
-        _ranges = NULL;
-    else
-    {
-        try
-        {
-            _ranges = new uint32_t[_length];
-        }
-        catch (std::bad_alloc &e)
-        {
-            _length = 0;
-            throw;
-        }
-        memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
-
-        if (rhs.intensities() != NULL)
-        {
-            _intensities = new uint32_t[_length];
-            memcpy(_intensities, rhs.intensities(),
-                    sizeof(uint32_t) * _length);
-        }
-    }
-    _error = rhs.get_error_status();
-    _time = rhs.time_stamp();
-}
-
-ScanData::~ScanData()
-{
-    if(_ranges != NULL)
-        delete[] _ranges;
-}
-
-std::string ScanData::error_code_to_string(uint32_t error_code)
-{
-    if (_sensorIsUTM30LX)
-    {
-        switch (error_code)
-        {
-            case 1:
-                return "No object in the range.";
-            case 2:
-                return "Object is too near (internal error).";
-            case 3:
-                return "Measurement error (may be due to interference).";
-            case 4:
-                return "Object out of range (at the near end).";
-            case 5:
-                return "Other error.";
-            default:
-                std::stringstream ss;
-                ss << "Unknown error code: " << error_code;
-                return ss.str();
-        }
-    }
-    else
-    {
-        switch (error_code)
-        {
-            case 0:
-                return "Detected object is possibly at 22m.";
-            case 1:
-                return "Reflected light has low intensity.";
-            case 2:
-                return "Reflected light has low intensity.";
-            case 3:
-                return "Reflected light has low intensity.";
-            case 4:
-                return "Reflected light has low intensity.";
-            case 5:
-                return "Reflected light has low intensity.";
-            case 6:
-                return "Possibility of detected object is at 5.7m.";
-            case 7:
-                return "Distance data on the preceding and succeeding steps "
-                    "have errors.";
-            case 8:
-                return "Others.";
-            case 9:
-                return "The same step had error in the last two scan.";
-            case 10:
-                return "Others.";
-            case 11:
-                return "Others.";
-            case 12:
-                return "Others.";
-            case 13:
-                return "Others.";
-            case 14:
-                return "Others.";
-            case 15:
-                return "Others.";
-            case 16:
-                return "Possibility of detected object is in the range "
-                    "4096mm.";
-            case 17:
-                return "Others.";
-            case 18:
-                return "Unspecified.";
-            case 19:
-                return "Non-measurable distance.";
-            default:
-                std::stringstream ss;
-                ss << "Unknown error code: " << error_code;
-                return ss.str();
-        }
-    }
-}
-
-ScanData& ScanData::operator=(const ScanData &rhs)
-{
-    if (rhs.length() == 0)
-    {
-        _length = 0;
-        if (_ranges != NULL)
-            delete[] _ranges;
-        _ranges = NULL;
-        if (_intensities != NULL)
-            delete[] _intensities;
-        _intensities = NULL;
-        _error = rhs.get_error_status();
-        _time = rhs.time_stamp();
-    }
-    else
-    {
-        unsigned int rhslength = rhs.length();
-        uint32_t *newData;
-        if (rhslength != _length)
-        {
-            // Copy the data into a temporary variable pointing to new space
-            // (prevents dangling pointers on allocation error and prevents
-            // self-assignment making a mess).
-            newData = new uint32_t[rhslength];
-            memcpy(newData, rhs.ranges(), sizeof(uint32_t) * rhslength);
-            if (_ranges != NULL)
-                delete[] _ranges;
-            _ranges = newData;
-            _length = rhs.length();
-
-            if (rhs.intensities() != NULL)
-            {
-                try
-                {
-                    newData = new uint32_t[rhslength];
-                }
-                catch (std::bad_alloc &e)
-                {
-                    // We have to remove any old intensity data or the length
-                    // won't match
-                    if (_intensities != NULL)
-                        delete[] _intensities;
-                    _intensities = NULL;
-                    throw;
-                }
-                memcpy(newData, rhs.intensities(), sizeof(uint32_t) *
-                        rhslength);
-                if (_intensities != NULL)
-                    delete[] _intensities;
-                _intensities = newData;
-            }
-        }
-        else
-        {
-            // If lengths are the same, no need to reallocate
-            memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
-            if (rhs.intensities() != NULL)
-                memcpy(_intensities, rhs.intensities(), sizeof(uint32_t) *
-                        _length);
-        }
-
-        _error = rhs.get_error_status();
-        _time = rhs.time_stamp();
-    }
-
-    return *this;
-}
-
-uint32_t ScanData::operator[](unsigned int index)
-{
-    if (index >= _length)
-        throw IndexError();
-    return _ranges[index];
-}
-
-std::string ScanData::as_string()
-{
-    std::stringstream ss;
-
-    ss << _length << " readings:\n";
-    for (unsigned int ii = 0; ii < _length; ii++)
-        ss << _ranges[ii] << "\t";
-    if (_intensities != NULL)
-    {
-        ss << std::endl << _length << " intensities:\n";
-        for (unsigned int ii = 0; ii < _length; ii++)
-            ss << _intensities[ii] << "\t";
-    }
-    ss << '\n';
-    if (_error)
-    {
-        ss << "Detected data errors:\n";
-        for (unsigned int ii = 0; ii < _length; ii++)
-        {
-            if (_ranges[ii] < 20)
-                ss << ii << ": " <<
-                    error_code_to_string(_ranges[ii]) << '\n';
-        }
-    }
-    else
-        ss << "No data errors.\n";
-    ss << "Time stamp: " << _time << '\n';
-
-    return ss.str();
-}
-
-void ScanData::clean_up()
-{
-    if (_ranges != NULL)
-        delete[] _ranges;
-    _ranges = NULL;
-    if (_intensities != NULL)
-        delete[] _intensities;
-    _intensities = NULL;
-    _length = 0;
-    _error = false;
-    _time = 0;
-}
-
-void ScanData::allocate_data(unsigned int length, bool include_intensities)
-{
-    // If no data yet, allocate new
-    if (_ranges == NULL)
-    {
-        try
-        {
-            _ranges = new uint32_t[length];
-        }
-        catch (std::bad_alloc &e)
-        {
-            _length = 0;
-            throw;
-        }
-        _length = length;
-    }
-    // If there is data, reallocate only if the length is different
-    else if (length != _length)
-    {
-        delete[] _ranges;
-        try
-        {
-            _ranges = new uint32_t[length];
-        }
-        catch (std::bad_alloc &e)
-        {
-            _length = 0;
-            throw;
-        }
-        _length = length;
-    }
-    // Else data is already allocated to the right length, so do nothing
-
-    if (include_intensities)
-    {
-        // If no data yet, allocate new
-        if (_intensities == NULL)
-        {
-            _intensities = new uint32_t[length];
-        }
-        // If there is data, reallocate only if the length is different
-        else if (length != _length)
-        {
-            delete[] _intensities;
-            _intensities = new uint32_t[length];
-        }
-        // Else data is already allocated to the right length, so do nothing
-    }
-    else if (_intensities != NULL)
-    {
-        // If not told to allocate space for intensity data and it exists,
-        // remove it
-        delete[] _intensities;
-        _intensities = NULL;
-    }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Sensor class
-///////////////////////////////////////////////////////////////////////////////
-
-// Public API
-///////////////////////////////////////////////////////////////////////////////
-
-Sensor::Sensor()
-    : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
-    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
-    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
-    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
-{
-}
-
-Sensor::Sensor(std::ostream& err_output)
-    : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
-    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
-    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
-    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
-{
-}
-
-Sensor::~Sensor()
-{
-    if (_port != NULL)
-        delete _port;
-}
-
-void Sensor::open(std::string port_options)
-{
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ <<
-            "() Creating and opening port using options: " <<
-            port_options << '\n';
-    }
-    _port = flexiport::CreatePort(port_options);
-    _port->Open();
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection.\n";
-        _err_output << _port->GetStatus();
-    }
-    _port->Flush();
-
-    // Figure out the SCIP version currently in use and switch to a higher one
-    // if possible
-    _get_and_set_scip_version();
-    // Get some values we need for providing default ranges
-    _get_defaults();
-}
-
-unsigned int Sensor::open_with_probing(std::string port_options)
-{
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ <<
-            "() Creating and opening port using options: " << port_options <<
-            '\n';
-    }
-    _port = flexiport::CreatePort(port_options);
-    _port->Open();
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection.\n";
-        _err_output << _port->GetStatus();
-    }
-    _port->Flush();
-
-    try
-    {
-        // Figure out the SCIP version currently in use and switch to a higher
-        // one if possible
-        _get_and_set_scip_version();
-        // Get some values we need for providing default ranges
-        _get_defaults();
-    }
-    catch (BaseError)
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Failed to connect at the default baud rate.\n";
-        }
-        if (_port->GetPortType() == "serial")
-        {
-            // Failed at the default baud rate, so try again at the other
-            // rates. Note that a baud rate of 750000 or 250000 doesn't appear
-            // to be supported on any common OS.
-            const unsigned int bauds[] = {500000, 115200, 57600, 38400, 19200};
-            const unsigned int numBauds = 5;
-            for (unsigned int ii = 0; ii < numBauds; ii++)
-            {
-                reinterpret_cast<SerialPort*>(_port)->SetBaudRate(bauds[ii]);
-                try
-                {
-                    _get_and_set_scip_version();
-                    _get_defaults();
-                    // If the above two functions succeed, break out of the
-                    // loop and be happy
-                    if (_verbose)
-                    {
-                        _err_output << "Sensor::" << __func__ <<
-                            "() Connected at " << bauds[ii] << '\n';
-                    }
-                    return bauds[ii];
-                }
-                catch (BaseError)
-                {
-                    if (ii == numBauds - 1)
-                    {
-                        // Last baud rate, give up and rethrow
-                        if (_verbose)
-                        {
-                            _err_output << "Sensor::" << __func__ <<
-                                "() Failed to connect at any baud rate.\n";
-                        }
-                        throw;
-                    }
-                    // Otherwise go around again
-                }
-            }
-        }
-        else
-        {
-            if (_verbose)
-            {
-                _err_output << "Sensor::" << __func__ <<
-                    "() Port is not serial, cannot probe.\n";
-            }
-            throw;
-        }
-    }
-
-    if (_port->GetPortType() == "serial")
-        return reinterpret_cast<SerialPort*>(_port)->GetBaudRate();
-    else
-        return 0;
-}
-
-void Sensor::close()
-{
-    if (!_port)
-        throw CloseError();
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
-    delete _port;
-    _port = NULL;
-}
-
-bool Sensor::is_open() const
-{
-    if (_port != NULL)
-        return _port->IsOpen();
-    return false;
-}
-
-void Sensor::set_power(bool on)
-{
-    if (_scip_version == 1)
-    {
-        if (on)
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Turning laser on.\n";
-            _send_command("L", "1", 1, NULL);
-        }
-        else
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Turning laser off.\n";
-            _send_command("L", "0", 1, NULL);
-        }
-        _skip_lines(1);
-    }
-    else if (_scip_version == 2)
-    {
-        if (on)
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Turning laser on.\n";
-            _send_command("BM", NULL, 0, "02");
-        }
-        else
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Turning laser off.\n";
-            _send_command("QT", NULL, 0, "02");
-        }
-        _skip_lines(1);
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-// This function assumes that both the port and the laser scanner are already
-// set to the same baud.
-void Sensor::set_baud(unsigned int baud)
-{
-    if (_port->GetPortType() != "serial")
-    {
-        throw NotSerialError();
-    }
-
-    char newBaud[13];
-    memset(newBaud, 0, sizeof(char) * 13);
-
-    if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
-        baud != 250000 && baud != 500000 && baud != 750000)
-    {
-        throw BaudrateError(baud);
-    }
-    number_to_string(baud, newBaud, 6);
-
-    if (_scip_version == 1)
-    {
-        // Send the command to change baud rate
-        _send_command("S", newBaud, 13, NULL);
-        _skip_lines(1);
-        // Change the port's baud rate
-        reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
-    }
-    else if (_scip_version == 2)
-    {
-        // Send the command to change baud rate
-        _send_command("SS", newBaud, 6, "03");
-        _skip_lines(1);
-        // Change the port's baud rate
-        reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-void Sensor::reset()
-{
-    if (_scip_version == 1)
-        throw UnsupportedError(7);
-    else if (_scip_version == 2)
-    {
-        if (_verbose)
-            _err_output << "Sensor::" << __func__ <<
-                "() Resetting laser.\n";
-        _send_command("RS", NULL, 0, NULL);
-        _skip_lines(1);
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-void Sensor::set_motor_speed(unsigned int speed)
-{
-    if (_scip_version == 1)
-        throw UnsupportedError(8);
-    else if (_scip_version == 2)
-    {
-        // Sanity check the value
-        if (speed > 10 && speed != 99)
-            throw MotorSpeedError();
-        char buffer[3];
-        if (speed == 0)
-        {
-            if (_verbose)
-            {
-                _err_output << "Sensor::" << __func__ <<
-                    "() Reseting motor speed to default.\n";
-            }
-            buffer[0] = '0';
-            buffer[1] = '0';
-            buffer[2] = '\0';
-        }
-        else if (speed == 99)
-        {
-            if (_verbose)
-            {
-                _err_output << "Sensor::" << __func__ <<
-                    "() Reseting motor speed to default.\n";
-            }
-            buffer[0] = '9';
-            buffer[1] = '9';
-            buffer[2] = '\0';
-        }
-        else
-        {
-            if (_verbose)
-            {
-                _err_output << "Sensor::" << __func__ <<
-                    "() Setting motor speed to ratio " << speed << '\n';
-            }
-            number_to_string(speed, buffer, 2);
-        }
-        _send_command("CR", buffer, 2, "03");
-        _skip_lines(1);
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-void Sensor::set_high_sensitivity(bool on)
-{
-    if (_scip_version == 1)
-        throw UnsupportedError(10);
-    else if (_scip_version == 2)
-    {
-        if (on)
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Switching to high sensitivity.\n";
-            _send_command("HS", "1", 1, "02");
-        }
-        else
-        {
-            if (_verbose)
-            {
-                _err_output << "Sensor::" << __func__ <<
-                    "() Switching to normal sensitivity.\n";
-            }
-            _send_command("HS", "0", 1, "02");
-        }
-        _skip_lines(1);
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-void Sensor::get_sensor_info(SensorInfo *info)
-{
-    if (info == NULL)
-        throw NoDestinationError();
-
-    if (_scip_version == 1)
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Getting sensor information using SCIP version 1.\n";
-        }
-
-        info->set_defaults();
-
-        char buffer[SCIP1_LINE_LENGTH];
-        memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
-
-        _send_command("V", NULL, 0, NULL);
-        // Get the vendor info line
-        _read_line(buffer);
-        info->vendor = &buffer[5]; // Chop off the "VEND:" tag
-        // Get the product info line
-        _read_line(buffer);
-        info->product = &buffer[5];
-        // Get the firmware line
-        _read_line(buffer);
-        info->firmware = &buffer[5];
-        // Get the protocol version line
-        _read_line(buffer);
-        info->protocol = &buffer[5];
-        // Get the serial number
-        _read_line(buffer);
-        info->serial = &buffer[5];
-        // Get either the status line or the end of message
-        _read_line(buffer);
-        if (buffer[0] != '\0')
-        {
-            // Got a status line
-            info->sensor_diagnostic = &buffer[5];
-            _skip_lines(1);
-        }
-
-        // Check the firmware version major number. If it's >=3 there is
-        // probably some extra info in the firmware line.
-        // eg: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
-        // Note that this example is right up against the maximum SCIP v1 line
-        // length of 64 bytes.
-        if (atoi(info->firmware.c_str()) >= 3)
-        {
-            if (_verbose)
-                _err_output << "SCIP1 Firmware line for parsing: " <<
-                    info->firmware << '\n';
-            // Now the fun part: parsing the line. It would be nice if we could
-            // use the POSIX regex functions, but since MS doesn't believe in
-            // POSIX we get to do it the hard way.
-            // Start by finding the first (
-            const char *valueStart;
-            if ((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
-            {
-                // No bracket? Crud. Fail and use the hard-coded values from
-                // the manual.
-                info->calculate_values();
-            }
-            // Now put it through sscanf and hope...
-            int aperture;
-            int numFound = sscanf(valueStart,
-                    "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info->min_range,
-                    &info->max_range, &aperture, &info->first_step,
-                    &info->last_step, &info->speed);
-            if (numFound != 6)
-            {
-                // Didn't get enough values out, assume unknown format and fall
-                // back on the defaults
-                info->set_defaults();
-                info->calculate_values();
-                if (_verbose)
-                {
-                    _err_output << "Retrieved sensor info (hard-coded, not "
-                        "enough values):\n";
-                    _err_output << info->as_string();
-                }
-            }
-            else
-            {
-                // Need to calculate stuff differently since it gave us an
-                // aperture value
-                info->resolution = DTOR(static_cast<double>(aperture)) /
-                    static_cast<double>(info->last_step - info->first_step);
-                // Assume that the range is evenly spread
-                info->scanable_steps = info->last_step - info->first_step + 1;
-                info->front_step = info->scanable_steps / 2 +
-                    info->first_step - 1;
-                info->min_angle = (static_cast<int>(info->first_step) -
-                        static_cast<int>(info->front_step)) * info->resolution;
-                info->max_angle = (info->last_step - info->front_step) *
-                    info->resolution;
-
-                if (_verbose)
-                {
-                    _err_output << "Retrieved sensor info (from FIRM line):\n";
-                    _err_output << info->as_string();
-                }
-            }
-        }
-        else
-        {
-            // We're stuck with hard-coded defaults from the manual (already
-            // set earlier).
-            info->calculate_values();
-            if (_verbose)
-            {
-                _err_output << "Retrieved sensor info (hard-coded):\n";
-                _err_output << info->as_string();
-            }
-        }
-    }
-    else if (_scip_version == 2)
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Getting sensor information using SCIP version 2.\n";
-        }
-
-        info->set_defaults();
-
-        char buffer[SCIP2_LINE_LENGTH];
-        memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
-
-        // We need to send three commands to get all the info we want: VV, PP
-        // and II
-        _send_command("VV", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
-            _process_vv_line(buffer, info);
-
-        // Next up, PP
-        _send_command("PP", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
-            _process_pp_line(buffer, info);
-
-        // Command II: Revenge of the Commands.
-        _send_command("II", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
-            _process_ii_line(buffer, info);
-
-        _enable_checksum_workaround = false;
-
-        info->calculate_values();
-        if (_verbose)
-        {
-            _err_output << "Retrieved sensor info:\n";
-            _err_output << info->as_string();
-        }
-    }
-    else
-        throw UnknownScipVersionError();
-}
-
-unsigned int Sensor::get_time()
-{
-    if (_scip_version == 1)
-        throw UnsupportedError(12);
-    else if (_scip_version == 2)
-    {
-        if (_verbose)
-            _err_output << "Sensor::" << __func__ <<
-                "() Retrieving time from laser.\n";
-        _send_command("TM", "0", 1, NULL);
-        _send_command("TM", "1", 1, NULL);
-        char buffer[7];
-        _read_line_with_check(buffer, 6);
-        _send_command("TM", "2", 1, NULL);
-        _skip_lines(1);
-        // We need to decode the time value that's in the buffer
-        return decode_4_byte_value(buffer);
-    }
-    else
-        throw UnknownScipVersionError();
-
-    return 0;
-}
-
-unsigned int Sensor::get_ranges(ScanData *data, int start_step,
-        int end_step, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDestinationError();
-
-    char buffer[11];
-    memset(buffer, 0, sizeof(char) * 11);
-
-    if (start_step < 0)
-        start_step = _first_step;
-    if (end_step < 0)
-        end_step = _last_step;
-
-    unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges between " << start_step << " and " <<
-            end_step << " with a cluster count of " << cluster_count <<
-            '\n';
-    }
-
-    if (_scip_version == 1)
-    {
-        // Send the command to ask for the most recent range data from
-        // start_step to end_step
-        number_to_string(start_step, buffer, 3);
-        number_to_string(end_step, &buffer[3], 3);
-        number_to_string(cluster_count, &buffer[6], 2);
-        _send_command("G", buffer, 8, NULL);
-        // In SCIP1 mode we're going to get back 2-byte data
-        _read_2_byte_range_data(data, num_steps);
-    }
-    else if (_scip_version == 2)
-    {
-        // Send the command to ask for the most recent range data from
-        // start_step to end_step
-        number_to_string(start_step, buffer, 4);
-        number_to_string(end_step, &buffer[4], 4);
-        number_to_string(cluster_count, &buffer[8], 2);
-        _send_command("GD", buffer, 10, NULL);
-        // There will be a timestamp before the data (if there is data)
-        // Normally we would send 6 for the expected length, but we may get no
-        // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
-            throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
-        // In SCIP2 mode we're going to get back 3-byte data because we're
-        // sending the GD command
-        _read_3_byte_range_data(data, num_steps);
-    }
-    else
-        throw UnknownScipVersionError();
-
-    return data->_length;
-}
-
-unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
-        double end_angle, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDataError();
-
-    // Calculate the given angles in steps, rounding towards _front_step
-    int start_step, end_step;
-    start_step = angle_to_step(start_angle);
-    end_step = angle_to_step(end_angle);
-
-    // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
-        throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
-        throw EndStepError();
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
-            start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << '\n';
-    }
-
-    // Get the data
-    return get_ranges(data, start_step, end_step, cluster_count);
-}
-
-unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
-        int end_step, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDestinationError();
-
-    if (_scip_version == 1)
-        throw UnsupportedError(16);
-    else if (_scip_version == 2)
-    {
-        char buffer[14];
-        memset(buffer, 0, sizeof(char) * 14);
-
-        if (start_step < 0)
-            start_step = _first_step;
-        if (end_step < 0)
-            end_step = _last_step;
-
-        unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ << "() Reading " <<
-                num_steps << " new ranges between " << start_step << " and " <<
-                end_step << " with a cluster count of " << cluster_count <<
-                '\n';
-        }
-
-        // Send the command to ask for the most recent range data from start_step to end_step
-        number_to_string(start_step, buffer, 4);
-        number_to_string(end_step, &buffer[4], 4);
-        number_to_string(cluster_count, &buffer[8], 2);
-        number_to_string(1, &buffer[10], 1);
-        number_to_string(1, &buffer[11], 2);
-        _send_command("MD", buffer, 13, NULL);
-        // Mx commands will perform a scan, then send the data prefixed with
-        // another command echo.
-        // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        char response[17];
-        _skip_lines(1); // End of the command echo message
-        _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
-        // Check the echo is correct
-        if (response[0] != 'M' || response[1] != 'D')
-            throw CommandEchoError("MD", response);
-        // Then compare the parameters
-        buffer[12] = '0'; // There will be zero scans remaining after this one
-        if (memcmp(&response[2], buffer, 13) != 0)
-            throw ParamEchoError("MD");
-        // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() MD data prefix status: " << response[0] << response[1] <<
-                '\n';
-        }
-        // Check the status code is OK - should only get 99 here
-        if (response[0] != '9' || response[1] != '9')
-        {
-            // There is an extra line feed after an error status (signalling
-            // end of message)
-            _skip_lines(1);
-            throw ResponseError(response, "MD");
-        }
-
-        // Now the actual data will arrive
-        // There will be a timestamp before the data (if there is data)
-        // Normally we would send 6 for the expected length, but we may get no
-        // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
-            throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
-        // In SCIP2 mode we're going to get back 3-byte data because we're
-        // sending the MD command
-        _read_3_byte_range_data(data, num_steps);
-    }
-    else
-        throw UnknownScipVersionError();
-
-    return data->_length;
-}
-
-unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
-        double start_angle, double end_angle, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDestinationError();
-    if (_scip_version == 1)
-        throw UnsupportedError(16);
-
-    // Calculate the given angles in steps, rounding towards _front_step
-    int start_step, end_step;
-    start_step = angle_to_step(start_angle);
-    end_step = angle_to_step(end_angle);
-
-    // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
-        throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
-        throw EndStepError();
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
-            start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << '\n';
-    }
-
-    // Get the data
-    return get_new_ranges(data, start_step, end_step, cluster_count);
-}
-
-unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
-        int start_step, int end_step, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDestinationError();
-
-    if (_scip_version == 1)
-        throw UnsupportedError(17);
-    else if (_scip_version == 2)
-    {
-        char buffer[14];
-        memset(buffer, 0, sizeof(char) * 14);
-
-        if (start_step < 0)
-            start_step = _first_step;
-        if (end_step < 0)
-            end_step = _last_step;
-
-        unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ << "() Reading " <<
-                num_steps << " new ranges between " << start_step << " and " <<
-                end_step << " with a cluster count of " << cluster_count <<
-                '\n';
-        }
-
-        // Send the command to ask for the most recent range data with
-        // intensity data from start_step to end_step
-        number_to_string(start_step, buffer, 4);
-        number_to_string(end_step, &buffer[4], 4);
-        number_to_string(cluster_count, &buffer[8], 2);
-        number_to_string(1, &buffer[10], 1);
-        number_to_string(1, &buffer[11], 2);
-        _send_command("ME", buffer, 13, NULL);
-        // Mx commands will perform a scan, then send the data prefixed with
-        // another command echo.
-        // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        char response[17];
-        _skip_lines(1); // End of the command echo message
-        _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
-        // Check the echo is correct
-        if (response[0] != 'M' || response[1] != 'E')
-            throw CommandEchoError("ME", response);
-        // Then compare the parameters
-        if (memcmp(&response[2], buffer, 13) != 0)
-            throw ParamEchoError("ME");
-        // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() ME data prefix status: " << response[0] << response[1] <<
-                '\n';
-        }
-        // Check the status code is OK - should only get 99 here
-        if (response[0] != '9' || response[1] != '9')
-        {
-            // There is an extra line feed after an error status (signalling
-            // end of message)
-            _skip_lines(1);
-            throw ResponseError(response, "ME");
-        }
-
-        // Now the actual data will arrive
-        // There will be a timestamp before the data (if there is data)
-        // Normally we would send 6 for the expected length, but we may get no
-        // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
-            throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
-        // In SCIP2 mode we're going to get back 3-byte data because we're
-        // sending the ME command
-        _read_3_byte_range_and_intensity_data(data, num_steps);
-    }
-    else
-        throw UnknownScipVersionError();
-
-    return data->_length;
-}
-
-unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
-        double start_angle, double end_angle, unsigned int cluster_count)
-{
-    if (data == NULL)
-        throw NoDestinationError();
-    if (_scip_version == 1)
-        throw UnsupportedError(17);
-
-    // Calculate the given angles in steps, rounding towards _front_step
-    int start_step, end_step;
-    start_step = angle_to_step(start_angle);
-    end_step = angle_to_step(end_angle);
-
-    // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
-        throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
-        throw EndStepError();
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
-            start_angle << " is step " << start_step << ", end angle " <<
-            end_angle << " is step " << end_step << '\n';
-    }
-
-    // Get the data
-    return get_new_ranges_intensities(data, start_step, end_step,
-            cluster_count);
-}
-
-double Sensor::step_to_angle(unsigned int step)
-{
-    return (static_cast<int>(step) - static_cast<int>(_front_step)) *
-        _resolution;
-}
-
-unsigned int Sensor::angle_to_step(double angle)
-{
-    unsigned int result;
-    double resultF;
-    resultF = _front_step +
-        (static_cast<double>(angle) / static_cast<double>(_resolution));
-    // Round towards _front_step so that the step values are always inside the
-    // angles given
-    if (resultF < _front_step)
-        result = static_cast<int>(ceil(resultF));
-    else
-        result = static_cast<int>(floor(resultF));
-
-    return result;
-}
-
-// Private functions
-///////////////////////////////////////////////////////////////////////////////
-
-// Sometimes, just flushing isn't enough, as it appears the scanner will wait
-// when the buffer gets full, then continue sending data. If we flush, we get
-// rid of what was sent, and the scanner just sends more. This is a problem if
-// we're trying to clear the result of a previous command.  To get around this,
-// keep flushing until the port reports there is no data left after a timeout.
-// This shouldn't be called too much, as it introduces a delay as big as the
-// timeout (which may be infinite).
-void Sensor::_clear_read_buffer()
-{
-    while (_port->BytesAvailableWait() > 0)
-        _port->Flush();
-}
-
-// If expected_length is not -1, it should include the terminating line feed but
-// not the NULL (although the buffer still has to include this).
-// If expected_length is -1, this function expects buffer to be a certain length
-// to allow up to the maximum line length to be read. See SCIP1_LINE_LENGTH and
-// SCIP2_LINE_LENGTH.
-// The line feed that terminates a line will be replaced with a NULL.
-// The return value is the number of bytes received, not including the NULL
-// byte or the line feed.
-int Sensor::_read_line(char *buffer, int expected_length)
-{
-    int linelength = 0;
-
-    if (expected_length == -1)
-    {
-        int maxlength = (_scip_version == 1) ?
-            SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ << "() Reading up to " <<
-                maxlength << " bytes.\n";
-        }
-        // We need to get at least 1 byte in a line: the line feed.
-        if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
-            throw ReadError(0);
-        else if (linelength == 0)
-            throw ReadError(1);
-        // Replace the line feed with a NULL
-        buffer[linelength - 1] = '\0';
-    }
-    else
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Reading exactly " << expected_length << " bytes.\n";
-        }
-        // expected_length+1 for the NULL
-        if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
-            throw ReadError(0);
-        else if (linelength == 0)
-            throw ReadError(1);
-        else if (linelength < expected_length)
-            throw LineLengthError(linelength, expected_length);
-        // Replace the line feed with a NULL
-        buffer[linelength - 1] = '\0';
-    }
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
-            " bytes.\n";
-        _err_output << "Sensor::" << __func__ << "() Line is " << buffer <<
-            '\n';
-    }
-    return linelength - 1; // Line feed not included
-}
-
-// This function will read a line and then calculate its checksum, comparing it
-// with the checksum at the end of the line. The checksum will be removed
-// (along with the semi-colon, if present).  buffer and expected_length args are
-// as for _read_line().
-// If has_semicolon is true, the byte before the checksum is assumed to be the
-// semi-colon separator and so not a part of the checksum. If it's not a
-// semi-colon, an exception is thrown.
-// Empty lines (i.e. a line that is just the line feed, as at the end of the
-// message) will result in a return value of zero and no checksum check will be
-// performed. Otherwise the number of actual data bytes (i.e. excluding the
-// checksum and semicolon) will be returned.
-// BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
-// whereby it does not include anything after and including "<-" in the
-// checksum calculation. A workaround is enabled in this function when
-// _sensorIsUTM30LX is true. In this case, if the checksum fails normally, it
-// scans the line for "<-" and recalculates the checksum on the bytes up to
-// that point. This only happens in SCIP v2.
-int Sensor::_read_line_with_check(char *buffer, int expected_length,
-        bool has_semicolon)
-{
-    int linelength = _read_line(buffer, expected_length);
-    if (_scip_version == 1)
-    {
-        // No checksums in SCIP version 1
-        return linelength;
-    }
-
-    // If the line is empty, assume it was a line-feed message terminator, in
-    // which case there is no checksum to check.
-    if (linelength == 0)
-        return 0;
-
-    // Ignore the checksum itself, and possibly a semicolon (_read_line has
-    // already chopped off the line feed for us).
-    int bytesToConsider = linelength - 1 - (has_semicolon ? 1 : 0);
-    int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Considering " <<
-            bytesToConsider << " bytes for checksum from a line length of " <<
-            linelength << " bytes.\n";
-    }
-    if (bytesToConsider < 1)
-        throw InsufficientBytesError(bytesToConsider, linelength);
-
-    int checkSum = 0;
-    try
-    {
-        checkSum = _confirm_checksum(buffer, bytesToConsider,
-                static_cast<int>(buffer[checksumIndex]));
-    }
-    catch (ProtocolError &e)
-    {
-        if (_sensorIsUTM30LX && _enable_checksum_workaround)
-        {
-            // Here comes the UTM-30LX workaround
-            char* hasComment = strstr(buffer, "<-");
-            if (hasComment != NULL)
-            {
-                int newBytesToConsider = hasComment - buffer;
-                if (_verbose)
-                {
-                    _err_output << "Sensor::" << __func__ <<
-                        "() Performing UTM-30LX II response "
-                        "checksum workaround: trying with " <<
-                        newBytesToConsider << " bytes.\n";
-                }
-                if (newBytesToConsider < 1)
-                {
-                    throw InsufficientBytesError(newBytesToConsider,
-                            linelength);
-                }
-
-                checkSum = _confirm_checksum(buffer, newBytesToConsider,
-                        static_cast<int>(buffer[checksumIndex]));
-            }
-            else
-                // Workaround is disabled - rethrow
-                throw;
-        }
-        else
-            // Not a workaround-compatible error - rethrow
-            throw;
-    }
-
-    // Null out the semi-colon (if there) and checksum
-    buffer[bytesToConsider] = '\0';
-
-    return bytesToConsider;
-}
-
-// Reads lines until the number specified has passed.
-void Sensor::_skip_lines(int count)
-{
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
-            " lines.\n";
-    if (_port->SkipUntil(0x0A, count) < 0)
-        throw ReadError(18);
-}
-
-// Sends a command with optional parameters and checks that the echo of the
-// command and parameters sent are correct, and that the returned status code
-// is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two
-// bytes of extra_ok (for SCIP2).
-// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string
-// for SCIP2.
-// If param_length is 0, no parameters will be sent or expected in the reply.
-// extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
-// Return value is the status code returned for the command.
-int Sensor::_send_command(const char *cmd, const char *param,
-        int param_length, const char *extra_ok)
-{
-    int statusCode = -1;
-    char response[17];
-
-    // Flush first to clear out the dregs of any previous commands
-    _port->Flush();
-
-    if (_scip_version == 1)
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Writing in SCIP1 mode. Command is " << cmd[0] <<
-                ", parameters length is " << param_length << '\n';
-        }
-        // Write the command
-        if (_port->Write(cmd, 1) < 1)
-            throw WriteError(19);
-        if (param_length > 0)
-        {
-            if (_port->Write(param, param_length) < param_length)
-                throw WriteError(20);
-        }
-        if (_port->Write("\n", 1) < 1)
-            throw WriteError(21);
-
-        // Read back the response (should get at least 4 bytes , possibly up to
-        // 16 including \n's depending on the parameters): cmd[0] params \n
-        // status \n
-        int statusIndex = 2 + param_length;
-        _read_line(response, 2 + param_length);
-        _read_line(&response[statusIndex], 2);
-        // First make sure that the echoed command matches
-        if (response[0] != cmd[0])
-        {
-            char temp_cmd[2];
-            temp_cmd[0] = cmd[0];
-            temp_cmd[1] = '\0';
-            char temp_echo[2];
-            temp_echo[0] = response[0];
-            temp_echo[1] = '\0';
-            throw CommandEchoError(temp_cmd, temp_echo);
-        }
-        // Then compare the parameters
-        if (param_length > 0)
-        {
-            if (memcmp(&response[1], param, param_length) != 0)
-            {
-                char temp_cmd[2];
-                temp_cmd[0] = cmd[0];
-                temp_cmd[1] = '\0';
-                throw ParamEchoError(temp_cmd);
-            }
-        }
-        // Next up, check the status byte
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Command response status: " << response[statusIndex] <<
-                '\n';
-        }
-        if (response[statusIndex] != '0')
-        {
-            if (extra_ok != NULL)
-            {
-                if (response[statusIndex] != extra_ok[0])
-                {
-                    // There is an extra line feed after an error status
-                    // (signalling end of message)
-                    _skip_lines(1);
-                    throw Scip1ResponseError(response[statusIndex], cmd[0]);
-                }
-            }
-            else
-            {
-                // There is an extra line feed after an error status
-                // (signalling end of message)
-                _skip_lines(1);
-                throw Scip1ResponseError(response[statusIndex], cmd[0]);
-            }
-        }
-        statusCode = atoi(&response[statusIndex]);
-        // All OK, data starts at beginning of port's buffer
-    }
-    else if (_scip_version == 2)
-    {
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Writing in SCIP2 mode. Command is " << cmd <<
-                ", parameters length is " << param_length << '\n';
-        }
-        // Write the command
-        if (_port->Write(cmd, 2) < 2)
-            throw WriteError(19);
-        if (param_length > 0)
-        {
-            if (_port->Write(param, param_length) < param_length)
-                throw WriteError(20);
-        }
-        if (_port->Write("\n", 1) < 1)
-            throw WriteError(21);
-
-        // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        _read_line(response, 3 + param_length);
-        // Check the echo is correct
-        if (response[0] != cmd[0] || response[1] != cmd[1])
-            throw CommandEchoError(cmd, response);
-        // Then compare the parameters
-        if (param_length > 0)
-        {
-            if (memcmp(&response[2], param, param_length) != 0)
-                throw ParamEchoError(cmd);
-        }
-
-        // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Command response status: " << response[0] << response[1] <<
-                '\n';
-        }
-        // Check the status code is OK
-        response[2] = '\0';
-        if (!(response[0] == '0' && response[1] == '0') &&
-            !(response[0] == '9' && response[1] == '9'))
-        {
-            if (extra_ok != NULL)
-            {
-                if (response[0] != extra_ok[0] || response[1] != extra_ok[1])
-                {
-                    // There is an extra line feed after an error status
-                    // (signalling end of message)
-                    _skip_lines(1);
-                    throw ResponseError(response, cmd);
-                }
-            }
-            else
-            {
-                // There is an extra line feed after an error status
-                // (signalling end of message)
-                _skip_lines(1);
-                throw ResponseError(response, cmd);
-            }
-        }
-        statusCode = atoi(response);
-        // All OK, data starts at beginning of port's buffer
-    }
-    else
-        throw UnknownScipVersionError();
-
-    return statusCode;
-}
-
-void Sensor::_get_and_set_scip_version()
-{
-    bool scip2Failed = false;
-
-
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ <<
-            "() Testing SCIP protocol version.\n";
-    // Try SCIP version 2 first by sending an info command
-    try
-    {
-        _send_command("VV", NULL, 0, NULL);
-    }
-    catch (BaseError)
-    {
-        // That didn't work too well...
-        if (_verbose)
-            _err_output << "Sensor::" << __func__ <<
-                "() Initial SCIP version 2 test failed.\n";
-        scip2Failed = true;
-    }
-
-    if (scip2Failed)
-    {
-        // Currently using SCIP version 1
-        // Get the firmware version and check if we can move to SCIP version 2
-        _scip_version = 1;
-
-        _port->Flush();
-        try
-        {
-            _send_command("V", NULL, 0, NULL);
-        }
-        catch (BaseError)
-        {
-            throw ScipVersionError();
-        }
-        // Skip the vendor and product info
-        _skip_lines(2);
-        // Get the firmware line
-        char buffer[SCIP1_LINE_LENGTH];
-        memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
-        _read_line(buffer);
-
-        if (strncmp(buffer, "FIRM:", 5) != 0)
-            throw MissingFirmSpecError();
-        // Pull out the major version number
-        // Note that although lasers such as the UTM-30LX appear to use a
-        // different firmware version format, that doesn't matter because they
-        // don't support SCIP v1 and so shouldn't get to this point anyway - if
-        // they do, it's an uncaught error.
-        int majorVer = strtol(&buffer[5], NULL, 10);
-        if (errno == ERANGE)
-            throw FirmwareError();
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() Firmware major version is " << majorVer <<
-                '\n';
-        }
-        // Dump the rest of the V command result (one of these will be the
-        // empty last line)
-        _skip_lines(3);
-
-        // If the firmware version is less than 3, we're stuck with SCIP
-        // version 1.
-        if (majorVer < 3)
-        {
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Firmware does not support SCIP version 2; using SCIP "
-                    "version 1.\n";
-            return;
-        }
-        // Otherwise we can try SCIP version 2
-        else
-        {
-            _port->Flush();
-            // We'll hijack the _send_command function a bit here. Normally it
-            // takes 1-byte commands, (we're currently using SCIP version 1,
-            // remember), but the command to change to SCIP version 2 is 7
-            // bytes long (why did they have to do it that way?). So send the
-            // first byte as the command and the other 6 as parameters.
-            try
-            {
-                _send_command("S", "CIP2.0", 6, NULL);
-            }
-            catch (BaseError)
-            {
-                if (_verbose)
-                    _err_output << "Sensor::" << __func__ <<
-                        "() Could not change to SCIP version 2; using SCIP "
-                        "version 1.\n";
-                return;
-            }
-            // There'll be a trailing line on the end
-            _skip_lines(1);
-
-            // Changed to SCIP version 2
-            if (_verbose)
-                _err_output << "Sensor::" << __func__ <<
-                    "() Using SCIP version 2.\n";
-            _scip_version = 2;
-            return;
-        }
-    }
-    else
-    {
-        // Currently using SCIP version 2
-        _scip_version = 2;
-
-        // Dump the rest of the result
-        _skip_lines(6);
-        if (_verbose)
-            _err_output << "Sensor::" << __func__ <<
-                "() Using SCIP version 2.\n";
-        return;
-    }
-
-    // Fallback case if didn't find a good SCIP version and return above
-    throw ScipVersionError();
-}
-
-void Sensor::_get_defaults()
-{
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ <<
-            "() Getting default values.\n";
-
-    // Get the laser's info
-    SensorInfo info;
-    get_sensor_info(&info);
-
-    _min_angle = info.min_angle;
-    _max_angle = info.max_angle;
-    _resolution = info.resolution;
-    _first_step = info.first_step;
-    _last_step = info.last_step;
-    _front_step = info.front_step;
-    _max_range = info.max_range;
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ <<
-            "() Got default values: " << _min_angle << " " << _max_angle <<
-            " " << _resolution << " " << _first_step << " " << _last_step <<
-            " " << _front_step << " " << _max_range << '\n';
-    }
-}
-
-void Sensor::_process_vv_line(const char *buffer, SensorInfo *info)
-{
-    if (strncmp(buffer, "VEND", 4) == 0)
-        info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
-    else if (strncmp(buffer, "PROD", 4) == 0)
-    {
-        info->product = &buffer[5]; // Product info
-        // Also do a check to see if this is a UTM-30LX, because we need to
-        // work around a checksum problem and the error messages for data
-        // change if this is the case
-        if (strstr(buffer, "UTM-30LX") != NULL)
-        {
-            _sensorIsUTM30LX = true;
-            _enable_checksum_workaround = true;
-        }
-        else
-            _sensorIsUTM30LX = false;
-    }
-    else if (strncmp(buffer, "FIRM", 4) == 0)
-        info->firmware = &buffer[5]; // Firmware version
-    else if (strncmp(buffer, "PROT", 4) == 0)
-        info->protocol = &buffer[5]; // Protocol version
-    else if (strncmp(buffer, "SERI", 4) == 0)
-        info->serial = &buffer[5]; // Serial number
-    else if (!_ignore_unknowns)
-        throw UnknownLineError(buffer);
-}
-
-void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
-{
-    if (strncmp(buffer, "MODL", 4) == 0)
-        info->model = &buffer[5];   // Model
-    // On to the fun ones that require parsing
-    else if (strncmp(buffer, "DMIN", 4) == 0)
-        info->min_range = atoi(&buffer[5]);
-    else if (strncmp(buffer, "DMAX", 4) == 0)
-        info->max_range = atoi(&buffer[5]);
-    else if (strncmp(buffer, "ARES", 4) == 0)
-        info->steps = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AMIN", 4) == 0)
-        info->first_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AMAX", 4) == 0)
-        info->last_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AFRT", 4) == 0)
-        info->front_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "SCAN", 4) == 0)
-        info->standard_speed = atoi(&buffer[5]);
-    else if (!_ignore_unknowns)
-        throw UnknownLineError(buffer);
-}
-
-void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
-{
-    if (strncmp(buffer, "MODL", 4) == 0)
-        // Do nothing here - we already know this value from PP
-        return;
-    else if (strncmp(buffer, "LASR", 4) == 0)
-    {
-        if (strncmp(&buffer[5], "OFF", 3) == 0)
-            info->power = false;
-        else
-            info->power = true;
-    }
-    else if (strncmp(buffer, "SCSP", 4) == 0)
-    {
-        if (strncmp(&buffer[5], "Initial", 7) == 0)
-        {
-            // Unchanged motor speed
-            if (sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
-            {
-                throw ParseError(buffer, "Motor speed");
-            }
-            info->speed_level = 0;
-        }
-        else
-        {
-            // Changed motor speed, format is:
-            // <level>%<ignored string>(<speed>[rpm])
-            if (sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
-                        &info->speed) != 2)
-            {
-                throw ParseError(buffer, "Motor speed");
-            }
-        }
-    }
-    else if (strncmp(buffer, "MESM", 4) == 0)
-        info->measure_state = &buffer[5];
-    else if (strncmp(buffer, "SBPS", 4) == 0)
-    {
-        if (strncmp(&buffer[5], "USB only", 8) == 0 ||
-            strncmp(&buffer[5], "USB Full Speed", 14) == 0)
-        {
-            // No baud rate for USB-only devices such as the UHG-08LX
-            info->baud = 0;
-        }
-        else if (sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
-            throw ParseError(buffer, "Baud rate");
-    }
-    else if (strncmp(buffer, "TIME", 4) == 0)
-    {
-        if (sscanf(buffer, "TIME:%x", &info->time) != 1)
-            throw ParseError(buffer, "Timestamp");
-    }
-    else if (strncmp(buffer, "STAT", 4) == 0)
-        info->sensor_diagnostic = &buffer[5];
-    else if (!_ignore_unknowns)
-        throw UnknownLineError(buffer);
-}
-
-void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
-{
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges.\n";
-
-    // This will automatically take care of whether it actually needs to
-    // (re)allocate or not.
-    data->allocate_data(num_steps);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
-    data->_error = false;
-
-    // 2 byte data is easy since it fits neatly in a 64-byte block
-    char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentStep = 0;
-    int numBytesInLine = 0;
-    while (true)
-    {
-        // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
-        // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
-            break;
-        // Process pairs of bytes until we encounter the end of the line
-        for (int ii = 0; ii < numBytesInLine; ii += 2, currentStep++)
-        {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
-            {
-                // Line feed in the middle of a data block? Why?
-                throw MisplacedLineFeedError();
-            }
-            data->_ranges[currentStep] = decode_2_byte_value(&buffer[ii]);
-            if (data->_ranges[currentStep] < 20)
-                data->_error = true;
-        }
-        // End of this line. Go around again.
-    }
-
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges.\n";
-    if (currentStep != num_steps)
-        throw DataCountError();
-}
-
-void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
-{
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges.\n";
-
-    // This will automatically take care of whether it actually needs to
-    // (re)allocate or not.
-    data->allocate_data(num_steps);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
-    data->_error = false;
-
-    // 3 byte data is a pain because it crosses the line boundary, it may
-    // overlap by 0, 1 or 2 bytes
-    char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentStep = 0;
-    int numBytesInLine = 0, splitCount = 0;
-    char splitValue[3];
-    while (true)
-    {
-        // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
-        // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
-            break;
-        // Process triplets of bytes until we encounter or overrun the end of
-        // the line
-        for (int ii = 0; ii < numBytesInLine;)
-        {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
-            {
-                // Line feed in the middle of a line? Why?
-                throw MisplacedLineFeedError();
-            }
-            if (ii == numBytesInLine - 2)       // Short 1 byte
-            {
-                splitValue[0] = buffer[ii];
-                splitValue[1] = buffer[ii + 1];
-                // Will be reset on the next iteration, after it's used
-                splitCount = 1;
-                ii += 2;
-            }
-            else if (ii == numBytesInLine - 1)  // Short 2 bytes
-            {
-                splitValue[0] = buffer[ii];
-                // Will be reset on the next iteration, after it's used
-                splitCount = 2;
-                ii += 1;
-            }
-            else
-            {
-                if (splitCount == 1)
-                {
-                    splitValue[2] = buffer[ii++];
-                    data->_ranges[currentStep] =
-                        decode_3_byte_value(splitValue);
-                }
-                else if (splitCount == 2)
-                {
-                    splitValue[1] = buffer[ii++];
-                    splitValue[2] = buffer[ii++];
-                    data->_ranges[currentStep] =
-                        decode_3_byte_value(splitValue);
-                }
-                else
-                {
-                    data->_ranges[currentStep] =
-                        decode_3_byte_value(&buffer[ii]);
-                    ii += 3;
-                }
-                if (data->_ranges[currentStep] > _max_range)
-                {
-                    _err_output << "WARNING: Sensor::" << __func__ <<
-                        "() Value at step " << currentStep <<
-                        " beyond maximum range: " <<
-                        data->_ranges[currentStep] << " (raw bytes: ";
-                    if (splitCount != 0)
-                        _err_output << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")\n";
-                    else
-                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
-                            ")\n";
-                }
-                else if (data->_ranges[currentStep] < 20)
-                    data->_error = true;
-                currentStep++;
-                splitCount = 0;     // Reset this here now that it's been used
-            }
-        }
-        // End of this line. Go around again.
-    }
-
-    if (_verbose)
-        _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges.\n";
-    if (currentStep != num_steps)
-        throw DataCountError();
-}
-
-void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
-        unsigned int num_steps)
-{
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
-            num_steps << " ranges and intensities.\n";
-    }
-
-    // This will automatically take care of whether it actually needs to
-    // (re)allocate or not.
-    data->allocate_data(num_steps, true);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
-
-    // 3 byte data is a pain because it crosses the line boundary, it may
-    // overlap by 0, 1 or 2 bytes
-    char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentRange = 0, currentIntensity = 0;
-    int numBytesInLine = 0, splitCount = 0;
-    char splitValue[3];
-    bool nextIsIntensity = false;
-    while (true)
-    {
-        // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
-        // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
-            break;
-        // Process triplets of bytes until we encounter or overrun the end of
-        // the line
-        for (int ii = 0; ii < numBytesInLine;)
-        {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
-            {
-                // Line feed in the middle of a line? Why?
-                throw MisplacedLineFeedError();
-            }
-            if (ii == numBytesInLine - 2)       // Short 1 byte
-            {
-                splitValue[0] = buffer[ii];
-                splitValue[1] = buffer[ii + 1];
-                // Will be reset on the next iteration, after it's used
-                splitCount = 1;
-                ii += 2;
-            }
-            else if (ii == numBytesInLine - 1)  // Short 2 bytes
-            {
-                splitValue[0] = buffer[ii];
-                // Will be reset on the next iteration, after it's used
-                splitCount = 2;
-                ii += 1;
-            }
-            else
-            {
-                if (splitCount == 1)
-                {
-                    splitValue[2] = buffer[ii++];
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(splitValue);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(splitValue);
-                }
-                else if (splitCount == 2)
-                {
-                    splitValue[1] = buffer[ii++];
-                    splitValue[2] = buffer[ii++];
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(splitValue);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(splitValue);
-                }
-                else
-                {
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(&buffer[ii]);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(&buffer[ii]);
-                    ii += 3;
-                }
-                if (data->_ranges[currentRange] > _max_range &&
-                        !nextIsIntensity)
-                {
-                    _err_output << "WARNING: Sensor::" << __func__ <<
-                        "() Value at step " << currentRange <<
-                        " beyond maximum range: " <<
-                        data->_ranges[currentRange] << " (raw bytes: ";
-                    if (splitCount != 0)
-                        _err_output << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")\n";
-                    else
-                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
-                            ")\n";
-                }
-                else if (data->_ranges[currentRange] < 20)
-                    data->_error = true;
-                if (nextIsIntensity)
-                    currentIntensity++;
-                else
-                    currentRange++;
-                // Reset this here now that it's been used
-                splitCount = 0;
-                // Alternate between range and intensity values
-                nextIsIntensity = !nextIsIntensity;
-            }
-        }
-        // End of this line. Go around again.
-    }
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentRange << " ranges and " << currentIntensity <<
-            " intensities (expected " << num_steps << ").\n";
-    }
-    if (currentRange != num_steps || currentIntensity != num_steps)
-        throw DataCountError();
-}
-
-int Sensor::_confirm_checksum(const char *buffer, int length,
-        int expected_sum)
-{
-    int checksum = 0;
-    // Start by adding the byte values
-    for (int ii = 0; ii < length; ii++)
-        checksum += buffer[ii];
-    // Take the lowest 6 bits
-    checksum &= 0x3F;
-    // Add 0x30
-    checksum += 0x30;
-
-    if (_verbose)
-    {
-        _err_output << "Sensor::" << __func__ <<
-            "() Calculated checksum = " << checksum << " (" <<
-            static_cast<char>(checksum) << "), given checksum = " <<
-            expected_sum << " (" << static_cast<char> (expected_sum) <<
-            ")\n";
-    }
-    if (checksum != expected_sum)
-        throw ChecksumError(expected_sum, checksum);
-
-    return checksum;
-}
-
-}; // namespace hokuyo_aist
-

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 03:20:45 UTC (rev 502)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 04:25:13 UTC (rev 503)
@@ -20,527 +20,10 @@
 #ifndef HOKUYO_AIST_H__
 #define HOKUYO_AIST_H__
 
-#include <flexiport/port.h>
-#include <string>
+#include "hokuyo_errors.h"
+#include "sensor_info.h"
+#include "scan_data.h"
+#include "sensor.h"
 
-#if defined(WIN32)
-    typedef unsigned char           uint8_t;
-    typedef unsigned int            uint32_t;
-    #if defined(HOKUYO_AIST_STATIC)
-        #define HOKUYO_AIST_EXPORT
-    #elif defined(HOKUYO_AIST_EXPORTS)
-        #define HOKUYO_AIST_EXPORT       __declspec(dllexport)
-    #else
-        #define HOKUYO_AIST_EXPORT       __declspec(dllimport)
-    #endif
-#else
-    #include <stdint.h>
-    #define HOKUYO_AIST_EXPORT
-#endif
-
-/** @ingroup gbx_library_hokuyo_aist
-@{
-*/
-
-namespace hokuyo_aist
-{
-
-#if defined(WIN32)
-// Exporting data members on Windows is a bloody pain (they have to be
-// initialised somewhere else), so we'll use #define's on Win32 and lose the
-// type safety.
-/// Read error while reading from the laser.
-#define HOKUYO_ERR_READ            1
-/// Write error while writing to the laser.
-#define HOKUYO_ERR_WRITE           2
-/// Error in the SCIP protocol.
-#define HOKUYO_ERR_PROTOCOL        3
-/// Error while changing baud rate.
-#define HOKUYO_ERR_CHANGEBAUD      4
-/// Failed to connect to the laser.
-#define HOKUYO_ERR_CONNECT_FAILED  5
-/// Failed to close a port.
-#define HOKUYO_ERR_CLOSE_FAILED    6
-/// No destination buffer provided for range readings.
-#define HOKUYO_ERR_NODESTINATION   7
-/// Bad firmware version.
-#define HOKUYO_ERR_BADFIRMWARE     8
-/// Unknown/unsupported SCIP protocol version.
-#define HOKUYO_ERR_SCIPVERSION     9
-/// Memory allocation error.
-#define HOKUYO_ERR_MEMORY          10
-/// Unsupported function error.
-#define HOKUYO_ERR_UNSUPPORTED     11
-/// Argument error
-#define HOKUYO_ERR_BADARG          12
-/// No data received error
-#define HOKUYO_ERR_NODATA          13
-/// Not a serial connection error
-#define HOKUYO_ERR_NOTSERIAL       14
-#else
-/// Read error while reading from the laser.
-const unsigned int HOKUYO_ERR_READ            = 1;
-/// Write error while writing to the laser.
-const unsigned int HOKUYO_ERR_WRITE           = 2;
-/// Error in the SCIP protocol.
-const unsigned int HOKUYO_ERR_PROTOCOL        = 3;
-/// Error while changing baud rate.
-const unsigned int HOKUYO_ERR_CHANGEBAUD      = 4;
-/// Failed to connect to the laser.
-const unsigned int HOKUYO_ERR_CONNECT_FAILED  = 5;
-/// Failed to close a port.
-const unsigned int HOKUYO_ERR_CLOSE_FAILED    = 6;
-/// No destination buffer provided for range readings.
-const unsigned int HOKUYO_ERR_NODESTINATION   = 7;
-/// Bad firmware version.
-const unsigned int HOKUYO_ERR_BADFIRMWARE     = 8;
-/// Unknown/unsupported SCIP protocol version.
-const unsigned int HOKUYO_ERR_SCIPVERSION     = 9;
-/// Memory allocation error.
-const unsigned int HOKUYO_ERR_MEMORY          = 10;
-/// Unsupported function error.
-const unsigned int HOKUYO_ERR_UNSUPPORTED     = 11;
-/// Argument error
-const unsigned int HOKUYO_ERR_BADARG          = 12;
-/// No data received error
-const unsigned int HOKUYO_ERR_NODATA          = 13;
-/// Not a serial connection error
-const unsigned int HOKUYO_ERR_NOTSERIAL       = 14;
-#endif // defined(WIN32)
-
-
-/** @brief Sensor information.
-
-Returned from a call to @get_sensor_info. Contains various information about the
-laser scanner such as firmware version and maximum possible range. */
-class HOKUYO_AIST_EXPORT SensorInfo
-{
-    public:
-        friend class Sensor;
-
-        SensorInfo();
-        SensorInfo(const SensorInfo &rhs);
-
-        /// @brief Assignment operator.
-        SensorInfo& operator=(const SensorInfo &rhs);
-
-        /// @brief Format the entire object into a string.
-        std::string as_string();
-
-        // Version details.
-        /// Vendor name.
-        std::string vendor;
-        /// Product name.
-        std::string product;
-        /// Firmware version.
-        std::string firmware;
-        /// Protocol version in use.
-        std::string protocol;
-        /// Serial number of this device.
-        std::string serial;
-
-        // Specification details.
-        /// Sensor model number.
-        std::string model;
-        /// Minimum detectable range (mm).
-        unsigned int min_range;
-        /// Maximum detectable range (mm).
-        unsigned int max_range;
-        /// Number of steps in a 360-degree scan.
-        unsigned int steps;
-        /// First scanable step of a full scan.
-        unsigned int first_step;
-        /// Last scanable step of a full scan.
-        unsigned int last_step;
-        /// Step number that points forward (typically the centre of a full
-        /// scan).
-        unsigned int front_step;
-        /// Standard motor speed (rpm).
-        unsigned int standard_speed;
-
-        // Status details.
-        /// Operational status - illuminated or not.
-        bool power;
-        /// Current motor speed (rpm).
-        unsigned int speed;
-        /// Speed level (0 for default)
-        unsigned short speed_level;
-        /// Measurement state.
-        std::string measure_state;
-        /// Baud rate.
-        unsigned int baud;
-        /// Current sensor time (s).
-        unsigned int time;
-        /// Diagnostic status string.
-        std::string sensor_diagnostic;
-
-        // Calculated details
-        /// Minimum possible scan angle (radians). Scans go anti-clockwise with
-        /// negative angles on the right.
-        double min_angle;
-        /// Maximum possible scan angle (radians). Scans go anti-clockwise with
-        /// negative angles on the right.
-        double max_angle;
-        /// Angle between two scan points (radians).
-        double resolution;
-        /// Total number of steps in a full scan (lastStep - firstStep).
-        unsigned int scanable_steps;
-
-    private:
-        void set_defaults();
-        void calculate_values();
-}; // class SensorInfo
-
-
-/** @brief Structure to store data returned from the laser scanner. */
-class HOKUYO_AIST_EXPORT ScanData
-{
-    public:
-        friend class Sensor;
-
-        /// This constructor creates an empty ScanData with no data currently
-        /// allocated.
-        ScanData();
-        /// This constructor performs a deep copy of existing range data.
-        ScanData(uint32_t* ranges, unsigned int length, bool error,
-                unsigned int time);
-        /// This constructor performs a deep copy of existing range and
-        /// intensity data.
-        ScanData(uint32_t* ranges, uint32_t* intensities,
-                unsigned int length, bool error, unsigned int time);
-        /// This copy constructor performs a deep copy of present data.
-        ScanData(const ScanData &rhs);
-        ~ScanData();
-
-        /** @brief Return a pointer to array of range readings in millimetres.
-
-        Values less than 20mm indicate an error. Check the error value for the
-        data to see a probable cause for the error. Most of the time, it will
-        just be an out-of-range reading. */
-        const uint32_t* ranges() const
-            { return _ranges; }
-        /// @brief Return a pointer to an array of intensity readings.
-        const uint32_t* intensities() const
-            { return _intensities; }
-        /// @brief Get the number of samples in the data.
-        unsigned int length() const { return _length; }
-        /** @brief Indicates if one or more steps had an error.
-
-        A step's value will be less than 20 if it had an error. Use @ref
-        error_code_to_string to get a textual representation of the error. */
-        bool get_error_status() const { return _error; }
-        /// @brief Return a string representing the error for the given error
-        /// code.
-        std::string error_code_to_string(uint32_t error_code);
-        /** @brief Get the time stamp of the data in milliseconds
-
-        This value is only available using SCIP version 2). */
-        unsigned int time_stamp() const { return _time; }
-
-        /// @brief Assignment operator.
-        ScanData& operator=(const ScanData &rhs);
-        /** @brief Subscript operator.
-
-        Provides direct access to an element of the range data. */
-        uint32_t operator[](unsigned int index);
-
-        /// @brief Format the entire object into a string.
-        std::string as_string();
-
-        /// @brief Force the data to clean up.
-        void clean_up();
-
-    protected:
-        uint32_t *_ranges;
-        uint32_t *_intensities;
-        unsigned int _length;
-        bool _error;
-        unsigned int _time;
-        bool _sensorIsUTM30LX;
-
-        void allocate_data(unsigned int length,
-                bool include_intensities = false);
-}; // class ScanData
-
-
-/** @brief Hokuyo laser scanner class.
-
-Provides an interface for interacting with a Hokuyo laser scanner using SCIP
-protocol version 1 or 2. The FlexiPort library is used to implement the data
-communications with the scanner. See its documentation for details on
-controlling the connection.
-
-To use a serial connection, ensure that you do not also have a USB cable
-connected, as this will force the scanner into USB mode, preventing the serial
-connection from functioning correctly.
-
-All functions may throw instances of @ref BaseError or its children.
-Exceptions from @ref FlexiPort may also occur. */
-class HOKUYO_AIST_EXPORT Sensor
-{
-    public:
-        Sensor();
-        Sensor(std::ostream& err_output);
-        ~Sensor();
-
-        /// @brief Open the laser scanner and begin scanning.
-        void open(std::string port_options);
-
-        /** @brief Open the laser scanner and begin scanning, probing the baud
-        rate.
-
-        If the port is a serial connection and communication with the laser
-        fails at the given baud rate, the alternative baud rates supported by
-        the device are tried (see @ref set_baud for these) in order from fastest
-        to slowest.
-
-        @return The baud rate at which connection with the laser succeeded, or
-        0 for non-serial connections. */
-        unsigned int open_with_probing(std::string port_options);
-
-        /// @brief Close the connection to the laser scanner.
-        void close();
-
-        /// @brief Checks if the connection to the laser scanner is open.
-        bool is_open() const;
-
-        /// @brief Switch the laser scanner on or off.
-        void set_power(bool on);
-
-        /** @brief Change the baud rate when using a serial connection.
-
-        Valid rates are 19.2Kbps, 38.4Kbps, 57.6Kbps, 115.2Kbps, 250.0Kbps,
-        500.0Kbps, 750.0Kbps (dependent on those available in FlexiPort). */
-        void set_baud(unsigned int baud);
-
-        /** @brief Reset the laser scanner to its default settings.
-
-        Not available with the SCIP v1 protocol. */
-        void reset();
-
-        /** @brief Set the speed at which the scanner's sensor spins.
-
-        Set the speed to 0 to have it reset to the default value, and 99 to
-        reset it to the initial (startup) value. Values between 1 and 10
-        specify a ratio of the default speed. The speeds in revolutions per
-        minute that these correspond to will depend on the scanner model. For
-        example, for a URG-04LX, they are (from 1 to 10) 594, 588, 576, 570,
-        564, 558, 552, 546, and 540 rpm.
-
-        Not available with the SCIP v1 protocol. */
-        void set_motor_speed(unsigned int speed);
-
-        /** @brief Switch the scanner between normal and high sensitivity
-        modes. */
-        void set_high_sensitivity(bool on);
-
-        /** @brief Get various information about the scanner.
-
-        Much of the information is not available with the SCIP v1 protocol. */
-        void get_sensor_info(SensorInfo *info);
-
-        /** @brief Get the value of the scanner's clock in milliseconds.
-
-        Not available with the SCIP v1 protocol. */
-        unsigned int get_time();
-
-        /** @brief Get the latest scan data from the scanner.
-
-        This function requires a pointer to a @ref ScanData object. It will
-        allocate space in this object as necessary for storing range data. If
-        the passed-in @ref ScanData object already has the correct quantity
-        of space to store the range data, it will not be re-allocated. If it
-        does not have any space, it will be allocated. If it has space, but it
-        is the wrong size, it will be re-allocated. This means you can
-        repeatedly send the same @ref ScanData object without having to worry
-        about allocating its data, whether it will change or not, while also
-        avoiding excessive allocations.
-
-        @param data Pointer to a @ref ScanData object to store the range
-        readings in.
-        @param cluster_count The number of readings to cluster together into a
-        single reading. The minimum value from a cluster is returned as the
-        range for that cluster.
-        @param start_step The first step to get ranges from. Set to -1 for the
-        first scannable step.
-        @param end_step The last step to get ranges from. Set to -1 for the last
-        scannable step.
-        @return The number of range readings read into @ref data. */
-        unsigned int get_ranges(ScanData *data, int start_step = -1,
-                int end_step = -1, unsigned int cluster_count = 1);
-
-        /** @brief Get the latest scan data from the scanner.
-
-        @param data Pointer to a @ref ScanData object to store the range
-        readings in.
-        @param start_angle The angle to get range readings from. Exclusive; if
-        this falls between two steps the step inside the angle will be
-        returned, but the step outside won't.
-        @param end_angle The angle to get range readings to. Exclusive; if this
-        falls between two steps the step inside the angle will be returned, but
-        the step outside won't.
-        @param cluster_count The number of readings to cluster together into a
-        single reading. The minimum value from a cluster is returned as the
-        range for that cluster.
-        @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_by_angle(ScanData *data, double start_angle,
-                double end_angle, unsigned int cluster_count = 1);
-
-        /** @brief Get a new scan from the scanner.
-
-        Unlike @ref get_ranges, which returns the most recent scan the scanner
-        took, this function will request a new scan. This means it will wait
-        while the scanner performs the scan, which means the rate at which
-        scans can be retrieved using this function is less than with @ref
-        get_ranges. Otherwise behaves identicallty to @ref get_ranges.
-
-        Not available with the SCIP v1 protocol.
-
-        @note The command used to retrieve a fresh scan is also used for the
-        continuous scanning mode (not yet supported by this library). After
-        completing a scan, it will turn the laser off (in anticipation of
-        another continuous scan command being sent, which will automatically
-        turn the laser back on again). If you want to mix @ref get_new_ranges and
-        @ref get_ranges, you will need to turn the laser on after each call to
-        @ref get_new_ranges.
-
-        @param data Pointer to a @ref ScanData object to store the range
-        readings in.
-        @param cluster_count The number of readings to cluster together into a
-        single reading. The minimum value from a cluster is returned as the
-        range for that cluster.
-        @param start_step The first step to get ranges from. Set to -1 for the
-        first scannable step.
-        @param end_step The last step to get ranges from. Set to -1 for the last
-        scannable step.
-        @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges(ScanData *data, int start_step = -1,
-                int end_step = -1, unsigned int cluster_count = 1);
-
-        /** @brief Get a new scan from the scanner.
-
-        Not available with the SCIP v1 protocol.
-
-        @param data Pointer to a @ref ScanData object to store the range
-        readings in.
-        @param start_angle The angle to get range readings from. Exclusive; if
-        this falls between two steps the step inside the angle will be
-        returned, but the step outside won't.
-        @param end_angle The angle to get range readings to. Exclusive; if this
-        falls between two steps the step inside the angle will be returned, but
-        the step outside won't.
-        @param cluster_count The number of readings to cluster together into a
-        single reading. The minimum value from a cluster is returned as the
-        range for that cluster.
-        @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_by_angle(ScanData *data,
-                double start_angle, double end_angle,
-                unsigned int cluster_count = 1);
-
-        /** @brief Get a new scan from the scanner with intensity data.
-
-        Unlike @ref get_ranges, which returns the most recent scan the scanner
-        took, this function will request a new scan. This means it will wait
-        while the scanner performs the scan.  Otherwise behaves identicallty to
-        @ref get_ranges.
-
-        Not available with the SCIP v1 protocol.
-
-        @note The command used to retrieve a fresh scan is also used for the
-        continuous scanning mode (not yet supported by this library). After
-        completing a scan, it will turn the laser off (in anticipation of
-        another continuous scan command being sent, which will automatically
-        turn the laser back on again). If you want to mix @ref get_new_ranges and
-        @ref get_ranges, you will need to turn the laser on after each call to
-        @ref get_new_ranges.
-
-        @param data Pointer to a @ref ScanData object to store the range
-        readings in.
-        @param cluster_count The number of readings to cluster together into a
-        single reading. The minimum value from a cluster is returned as the
-        range for that cluster.
-        @param start_step The first step to get ranges from. Set to -1 for the
-        first scannable step.
-        @param end_step The last step to get ranges from. Set to -1 for the last
-        scannable step.
-        @return The number of range readings read into @ref data. */

@@ Diff output truncated at 100000 characters. @@

This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 504
          http://gearbox.svn.sourceforge.net/gearbox/?rev=504&view=rev
Author:   gbiggs
Date:     2010-08-04 04:48:00 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Fixed linker errors

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/utils.h

Removed Paths:
-------------
    gearbox/trunk/src/hokuyo_aist/utils.cpp

Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-04 04:48:00 UTC (rev 504)
@@ -21,8 +21,7 @@
     set (srcs hokuyo_errors.cpp
               sensor_info.cpp
               scan_data.cpp
-              sensor.cpp
-              utils.cpp)
+              sensor.cpp)
 
     if (WIN32)
         if (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 04:48:00 UTC (rev 504)
@@ -36,8 +36,6 @@
 #include <sstream>
 #include <iostream>
 
-using namespace flexiport;
-
 #if defined(WIN32)
     #define __func__    __FUNCTION__
 #endif
@@ -171,6 +169,55 @@
 */
 
 ///////////////////////////////////////////////////////////////////////////////
+// Utility functions
+///////////////////////////////////////////////////////////////////////////////
+
+unsigned int decode_2_byte_value(char *data)
+{
+    unsigned int byte1, byte2;
+
+    byte1 = data[0] - 0x30;
+    byte2 = data[1] - 0x30;
+
+    return (byte1 << 6) + (byte2);
+}
+
+unsigned int decode_3_byte_value(char *data)
+{
+    unsigned int byte1, byte2, byte3;
+
+    byte1 = data[0] - 0x30;
+    byte2 = data[1] - 0x30;
+    byte3 = data[2] - 0x30;
+
+    return (byte1 << 12) + (byte2 << 6) + (byte3);
+}
+
+unsigned int decode_4_byte_value(char *data)
+{
+    unsigned int byte1, byte2, byte3, byte4;
+
+    byte1 = data[0] - 0x30;
+    byte2 = data[1] - 0x30;
+    byte3 = data[2] - 0x30;
+    byte4 = data[3] - 0x30;
+
+    return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
+}
+
+void number_to_string(unsigned int num, char *dest, int length)
+{
+#if defined(WIN32)
+    _snprintf(dest, length + 1, "%*d", length, num);
+#else
+    snprintf(dest, length + 1, "%*d", length, num);
+#endif
+    // Replace all leading spaces with '0'
+    for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
+        dest[ii] = '0';
+}
+
+///////////////////////////////////////////////////////////////////////////////
 // Sensor class
 ///////////////////////////////////////////////////////////////////////////////
 
@@ -268,7 +315,7 @@
             const unsigned int numBauds = 5;
             for (unsigned int ii = 0; ii < numBauds; ii++)
             {
-                reinterpret_cast<SerialPort*>(_port)->SetBaudRate(bauds[ii]);
+                reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(bauds[ii]);
                 try
                 {
                     _get_and_set_scip_version();
@@ -310,7 +357,7 @@
     }
 
     if (_port->GetPortType() == "serial")
-        return reinterpret_cast<SerialPort*>(_port)->GetBaudRate();
+        return reinterpret_cast<flexiport::SerialPort*>(_port)->GetBaudRate();
     else
         return 0;
 }
@@ -399,7 +446,7 @@
         _send_command("S", newBaud, 13, NULL);
         _skip_lines(1);
         // Change the port's baud rate
-        reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
+        reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
     }
     else if (_scip_version == 2)
     {
@@ -407,7 +454,7 @@
         _send_command("SS", newBaud, 6, "03");
         _skip_lines(1);
         // Change the port's baud rate
-        reinterpret_cast<SerialPort*>(_port)->SetBaudRate(baud);
+        reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
     }
     else
         throw UnknownScipVersionError();

Deleted: gearbox/trunk/src/hokuyo_aist/utils.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/utils.cpp	2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/utils.cpp	2010-08-04 04:48:00 UTC (rev 504)
@@ -1,70 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
- *
- * hokuyo_aist Hokuyo laser scanner driver.
- *
- * This distribution is licensed to you under the terms described in the
- * LICENSE file included in this distribution.
- *
- * This work is a product of the National Institute of Advanced Industrial
- * Science and Technology, Japan. Registration number: H22PRO-1086.
- *
- * This file is part of hokuyo_aist.
- *
- * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
- * http://www.opensource.org/licenses/eclipse-1.0.txt
- */
-
-#include "utils.h"
-
-#include <cstdio>
-
-using namespace hokuyo_aist;
-
-unsigned int decode_2_byte_value(char *data)
-{
-    unsigned int byte1, byte2;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-
-    return (byte1 << 6) + (byte2);
-}
-
-unsigned int decode_3_byte_value(char *data)
-{
-    unsigned int byte1, byte2, byte3;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-    byte3 = data[2] - 0x30;
-
-    return (byte1 << 12) + (byte2 << 6) + (byte3);
-}
-
-unsigned int decode_4_byte_value(char *data)
-{
-    unsigned int byte1, byte2, byte3, byte4;
-
-    byte1 = data[0] - 0x30;
-    byte2 = data[1] - 0x30;
-    byte3 = data[2] - 0x30;
-    byte4 = data[3] - 0x30;
-
-    return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
-}
-
-void number_to_string(unsigned int num, char *dest, int length)
-{
-#if defined(WIN32)
-    _snprintf(dest, length + 1, "%*d", length, num);
-#else
-    snprintf(dest, length + 1, "%*d", length, num);
-#endif
-    // Replace all leading spaces with '0'
-    for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
-        dest[ii] = '0';
-}
-

Modified: gearbox/trunk/src/hokuyo_aist/utils.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/utils.h	2010-08-04 04:25:13 UTC (rev 503)
+++ gearbox/trunk/src/hokuyo_aist/utils.h	2010-08-04 04:48:00 UTC (rev 504)
@@ -63,15 +63,6 @@
     }
 #endif
 
-/// Decode a 2 byte encoded value
-unsigned int decode_2_byte_value(char *data);
-/// Decode a 3 byte encoded value
-unsigned int decode_3_byte_value(char *data);
-/// Decode a 4 byte encoded value
-unsigned int decode_4_byte_value(char *data);
-/// Convert a number to a string
-void number_to_string(unsigned int num, char *dest, int length);
-
 } // namespace hokuyo_aist
 
 /** @} */


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 505
          http://gearbox.svn.sourceforge.net/gearbox/?rev=505&view=rev
Author:   gbiggs
Date:     2010-08-04 05:30:21 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Identify laser models

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-04 05:30:21 UTC (rev 505)
@@ -32,13 +32,13 @@
 
 ScanData::ScanData()
     : _ranges(NULL), _intensities(NULL), _length(0),
-    _error(false), _time(0)
+    _error(false), _time(0), _model(MODEL_UNKNOWN)
 {
 }
 
 ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
-        unsigned int time)
-    : _error(error), _time(time)
+        unsigned int time, LaserModel model)
+    : _error(error), _time(time), _model(model)
 {
     _length = length;
     if (_length == 0)
@@ -64,8 +64,9 @@
 }
 
 ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
-        unsigned int length, bool error, unsigned int time)
-    : _error(error), _time(time)
+        unsigned int length, bool error, unsigned int time,
+        LaserModel model)
+    : _error(error), _time(time), _model(model)
 {
     _length = length;
     if (_length == 0)
@@ -118,6 +119,7 @@
     }
     _error = rhs.get_error_status();
     _time = rhs.time_stamp();
+    _model = rhs.model();
 }
 
 ScanData::~ScanData()
@@ -128,7 +130,7 @@
 
 std::string ScanData::error_code_to_string(uint32_t error_code)
 {
-    if (_sensorIsUTM30LX)
+    if (_model == MODEL_UTM30LX)
     {
         switch (error_code)
         {
@@ -215,6 +217,7 @@
         _intensities = NULL;
         _error = rhs.get_error_status();
         _time = rhs.time_stamp();
+        _model = rhs.model();
     }
     else
     {
@@ -265,6 +268,7 @@
 
         _error = rhs.get_error_status();
         _time = rhs.time_stamp();
+        _model = rhs.model();
     }
 
     return *this;
@@ -281,7 +285,8 @@
 {
     std::stringstream ss;
 
-    ss << _length << " readings:\n";
+    ss << _length << " readings from model ";
+    ss << model_to_string(_model) << ":\n";
     for (unsigned int ii = 0; ii < _length; ii++)
         ss << _ranges[ii] << "\t";
     if (_intensities != NULL)

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-04 05:30:21 UTC (rev 505)
@@ -35,6 +35,8 @@
     #define HOKUYO_AIST_EXPORT
 #endif
 
+#include "sensor_info.h"
+
 #include <string>
 
 /** @ingroup gbx_library_hokuyo_aist
@@ -57,11 +59,12 @@
         ScanData();
         /// This constructor performs a deep copy of existing range data.
         ScanData(uint32_t* ranges, unsigned int length, bool error,
-                unsigned int time);
+                unsigned int time, LaserModel model);
         /// This constructor performs a deep copy of existing range and
         /// intensity data.
         ScanData(uint32_t* ranges, uint32_t* intensities,
-                unsigned int length, bool error, unsigned int time);
+                unsigned int length, bool error, unsigned int time,
+                LaserModel model);
         /// This copy constructor performs a deep copy of present data.
         ScanData(const ScanData &rhs);
         ~ScanData();
@@ -90,6 +93,8 @@
 
         This value is only available using SCIP version 2). */
         unsigned int time_stamp() const { return _time; }
+        /// Get the model of the laser that produced this scan.
+        LaserModel model() const { return _model; }
 
         /// @brief Assignment operator.
         ScanData& operator=(const ScanData &rhs);
@@ -110,7 +115,7 @@
         unsigned int _length;
         bool _error;
         unsigned int _time;
-        bool _sensorIsUTM30LX;
+        LaserModel _model;
 
         void allocate_data(unsigned int length,
                 bool include_intensities = false);

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 05:30:21 UTC (rev 505)
@@ -225,16 +225,16 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 Sensor::Sensor()
-    : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
-    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+    : _port(NULL), _err_output(std::cerr), _scip_version(2),
+    _verbose(false), _enable_checksum_workaround(false),
     _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
     _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
 {
 }
 
 Sensor::Sensor(std::ostream& err_output)
-    : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
-    _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+    : _port(NULL), _err_output(err_output), _scip_version(2),
+    _verbose(false), _enable_checksum_workaround(false),
     _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
     _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
 {
@@ -577,6 +577,8 @@
         // Get the product info line
         _read_line(buffer);
         info->product = &buffer[5];
+        // Only the URG-04LX supports SCIP1
+        _model = MODEL_URG04LX;
         // Get the firmware line
         _read_line(buffer);
         info->firmware = &buffer[5];
@@ -1153,7 +1155,7 @@
 // BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
 // whereby it does not include anything after and including "<-" in the
 // checksum calculation. A workaround is enabled in this function when
-// _sensorIsUTM30LX is true. In this case, if the checksum fails normally, it
+// _model is MODEL_UTM30LX. In this case, if the checksum fails normally, it
 // scans the line for "<-" and recalculates the checksum on the bytes up to
 // that point. This only happens in SCIP v2.
 int Sensor::_read_line_with_check(char *buffer, int expected_length,
@@ -1192,7 +1194,7 @@
     }
     catch (ProtocolError &e)
     {
-        if (_sensorIsUTM30LX && _enable_checksum_workaround)
+        if (_model == MODEL_UTM30LX && _enable_checksum_workaround)
         {
             // Here comes the UTM-30LX workaround
             char* hasComment = strstr(buffer, "<-");
@@ -1406,6 +1408,23 @@
     return statusCode;
 }
 
+
+void Sensor::_find_model(char const* buffer)
+{
+    if (strstr(buffer, "URG-04LX") != NULL)
+        _model = MODEL_URG04LX;
+    else if (strstr(buffer, "UTM-30LX") != NULL)
+    {
+        _model = MODEL_UTM30LX;
+        // Also enable the work around for a checksum problem in this
+        // model.
+        _enable_checksum_workaround = true;
+    }
+    else
+        _model = MODEL_UNKNOWN;
+}
+
+
 void Sensor::_get_and_set_scip_version()
 {
     bool scip2Failed = false;
@@ -1562,16 +1581,8 @@
     else if (strncmp(buffer, "PROD", 4) == 0)
     {
         info->product = &buffer[5]; // Product info
-        // Also do a check to see if this is a UTM-30LX, because we need to
-        // work around a checksum problem and the error messages for data
-        // change if this is the case
-        if (strstr(buffer, "UTM-30LX") != NULL)
-        {
-            _sensorIsUTM30LX = true;
-            _enable_checksum_workaround = true;
-        }
-        else
-            _sensorIsUTM30LX = false;
+        // Find the product model
+        _find_model(&buffer[5]);
     }
     else if (strncmp(buffer, "FIRM", 4) == 0)
         info->firmware = &buffer[5]; // Firmware version
@@ -1673,7 +1684,7 @@
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+    data->_model = _model;
     data->_error = false;
 
     // 2 byte data is easy since it fits neatly in a 64-byte block
@@ -1718,7 +1729,7 @@
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+    data->_model = _model;
     data->_error = false;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
@@ -1820,7 +1831,7 @@
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps, true);
-    data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+    data->_model = _model;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-04 05:30:21 UTC (rev 505)
@@ -292,7 +292,8 @@
         std::ostream& _err_output;
 
         uint8_t _scip_version;
-        bool _verbose, _sensorIsUTM30LX, _enable_checksum_workaround,
+        LaserModel _model;
+        bool _verbose, _enable_checksum_workaround,
              _ignore_unknowns;
         double _min_angle, _max_angle, _resolution;
         int _first_step, _last_step, _front_step;
@@ -306,6 +307,7 @@
         int _send_command(const char *cmd, const char *param, int param_length,
                 const char *extra_ok);
 
+        void _find_model(char const* buffer);
         void _get_and_set_scip_version();
         void _get_defaults();
         void _process_vv_line(const char *buffer, SensorInfo *info);

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-04 05:30:21 UTC (rev 505)
@@ -32,7 +32,7 @@
     : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
     front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
     baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
-    scanable_steps(0)
+    scanable_steps(0), detected_model(MODEL_UNKNOWN)
 {
 }
 

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-04 05:30:21 UTC (rev 505)
@@ -44,6 +44,26 @@
 namespace hokuyo_aist
 {
 
+/// Laser models
+enum LaserModel {
+    MODEL_URG04LX,
+    MODEL_UTM30LX,
+    MODEL_UNKNOWN
+};
+
+HOKUYO_AIST_EXPORT inline char const* model_to_string(LaserModel model)
+{
+    switch(model)
+    {
+        case MODEL_URG04LX:
+            return "URG-04LX";
+        case MODEL_UTM30LX:
+            return "UTM-30LX";
+        default:
+            return "Unknown model";
+    }
+}
+
 class Sensor;
 
 /** @brief Sensor information.
@@ -122,13 +142,15 @@
         double resolution;
         /// Total number of steps in a full scan (lastStep - firstStep).
         unsigned int scanable_steps;
+        /// Detected model of the laser.
+        LaserModel detected_model;
 
     private:
         void set_defaults();
         void calculate_values();
 }; // class SensorInfo
 
-} // namespace hokuyo_aist
+}; // namespace hokuyo_aist
 
 /** @} */
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 506
          http://gearbox.svn.sourceforge.net/gearbox/?rev=506&view=rev
Author:   gbiggs
Date:     2010-08-04 10:42:44 +0000 (Wed, 04 Aug 2010)

Log Message:
-----------
Added GE, RT, $IP commands, find sensor model

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 10:42:44 UTC (rev 506)
@@ -81,7 +81,8 @@
 //                return ss.str ();
 //        }
 //    }
-    else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+    else if (((cmd[0] == 'G' || cmd[0] == 'H') &&
+                (cmd[1] == 'D' || cmd[1] == 'E')) ||
              (cmd[0] == 'G' && cmd[1] == 'S'))
     {
         switch (error_code)
@@ -108,7 +109,8 @@
                 return ss.str();
         }
     }
-    else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+    else if (((cmd[0] == 'M' || cmd[0] == 'N') && 
+                (cmd[1] == 'D' || cmd[1] == 'E')) ||
              (cmd[0] == 'M' && cmd[1] == 'S'))
     {
         switch (error_code)
@@ -245,6 +247,22 @@
 //                return "";
 //        }
 //    }
+    else if (cmd[0] == 'D' && cmd[1] == 'B')
+    {
+        switch (error_code)
+        {
+            case 1:
+                return "Parameter error";
+            case 2:
+                return "Already running in the set mode";
+            case 3:
+                return "Already returned to normal mode";
+            case 4:
+                return "Selected mode does not match the SCIP version in use";
+            case 5:
+                return "Sensor has a physical malfunction";
+        }
+    }
     else
     {
         ss << "Unknown command: " << cmd[0] << cmd[1];
@@ -261,41 +279,44 @@
 std::string desc_code_to_string(unsigned int code)
 {
     static char const* const descriptions[] = {
-        "Timed out trying to read a line",
-        "No data received when trying to read a line.",
-        "Invalid data index.",
-        "Port is not open.",
-        "Unknown SCIP version.",
-        "Cannot change baud rate of non-serial connection.",
-        "Bad baud rate: ",
-        "SCIP version 1 does not support the reset command.",
-        "SCIP version 1 does not support the set motor speed command.",
-        "Invalid motor speed.",
-        "SCIP version 1 does not support the high sensitivity command.",
-        "No info object provided.",
-        "SCIP version 1 does not support the get time command.",
-        "No data received. Check data error code.",
-        "Start step is out of range.",
-        "End step is out of range.",
-        "SCIP version 1 does not support the get new ranges command.",
-        "SCIP version 1 does not support the get new ranges and intensities command.",
-        "Timed out while skipping.",
-        "Failed to write command byte.",
-        "Failed to write command parameters.",
-        "Failed to write termination character.",
-        "SCIP versions 1 and 2 failed.",
-        "Out-of-range firmware version.",
-        "Invalid checksum: ",
-        "Read a different number of range or intensity readings than were asked for.",
-        "Found line feed in a data block.",
-        "Unknown line: ",
-        "Parse error: ",
-        "'FIRM:' was not found when checking firmware version.",
-        "Bad response.",
-        "Incorrect command echo.",
-        "Incorrect parameters echo for command.",
-        "Not enough bytes to calculate checksum.",
-        "Incorrect line length received.",
+/* 0 */  "Timed out trying to read a line",
+/* 1 */  "No data received when trying to read a line.",
+/* 2 */  "Invalid data index.",
+/* 3 */  "Port is not open.",
+/* 4 */  "Unknown SCIP version.",
+/* 5 */  "Cannot change baud rate of non-serial connection.",
+/* 6 */  "Bad baud rate: ",
+/* 7 */  "SCIP version 1 does not support the reset command.",
+/* 8 */  "SCIP version 1 does not support the set motor speed command.",
+/* 9 */  "Invalid motor speed.",
+/* 10 */ "SCIP version 1 does not support the high sensitivity command.",
+/* 11 */ "No info object provided.",
+/* 12 */ "SCIP version 1 does not support the get time command.",
+/* 13 */ "No data received. Check data error code.",
+/* 14 */ "Start step is out of range.",
+/* 15 */ "End step is out of range.",
+/* 16 */ "SCIP version 1 does not support the get new ranges command.",
+/* 17 */ "SCIP version 1 does not support the get new ranges and intensities command.",
+/* 18 */ "Timed out while skipping.",
+/* 19 */ "Failed to write command byte.",
+/* 20 */ "Failed to write command parameters.",
+/* 21 */ "Failed to write termination character.",
+/* 22 */ "SCIP versions 1 and 2 failed.",
+/* 23 */ "Out-of-range firmware version.",
+/* 24 */ "Invalid checksum: ",
+/* 25 */ "Read a different number of range or intensity readings than were asked for.",
+/* 26 */ "Found line feed in a data block.",
+/* 27 */ "Unknown line: ",
+/* 28 */ "Parse error: ",
+/* 29 */ "'FIRM:' was not found when checking firmware version.",
+/* 30 */ "Bad response.",
+/* 31 */ "Incorrect command echo.",
+/* 32 */ "Incorrect parameters echo for command.",
+/* 33 */ "Not enough bytes to calculate checksum.",
+/* 34 */ "Incorrect line length received.",
+/* 35 */ "SCIP version 1 does not support the semi-reset command.",
+/* 36 */ "SCIP version 1 does not support the get ranges and intensities command.",
+/* 37 */ "Error configuring IP address.",
     };
 
     return std::string(descriptions[code]);

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 10:42:44 UTC (rev 506)
@@ -281,6 +281,16 @@
 }; // class IndexError
 
 
+/// Set IP error class
+class HOKUYO_AIST_EXPORT SetIPError: public RuntimeError
+{
+    public:
+        SetIPError()
+            : RuntimeError(37, "SetIPError")
+        {}
+}; // class SetIPError
+
+
 /// Invalid motor speed error class
 class HOKUYO_AIST_EXPORT MotorSpeedError: public ArgError
 {

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-04 10:42:44 UTC (rev 506)
@@ -36,6 +36,7 @@
 {
 }
 
+
 ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
         unsigned int time, LaserModel model)
     : _error(error), _time(time), _model(model)
@@ -63,6 +64,7 @@
     }
 }
 
+
 ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
         unsigned int length, bool error, unsigned int time,
         LaserModel model)
@@ -92,6 +94,7 @@
     }
 }
 
+
 ScanData::ScanData(const ScanData &rhs)
 {
     _length = rhs.length();
@@ -122,12 +125,14 @@
     _model = rhs.model();
 }
 
+
 ScanData::~ScanData()
 {
     if(_ranges != NULL)
         delete[] _ranges;
 }
 
+
 std::string ScanData::error_code_to_string(uint32_t error_code)
 {
     if (_model == MODEL_UTM30LX)
@@ -204,6 +209,7 @@
     }
 }
 
+
 ScanData& ScanData::operator=(const ScanData &rhs)
 {
     if (rhs.length() == 0)
@@ -274,6 +280,7 @@
     return *this;
 }
 
+
 uint32_t ScanData::operator[](unsigned int index)
 {
     if (index >= _length)
@@ -281,6 +288,7 @@
     return _ranges[index];
 }
 
+
 std::string ScanData::as_string()
 {
     std::stringstream ss;
@@ -313,6 +321,7 @@
     return ss.str();
 }
 
+
 void ScanData::clean_up()
 {
     if (_ranges != NULL)
@@ -326,6 +335,7 @@
     _time = 0;
 }
 
+
 void ScanData::allocate_data(unsigned int length, bool include_intensities)
 {
     // If no data yet, allocate new

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 10:42:44 UTC (rev 506)
@@ -35,6 +35,7 @@
 #include <math.h>
 #include <sstream>
 #include <iostream>
+#include <iomanip>
 
 #if defined(WIN32)
     #define __func__    __FUNCTION__
@@ -80,7 +81,6 @@
    2 byte command block
 */
 
-///////////////////////////////////////////////////////////////////////////////
 // SCIP protocol version 2 notes
 ///////////////////////////////////////////////////////////////////////////////
 
@@ -96,7 +96,7 @@
    and a line feed.
  - Status codes 00 and 99 are OK, anything else is an error.
  - Checksum is calculated by... well, see the code.
- - BIG NOTE: The URG-30LX has a probable bug in its response to the II code
+ - BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
    whereby it does not include anything after and including "<-" in the
    checksum calculation.
 
@@ -107,8 +107,8 @@
       3 byte command block
 PP    Specification info
       P|P|LF
-      P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|
-        MinRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
+      P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MinRange...|;|Sum|LF|
+        MaxRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|
         LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF
       3 byte command block
 II    Status info
@@ -131,41 +131,82 @@
       9 byte command block
 MDMS  Get new data
       M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
-      M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|
-        LF|Data...|LF|LF
+      M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+        Status(2)|Sum|LF|Data...|LF|LF
       16 byte command block
 ME    Get new data, including intensity data
       M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
-      M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|
-        Data...|LF|LF
+      M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+        Status(2)|Sum|LF|Data...|LF|LF
       16 byte command block
-      Featured only on UTM-30LX for getting intensity data.
 GDGS  Get latest data
       G|D/S|Start(4)|End(4)|Cluster(2)|LF
       G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
       12 byte command block
+GE    Get latest data, including intensity data
+      G|E|Start(4)|End(4)|Cluster(2)|LF
+      G|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+      12 byte command block
 CR    Set motor speed
       C|R|Speed(2)|LF
       C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
       5 byte command block
 TM    Get sensor time
       T|M|Code|LF
-      T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
+      T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF
       4 byte command block
       Optional part only comes back for control code 1.
 RS    Reset
       R|S|LF
       R|S|LF|Status(2)|Sum|LF|LF
       3 byte command block
-GE
-ND
-NE
-HD
-HE
-HS
-$IP
-RT
-DB
+RT    Same as reset, but does not stop the motor or alter serial speed
+      R|T|LF
+      R|T|LF|Status(2)|Sum|LF|LF
+ND    Multi-echo version of MD
+      N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
+      N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+        Status(2)|Sum|LF|Data...|LF|LF
+      16 byte command block
+      Returned scan data may include echo data separated by '&'.
+NE    Multi-echo version of ME
+      N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
+      N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF|
+        Status(2)|Sum|LF|Data...|LF|LF
+      16 byte command block
+      Returned scan data may include echo data separated by '&'.
+HD    Multi-echo version of GD
+      H|D|Start(4)|End(4)|Cluster(2)|LF
+      H|D|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+      12 byte command block
+      Returned scan data may include echo data separated by '&'.
+HE    Multi-echo version of GE
+      H|E|Start(4)|End(4)|Cluster(2)|LF
+      H|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+      12 byte command block
+      Returned scan data may include echo data separated by '&'.
+HS    Set high sensitivity mode
+      H|S|On/Off(1)|LF
+      H|S|On/Off(1)|LF|Status(2)|Sum|LF|LF
+      3 byte command block
+      Use '0' for on, '1' for off.
+$IP   Change IP address settings
+      $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF
+      $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF|Status(2)|
+        Sum|LF|LF
+      Only available on models supporting ethernet connections.
+      Does not conform to standard command format.
+      Laser will require a restart after use (wait for response first).
+DB    Simulate sensor failure modes
+      D|B|Command(2)|LF
+      D|B|Command(2)|LF|Status(2)|Sum|LF|LF
+      Where command is one of:
+      "02" Enter failure state.
+      "03" While sending data, go through normal -> malfunction -> normal.
+      "04" While sending data, go through normal -> malfunction -> failure.
+      "05" While sending data, go through normal -> failure.
+      "10" Return to normal operation.
+      4 byte command block
 */
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -182,6 +223,7 @@
     return (byte1 << 6) + (byte2);
 }
 
+
 unsigned int decode_3_byte_value(char *data)
 {
     unsigned int byte1, byte2, byte3;
@@ -193,6 +235,7 @@
     return (byte1 << 12) + (byte2 << 6) + (byte3);
 }
 
+
 unsigned int decode_4_byte_value(char *data)
 {
     unsigned int byte1, byte2, byte3, byte4;
@@ -205,7 +248,8 @@
     return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
 }
 
-void number_to_string(unsigned int num, char *dest, int length)
+
+void number_to_string(unsigned int num, char* dest, int length)
 {
 #if defined(WIN32)
     _snprintf(dest, length + 1, "%*d", length, num);
@@ -217,6 +261,7 @@
         dest[ii] = '0';
 }
 
+
 ///////////////////////////////////////////////////////////////////////////////
 // Sensor class
 ///////////////////////////////////////////////////////////////////////////////
@@ -225,27 +270,31 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 Sensor::Sensor()
-    : _port(NULL), _err_output(std::cerr), _scip_version(2),
-    _verbose(false), _enable_checksum_workaround(false),
-    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
+    : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
+    _enable_checksum_workaround(false), _ignore_unknowns(false),
+    _multiecho_mode(ME_OFF), _min_angle(0.0), _max_angle(0.0),
     _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
 {
 }
 
+
 Sensor::Sensor(std::ostream& err_output)
     : _port(NULL), _err_output(err_output), _scip_version(2),
     _verbose(false), _enable_checksum_workaround(false),
-    _ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
-    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
+    _ignore_unknowns(false), _multiecho_mode(ME_OFF), _min_angle(0.0),
+    _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
+    _front_step(0)
 {
 }
 
+
 Sensor::~Sensor()
 {
     if (_port != NULL)
         delete _port;
 }
 
+
 void Sensor::open(std::string port_options)
 {
     if (_verbose)
@@ -272,6 +321,7 @@
     _get_defaults();
 }
 
+
 unsigned int Sensor::open_with_probing(std::string port_options)
 {
     if (_verbose)
@@ -362,6 +412,7 @@
         return 0;
 }
 
+
 void Sensor::close()
 {
     if (!_port)
@@ -372,6 +423,7 @@
     _port = NULL;
 }
 
+
 bool Sensor::is_open() const
 {
     if (_port != NULL)
@@ -379,6 +431,7 @@
     return false;
 }
 
+
 void Sensor::set_power(bool on)
 {
     if (_scip_version == 1)
@@ -421,6 +474,7 @@
         throw UnknownScipVersionError();
 }
 
+
 // This function assumes that both the port and the laser scanner are already
 // set to the same baud.
 void Sensor::set_baud(unsigned int baud)
@@ -460,6 +514,37 @@
         throw UnknownScipVersionError();
 }
 
+
+void Sensor::set_ip(IPAddr const& addr, IPAddr const& subnet,
+        IPAddr const& gateway)
+{
+    // Command is "$IP"(3) + IP(15) + ' ' + subnet(15) + ' ' + gateway(15),
+    // +1 for the null byte, but split two bytes off the front for sending.
+    // Hijack the _send_command function, treating the rest of the command as
+    // parameters.
+    char const command[3] = "$I";
+    std::stringstream params;
+    params << 'P';
+    params << std::setw(3) << std::setfill('0');
+    params << addr.first << '.' << addr.second << '.' << addr.third << '.' <<
+        addr.fourth << ' ';
+    params << subnet.first << '.' << subnet.second << '.' << subnet.third <<
+        '.' << subnet.fourth << ' ';
+    params << gateway.first << '.' << gateway.second << '.' << gateway.third <<
+        '.' << gateway.fourth;
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ <<
+            "() Setting IP information to $I" << params.str() << '\n';
+    }
+    int status = _send_command(&command[0], params.str().c_str(), 48, NULL);
+    // Skip the extra line feed
+    _skip_lines(1);
+    if (status != 0)
+        throw SetIPError();
+}
+
+
 void Sensor::reset()
 {
     if (_scip_version == 1)
@@ -467,8 +552,10 @@
     else if (_scip_version == 2)
     {
         if (_verbose)
+        {
             _err_output << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
+        }
         _send_command("RS", NULL, 0, NULL);
         _skip_lines(1);
     }
@@ -476,6 +563,24 @@
         throw UnknownScipVersionError();
 }
 
+
+void Sensor::semi_reset()
+{
+    if (_scip_version == 1)
+        throw UnsupportedError(35);
+    else if (_scip_version == 2)
+    {
+        if (_verbose)
+            _err_output << "Sensor::" << __func__ <<
+                "() Resetting laser.\n";
+        _send_command("RS", NULL, 0, NULL);
+        _skip_lines(1);
+    }
+    else
+        throw UnknownScipVersionError();
+}
+
+
 void Sensor::set_motor_speed(unsigned int speed)
 {
     if (_scip_version == 1)
@@ -524,6 +629,7 @@
         throw UnknownScipVersionError();
 }
 
+
 void Sensor::set_high_sensitivity(bool on)
 {
     if (_scip_version == 1)
@@ -552,6 +658,7 @@
         throw UnknownScipVersionError();
 }
 
+
 void Sensor::get_sensor_info(SensorInfo *info)
 {
     if (info == NULL)
@@ -713,6 +820,7 @@
         throw UnknownScipVersionError();
 }
 
+
 unsigned int Sensor::get_time()
 {
     if (_scip_version == 1)
@@ -737,6 +845,7 @@
     return 0;
 }
 
+
 unsigned int Sensor::get_ranges(ScanData *data, int start_step,
         int end_step, unsigned int cluster_count)
 {
@@ -778,7 +887,10 @@
         number_to_string(start_step, buffer, 4);
         number_to_string(end_step, &buffer[4], 4);
         number_to_string(cluster_count, &buffer[8], 2);
-        _send_command("GD", buffer, 10, NULL);
+        if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+            _send_command("HD", buffer, 10, NULL);
+        else
+            _send_command("GD", buffer, 10, NULL);
         // There will be a timestamp before the data (if there is data)
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
@@ -795,6 +907,7 @@
     return data->_length;
 }
 
+
 unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
         double end_angle, unsigned int cluster_count)
 {
@@ -823,6 +936,87 @@
     return get_ranges(data, start_step, end_step, cluster_count);
 }
 
+
+unsigned int Sensor::get_ranges_intensities(ScanData *data, int start_step,
+        int end_step, unsigned int cluster_count)
+{
+    if (data == NULL)
+        throw NoDestinationError();
+
+    char buffer[11];
+    memset(buffer, 0, sizeof(char) * 11);
+
+    if (start_step < 0)
+        start_step = _first_step;
+    if (end_step < 0)
+        end_step = _last_step;
+
+    unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " ranges between " << start_step << " and " <<
+            end_step << " with a cluster count of " << cluster_count <<
+            '\n';
+    }
+
+    if (_scip_version == 1)
+        throw UnsupportedError(36);
+    else if (_scip_version != 2)
+        throw UnknownScipVersionError();
+
+    // Send the command to ask for the most recent data from
+    // start_step to end_step
+    number_to_string(start_step, buffer, 4);
+    number_to_string(end_step, &buffer[4], 4);
+    number_to_string(cluster_count, &buffer[8], 2);
+    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+        _send_command("HE", buffer, 10, NULL);
+    else
+        _send_command("GE", buffer, 10, NULL);
+    // There will be a timestamp before the data (if there is data)
+    // Normally we would send 6 for the expected length, but we may get no
+    // timestamp back if there was no data.
+    if (_read_line_with_check(buffer) == 0)
+        throw NoDataError();
+    data->_time = decode_4_byte_value(buffer);
+    // In SCIP2 mode we're going to get back 3-byte data because we're
+    // sending the GE command
+    _read_3_byte_range_data(data, num_steps);
+
+    return data->_length;
+}
+
+
+unsigned int Sensor::get_ranges_intensities_by_angle(ScanData *data,
+        double start_angle, double end_angle, unsigned int cluster_count)
+{
+    if (data == NULL)
+        throw NoDataError();
+
+    // Calculate the given angles in steps, rounding towards _front_step
+    int start_step, end_step;
+    start_step = angle_to_step(start_angle);
+    end_step = angle_to_step(end_angle);
+
+    // Check the steps are within the allowable range
+    if (start_step < _first_step || start_step > _last_step)
+        throw StartStepError();
+    if (end_step < _first_step || end_step > _last_step)
+        throw EndStepError();
+
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ << "() Start angle " <<
+            start_angle << " is step " << start_step << ", end angle " <<
+            end_angle << " is step " << end_step << '\n';
+    }
+
+    // Get the data
+    return get_ranges_intensities(data, start_step, end_step, cluster_count);
+}
+
+
 unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
         int end_step, unsigned int cluster_count)
 {
@@ -831,79 +1025,85 @@
 
     if (_scip_version == 1)
         throw UnsupportedError(16);
-    else if (_scip_version == 2)
-    {
-        char buffer[14];
-        memset(buffer, 0, sizeof(char) * 14);
+    else if (_scip_version != 2)
+        throw UnknownScipVersionError();
 
-        if (start_step < 0)
-            start_step = _first_step;
-        if (end_step < 0)
-            end_step = _last_step;
+    char buffer[14];
+    memset(buffer, 0, sizeof(char) * 14);
 
-        unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ << "() Reading " <<
-                num_steps << " new ranges between " << start_step << " and " <<
-                end_step << " with a cluster count of " << cluster_count <<
-                '\n';
-        }
+    if (start_step < 0)
+        start_step = _first_step;
+    if (end_step < 0)
+        end_step = _last_step;
 
-        // Send the command to ask for the most recent range data from start_step to end_step
-        number_to_string(start_step, buffer, 4);
-        number_to_string(end_step, &buffer[4], 4);
-        number_to_string(cluster_count, &buffer[8], 2);
-        number_to_string(1, &buffer[10], 1);
-        number_to_string(1, &buffer[11], 2);
-        _send_command("MD", buffer, 13, NULL);
-        // Mx commands will perform a scan, then send the data prefixed with
-        // another command echo.
-        // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        char response[17];
-        _skip_lines(1); // End of the command echo message
-        _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
-        // Check the echo is correct
-        if (response[0] != 'M' || response[1] != 'D')
-            throw CommandEchoError("MD", response);
-        // Then compare the parameters
-        buffer[12] = '0'; // There will be zero scans remaining after this one
-        if (memcmp(&response[2], buffer, 13) != 0)
-            throw ParamEchoError("MD");
-        // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() MD data prefix status: " << response[0] << response[1] <<
-                '\n';
-        }
-        // Check the status code is OK - should only get 99 here
-        if (response[0] != '9' || response[1] != '9')
-        {
-            // There is an extra line feed after an error status (signalling
-            // end of message)
-            _skip_lines(1);
-            throw ResponseError(response, "MD");
-        }
+    unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " new ranges between " << start_step << " and " <<
+            end_step << " with a cluster count of " << cluster_count <<
+            '\n';
+    }
 
-        // Now the actual data will arrive
-        // There will be a timestamp before the data (if there is data)
-        // Normally we would send 6 for the expected length, but we may get no
-        // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
-            throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
-        // In SCIP2 mode we're going to get back 3-byte data because we're
-        // sending the MD command
-        _read_3_byte_range_data(data, num_steps);
+    // Send the command to ask for the most recent range data from start_step to end_step
+    number_to_string(start_step, buffer, 4);
+    number_to_string(end_step, &buffer[4], 4);
+    number_to_string(cluster_count, &buffer[8], 2);
+    number_to_string(1, &buffer[10], 1);
+    number_to_string(1, &buffer[11], 2);
+    char command[3];
+    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+        command[0] = 'N';
+    else
+        command[0] = 'M';
+    command[1] = 'D';
+    command[2] = '\0';
+    _send_command(command, buffer, 13, NULL);
+    // Mx commands will perform a scan, then send the data prefixed with
+    // another command echo.
+    // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
+    char response[17];
+    _skip_lines(1); // End of the command echo message
+    _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
+    // Check the echo is correct
+    if (response[0] != command[0] || response[1] != command[1])
+        throw CommandEchoError(command, response);
+    // Then compare the parameters
+    buffer[12] = '0'; // There will be zero scans remaining after this one
+    if (memcmp(&response[2], buffer, 13) != 0)
+        throw ParamEchoError(command);
+    // The next line should be the status line
+    _read_line_with_check(response, 4);
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ <<
+            "() " << command << " data prefix status: " << response[0] <<
+            response[1] << '\n';
     }
-    else
-        throw UnknownScipVersionError();
+    // Check the status code is OK - should only get 99 here
+    if (response[0] != '9' || response[1] != '9')
+    {
+        // There is an extra line feed after an error status (signalling
+        // end of message)
+        _skip_lines(1);
+        throw ResponseError(response, command);
+    }
 
+    // Now the actual data will arrive
+    // There will be a timestamp before the data (if there is data)
+    // Normally we would send 6 for the expected length, but we may get no
+    // timestamp back if there was no data.
+    if (_read_line_with_check(buffer) == 0)
+        throw NoDataError();
+    data->_time = decode_4_byte_value(buffer);
+    // In SCIP2 mode we're going to get back 3-byte data because we're
+    // sending the MD command
+    _read_3_byte_range_data(data, num_steps);
+
     return data->_length;
 }
 
+
 unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
@@ -934,6 +1134,7 @@
     return get_new_ranges(data, start_step, end_step, cluster_count);
 }
 
+
 unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
         int start_step, int end_step, unsigned int cluster_count)
 {
@@ -942,79 +1143,85 @@
 
     if (_scip_version == 1)
         throw UnsupportedError(17);
-    else if (_scip_version == 2)
-    {
-        char buffer[14];
-        memset(buffer, 0, sizeof(char) * 14);
+    else if (_scip_version != 2)
+        throw UnknownScipVersionError();
 
-        if (start_step < 0)
-            start_step = _first_step;
-        if (end_step < 0)
-            end_step = _last_step;
+    char buffer[14];
+    memset(buffer, 0, sizeof(char) * 14);
 
-        unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ << "() Reading " <<
-                num_steps << " new ranges between " << start_step << " and " <<
-                end_step << " with a cluster count of " << cluster_count <<
-                '\n';
-        }
+    if (start_step < 0)
+        start_step = _first_step;
+    if (end_step < 0)
+        end_step = _last_step;
 
-        // Send the command to ask for the most recent range data with
-        // intensity data from start_step to end_step
-        number_to_string(start_step, buffer, 4);
-        number_to_string(end_step, &buffer[4], 4);
-        number_to_string(cluster_count, &buffer[8], 2);
-        number_to_string(1, &buffer[10], 1);
-        number_to_string(1, &buffer[11], 2);
-        _send_command("ME", buffer, 13, NULL);
-        // Mx commands will perform a scan, then send the data prefixed with
-        // another command echo.
-        // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        char response[17];
-        _skip_lines(1); // End of the command echo message
-        _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
-        // Check the echo is correct
-        if (response[0] != 'M' || response[1] != 'E')
-            throw CommandEchoError("ME", response);
-        // Then compare the parameters
-        if (memcmp(&response[2], buffer, 13) != 0)
-            throw ParamEchoError("ME");
-        // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if (_verbose)
-        {
-            _err_output << "Sensor::" << __func__ <<
-                "() ME data prefix status: " << response[0] << response[1] <<
-                '\n';
-        }
-        // Check the status code is OK - should only get 99 here
-        if (response[0] != '9' || response[1] != '9')
-        {
-            // There is an extra line feed after an error status (signalling
-            // end of message)
-            _skip_lines(1);
-            throw ResponseError(response, "ME");
-        }
+    unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ << "() Reading " <<
+            num_steps << " new ranges and intensities between " <<
+            start_step << " and " << end_step <<
+            " with a cluster count of " << cluster_count << '\n';
+    }
 
-        // Now the actual data will arrive
-        // There will be a timestamp before the data (if there is data)
-        // Normally we would send 6 for the expected length, but we may get no
-        // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
-            throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
-        // In SCIP2 mode we're going to get back 3-byte data because we're
-        // sending the ME command
-        _read_3_byte_range_and_intensity_data(data, num_steps);
+    // Send the command to ask for the most recent range data with
+    // intensity data from start_step to end_step
+    number_to_string(start_step, buffer, 4);
+    number_to_string(end_step, &buffer[4], 4);
+    number_to_string(cluster_count, &buffer[8], 2);
+    number_to_string(1, &buffer[10], 1);
+    number_to_string(1, &buffer[11], 2);
+    char command[3];
+    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+        command[0] = 'N';
+    else
+        command[0] = 'M';
+    command[1] = 'E';
+    command[2] = '\0';
+    _send_command(command, buffer, 13, NULL);
+    // Mx commands will perform a scan, then send the data prefixed with
+    // another command echo.
+    // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
+    char response[17];
+    _skip_lines(1); // End of the command echo message
+    _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
+    // Check the echo is correct
+    if (response[0] != command[0] || response[1] != command[1])
+        throw CommandEchoError(command, response);
+    // Then compare the parameters
+    if (memcmp(&response[2], buffer, 13) != 0)
+        throw ParamEchoError(command);
+    // The next line should be the status line
+    _read_line_with_check(response, 4);
+    if (_verbose)
+    {
+        _err_output << "Sensor::" << __func__ <<
+            "() " << command << " data prefix status: " << response[0] <<
+            response[1] << '\n';
     }
-    else
-        throw UnknownScipVersionError();
+    // Check the status code is OK - should only get 99 here
+    if (response[0] != '9' || response[1] != '9')
+    {
+        // There is an extra line feed after an error status (signalling
+        // end of message)
+        _skip_lines(1);
+        throw ResponseError(response, command);
+    }
 
+    // Now the actual data will arrive
+    // There will be a timestamp before the data (if there is data)
+    // Normally we would send 6 for the expected length, but we may get no
+    // timestamp back if there was no data.
+    if (_read_line_with_check(buffer) == 0)
+        throw NoDataError();
+    data->_time = decode_4_byte_value(buffer);
+    // In SCIP2 mode we're going to get back 3-byte data because we're
+    // sending the ME command
+    _read_3_byte_range_and_intensity_data(data, num_steps);
+
     return data->_length;
 }
 
+
 unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
@@ -1046,12 +1253,14 @@
             cluster_count);
 }
 
+
 double Sensor::step_to_angle(unsigned int step)
 {
     return (static_cast<int>(step) - static_cast<int>(_front_step)) *
         _resolution;
 }
 
+
 unsigned int Sensor::angle_to_step(double angle)
 {
     unsigned int result;
@@ -1068,6 +1277,7 @@
     return result;
 }
 
+
 // Private functions
 ///////////////////////////////////////////////////////////////////////////////
 
@@ -1084,6 +1294,7 @@
         _port->Flush();
 }
 
+
 // If expected_length is not -1, it should include the terminating line feed but
 // not the NULL (although the buffer still has to include this).
 // If expected_length is -1, this function expects buffer to be a certain length
@@ -1141,6 +1352,7 @@
     return linelength - 1; // Line feed not included
 }
 
+
 // This function will read a line and then calculate its checksum, comparing it
 // with the checksum at the end of the line. The checksum will be removed
 // (along with the semi-colon, if present).  buffer and expected_length args are
@@ -1232,6 +1444,7 @@
     return bytesToConsider;
 }
 
+
 // Reads lines until the number specified has passed.
 void Sensor::_skip_lines(int count)
 {
@@ -1242,6 +1455,7 @@
         throw ReadError(18);
 }
 
+
 // Sends a command with optional parameters and checks that the echo of the
 // command and parameters sent are correct, and that the returned status code
 // is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two
@@ -1413,6 +1627,10 @@
 {
     if (strstr(buffer, "URG-04LX") != NULL)
         _model = MODEL_URG04LX;
+    else if (strstr(buffer, "UBG-04LX") != NULL)
+        _model = MODEL_UBG04LXF01;
+    else if (strstr(buffer, "UHG-08LX") != NULL)
+        _model = MODEL_UHG08LX;
     else if (strstr(buffer, "UTM-30LX") != NULL)
     {
         _model = MODEL_UTM30LX;
@@ -1420,6 +1638,8 @@
         // model.
         _enable_checksum_workaround = true;
     }
+    else if (strstr(buffer, "UXM-30LX") != NULL)
+        _model = MODEL_UXM30LXE;
     else
         _model = MODEL_UNKNOWN;
 }
@@ -1548,6 +1768,7 @@
     throw ScipVersionError();
 }
 
+
 void Sensor::_get_defaults()
 {
     if (_verbose)
@@ -1574,6 +1795,7 @@
     }
 }
 
+
 void Sensor::_process_vv_line(const char *buffer, SensorInfo *info)
 {
     if (strncmp(buffer, "VEND", 4) == 0)
@@ -1583,6 +1805,7 @@
         info->product = &buffer[5]; // Product info
         // Find the product model
         _find_model(&buffer[5]);
+        info->detected_model = _model;
     }
     else if (strncmp(buffer, "FIRM", 4) == 0)
         info->firmware = &buffer[5]; // Firmware version
@@ -1594,6 +1817,7 @@
         throw UnknownLineError(buffer);
 }
 
+
 void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
 {
     if (strncmp(buffer, "MODL", 4) == 0)
@@ -1613,10 +1837,20 @@
         info->front_step = atoi(&buffer[5]);
     else if (strncmp(buffer, "SCAN", 4) == 0)
         info->standard_speed = atoi(&buffer[5]);
+    /* No example in the manual and sensor with support for this has not
+     * arrived yet, so don't know what to look for.
+    else if (strncmp(buffer, "", 4) == 0)
+    {
+        if (strstr(buffer, "CCW") != NULL)
+            info->rot_dir = COUNTERCLOCKWISE;
+        else
+            info->rot_dir = CLOCKWISE;
+    }*/
     else if (!_ignore_unknowns)
         throw UnknownLineError(buffer);
 }
 
+
 void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
 {
     if (strncmp(buffer, "MODL", 4) == 0)
@@ -1675,11 +1909,14 @@
         throw UnknownLineError(buffer);
 }
 
+
 void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
 {
     if (_verbose)
+    {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
+    }
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
@@ -1720,11 +1957,20 @@
         throw DataCountError();
 }
 
+
 void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
 {
     if (_verbose)
+    {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
+        if (_multiecho_mode != ME_OFF)
+        {
+            _err_output << "Sensor::" << __func__ <<
+                "() Multi-echo mode is set to " <<
+                multiecho_mode_to_string(_multiecho_mode) << '\n';
+        }
+    }
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
@@ -1819,6 +2065,7 @@
         throw DataCountError();
 }
 
+
 void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
         unsigned int num_steps)
 {
@@ -1826,6 +2073,12 @@
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges and intensities.\n";
+        if (_multiecho_mode != ME_OFF)
+        {
+            _err_output << "Sensor::" << __func__ <<
+                "() Multi-echo mode is set to " <<
+                multiecho_mode_to_string(_multiecho_mode) << '\n';
+        }
     }
 
     // This will automatically take care of whether it actually needs to
@@ -1943,6 +2196,7 @@
         throw DataCountError();
 }
 
+
 int Sensor::_confirm_checksum(const char *buffer, int length,
         int expected_sum)
 {

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-04 10:42:44 UTC (rev 506)
@@ -45,6 +45,60 @@
 namespace hokuyo_aist
 {
 
+/// @brief Possible values of the multiecho mode setting.
+///
+/// The Tough-URG features multiecho detection capability. To use this, set
+/// the sensor to use any mode other than ME_OFF.
+/// The sensor can register up to three echos for a single reading. The
+/// multiecho mode determines how these are combined into a single value. If
+/// ME_FRONT is set, only the closest reading will be used. If ME_MIDDLE is
+/// set, only the middle reading will be used, or the closest reading if there
+/// are only two echos. If ME_REAR is set, the furthest reading will be used.
+/// Setting ME_AVERAGE will use the average of all two or three echos.
+enum MultiechoMode
+{
+    ME_OFF,
+    ME_FRONT,
+    ME_MIDDLE,
+    ME_REAR,
+    ME_AVERAGE
+};
+
+
+HOKUYO_AIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode)
+{
+    switch(mode)
+    {
+        case ME_OFF:
+            return "Off";
+        case ME_FRONT:
+            return "Front";
+        case ME_MIDDLE:
+            return "Middle";
+        case ME_REAR:
+            return "Rear";
+        case ME_AVERAGE:
+            return "Average";
+        default:
+            return "Unknown";
+    }
+}
+
+
+/// Structure to store an IP address.
+typedef struct IPAddr
+{
+    /// First byte
+    unsigned int first;
+    /// Second byte
+    unsigned int second;
+    /// Third byte
+    unsigned int third;
+    /// Fourth byte
+    unsigned int fourth;
+} IPAddr;
+
+
 /** @brief Hokuyo laser scanner class.
 
 Provides an interface for interacting with a Hokuyo laser scanner using SCIP
@@ -95,11 +149,25 @@
         500.0Kbps, 750.0Kbps (dependent on those available in FlexiPort). */
         void set_baud(unsigned int baud);
 
+        /** @brief Change the IP address information.
+
+        This function only works on devices providing an ethernet connection.
+
+        Once this function has successfully completed, the laser must be
+        restarted for it to take effect. */
+        void set_ip(IPAddr const& addr, IPAddr const& subnet,
+                IPAddr const& gateway);
+
         /** @brief Reset the laser scanner to its default settings.
 
         Not available with the SCIP v1 protocol. */
         void reset();
 
+        /** @brief Reset everything except motor and serial speed.
+
+        Requires SCIP v2.1 or higher. */
+        void semi_reset();
+
         /** @brief Set the speed at which the scanner's sensor spins.
 
         Set the speed to 0 to have it reset to the default value, and 99 to
@@ -168,6 +236,50 @@
         unsigned int get_ranges_by_angle(ScanData *data, double start_angle,
                 double end_angle, unsigned int cluster_count = 1);
 
+        /** @brief Get the latest scan data from the scanner with intensities.
+
+        This function requires a pointer to a @ref ScanData object. It will
+        allocate space in this object as necessary for storing range data. If
+        the passed-in @ref ScanData object already has the correct quantity
+        of space to store the range data, it will not be re-allocated. If it
+        does not have any space, it will be allocated. If it has space, but it
+        is the wrong size, it will be re-allocated. This means you can
+        repeatedly send the same @ref ScanData object without having to worry
+        about allocating its data, whether it will change or not, while also
+        avoiding excessive allocations.
+
+        @param data Pointer to a @ref ScanData object to store the range
+        readings in.
+        @param cluster_count The number of readings to cluster together into a
+        single reading. The minimum value from a cluster is returned as the
+        range for that cluster.
+        @param start_step The first step to get ranges from. Set to -1 for the
+        first scannable step.
+        @param end_step The last step to get ranges from. Set to -1 for the last
+        scannable step.
+        @return The number of range readings read into @ref data. */
+        unsigned int get_ranges_intensities(ScanData *data,
+                int start_step = -1, int end_step = -1,
+                unsigned int cluster_count = 1);
+
+        /** @brief Get the latest scan data from the scanner with intensities.
+
+        @param data Pointer to a @ref ScanData object to store the range
+        readings in.
+        @param start_angle The angle to get range readings from. Exclusive; if
+        this falls between two steps the step inside the angle will be
+        returned, but the step outside won't.
+        @param end_angle The angle to get range readings to. Exclusive; if this
+        falls between two steps the step inside the angle will be returned, but
+        the step outside won't.
+        @param cluster_count The number of readings to cluster together into a
+        single reading. The minimum value from a cluster is returned as the
+        range for that cluster.
+        @return The number of range readings read into @ref data. */
+        unsigned int get_ranges_intensities_by_angle(ScanData *data,
+                double start_angle, double end_angle,
+                unsigned int cluster_count = 1);
+
         /** @brief Get a new scan from the scanner.
 
         Unlike @ref get_ranges, which returns the most recent scan the scanner
@@ -281,6 +393,9 @@
         messages. Default is off. */
         void ignore_unknowns(bool ignore)       { _ignore_unknowns = ignore; }
 
+        /** @brief Set the multi-echo mode to use. Default is ME_OFF. */
+        void set_multiecho_mode(MultiechoMode mode) { _multiecho_mode = mode; }
+
         /// @brief A convenience function to convert a step index to an angle.
         double step_to_angle(unsigned int step);
         /** @brief A convenience function to convert an angle to a step
@@ -295,6 +410,7 @@
         LaserModel _model;
         bool _verbose, _enable_checksum_workaround,
              _ignore_unknowns;
+        MultiechoMode _multiecho_mode;
         double _min_angle, _max_angle, _resolution;
         int _first_step, _last_step, _front_step;
         unsigned int _max_range;

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-04 10:42:44 UTC (rev 506)
@@ -30,23 +30,79 @@
 
 SensorInfo::SensorInfo()
     : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
-    front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
-    baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
-    scanable_steps(0), detected_model(MODEL_UNKNOWN)
+    front_step(0), standard_speed(0), rot_dir(COUNTERCLOCKWISE), power(false),
+    speed(0), speed_level(0), baud(0), time(0), min_angle(0.0), max_angle(0.0),
+    resolution(0.0), scanable_steps(0), max_step(0),
+    detected_model(MODEL_UNKNOWN)
 {
 }
 
+
 // Set various known values based on what the manual says
 void SensorInfo::set_defaults()
 {
-    min_range = 20;
-    max_range = 4095;
-    steps = 1024;
-    first_step = 44;
-    last_step = 725;
-    front_step = 384;
+    switch (detected_model)
+    {
+        case MODEL_URG04LX:
+            min_range = 20;
+            max_range = 4095;
+            steps = 1024;
+            first_step = 44;
+            last_step = 725;
+            front_step = 384;
+            max_step = 768;
+            break;
+        case MODEL_UBG04LXF01:
+            min_range = 20;
+            max_range = 4095;
+            steps = 1024;
+            first_step = 44;
+            last_step = 725;
+            front_step = 384;
+            max_step = 768;
+            break;
+        case MODEL_UHG08LX:
+            min_range = 20;
+            max_range = 4095;
+            steps = 1024;
+            first_step = 0;
+            last_step = 768;
+            front_step = 384;
+            max_step = 768;
+            break;
+        case MODEL_UTM30LX:
+            min_range = 20;
+            max_range = 4095;
+            steps = 1440;
+            first_step = 0;
+            last_step = 1080;
+            front_step = 540;
+            max_step = 1080;
+            break;
+        case MODEL_UXM30LXE:
+            min_range = 20;
+            max_range = 4095;
+            steps = 1440;
+            first_step = 0;
+            last_step = 760;
+            front_step = 380;
+            max_step = 760;
+            break;
+        case MODEL_UNKNOWN:
+        default:
+            // Use the URG-04LX settings
+            min_range = 20;
+            max_range = 4095;
+            steps = 1024;
+            first_step = 44;
+            last_step = 725;
+            front_step = 384;
+            max_step = 768;
+            break;
+    }
 }
 
+
 void SensorInfo::calculate_values()
 {
     resolution = DTOR(360.0) / steps;
@@ -59,12 +115,14 @@
     scanable_steps = last_step - first_step + 1;
 }
 
+
 std::string SensorInfo::as_string()
 {
     std::stringstream ss;
 
     ss << "Vendor: " << vendor << '\n';
     ss << "Product: " << product << '\n';
+    ss << "Identified as: " << model_to_string(detected_model) << '\n';
     ss << "Firmware: " << firmware << '\n';
     ss << "Protocol: " << protocol << '\n';
     ss << "Serial: " << serial << '\n';
@@ -75,11 +133,12 @@
     ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " <<
         scanable_steps << '\n';
     ss << "First step: " << first_step << "\tFront step: " << front_step <<
-        "\tLast step: " << last_step << '\n';
+        "\tLast step: " << last_step << "\tMax step: " << max_step << '\n';
     ss << "Resolution: " << resolution << " radians/step\n";
     ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " <<
         max_angle << " radians\n";
     ss << "Standard motor speed: " << standard_speed << "rpm\n";
+    ss << "Rotation direction: " << rot_dir_to_string(rot_dir) << '\n';
 
     ss << "Power status: " << (power ? "On" : "Off") <<
         "\tMeasurement state: " << measure_state << '\n';
@@ -91,4 +150,3 @@
     return ss.str();
 }
 
-

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-04 05:30:21 UTC (rev 505)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-04 10:42:44 UTC (rev 506)
@@ -36,6 +36,7 @@
 #endif
 
 #include <string>
+#include <cstring>
 
 /** @ingroup gbx_library_hokuyo_aist
 @{
@@ -45,27 +46,80 @@
 {
 
 /// Laser models
-enum LaserModel {
-    MODEL_URG04LX,
-    MODEL_UTM30LX,
+enum LaserModel
+{
+    MODEL_URG04LX, // Classic-URG
+    MODEL_UBG04LXF01, // Rapid-URG
+    MODEL_UHG08LX, // Hi-URG
+    MODEL_UTM30LX, // Top-URG
+    MODEL_UXM30LXE, // Tough-URG
     MODEL_UNKNOWN
 };
 
+
 HOKUYO_AIST_EXPORT inline char const* model_to_string(LaserModel model)
 {
     switch(model)
     {
         case MODEL_URG04LX:
             return "URG-04LX";
+        case MODEL_UBG04LXF01:
+            return "UBG-04LX-F01";
+        case MODEL_UHG08LX:
+            return "UHG-08LX";
         case MODEL_UTM30LX:
             return "UTM-30LX";
+        case MODEL_UXM30LXE:
+            return "UXM-30LX-E";
         default:
             return "Unknown model";
     }
 }
 
+
+HOKUYO_AIST_EXPORT inline LaserModel string_to_model(char const* model)
+{
+    if (strncmp(model, "URG-04LX", 8) == 0)
+        return MODEL_URG04LX;
+    else if (strncmp(model, "UBG-04LX-F01", 8) == 0)
+        return MODEL_UBG04LXF01;
+    else if (strncmp(model, "UHG-08LX", 8) == 0)
+        return MODEL_UHG08LX;
+    else if (strncmp(model, "UTM-30LX", 8) == 0)
+        return MODEL_UTM30LX;
+    else if (strncmp(model, "UXM-30LX-E", 8) == 0)
+        return MODEL_UXM30LXE;
+    else
+        return MODEL_UNKNOWN;
+}
+
+
+/// Sensor direction of rotation
+enum RotationDirection
+{
+    CLOCKWISE,
+    COUNTERCLOCKWISE
+};
+
+
+HOKUYO_AIST_EXPORT inline char const* rot_dir_to_string(RotationDirection dir)
+{
+    switch(dir)
+    {
+        case CLOCKWISE:
+            return "Clockwise";
+        case COUNTERCLOCKWISE:
+            return "Counter-clockwise";
+        default:
+            return "Unknown";
+    }
+}
+
+
+// Forward declaration
 class Sensor;
 
+
 /** @brief Sensor information.
 
 Returned from a call to @get_sensor_info. Contains various information about the
@@ -114,6 +168,8 @@
         unsigned int front_step;
         /// Standard motor speed (rpm).
         unsigned int standard_speed;
+        /// Rotation direction.
+        RotationDirection rot_dir;
 
         // Status details.
         /// Operational status - illuminated or not.
@@ -142,6 +198,8 @@
         double resolution;
         /// Total number of steps in a full scan (lastStep - firstStep).
         unsigned int scanable_steps;
+        /// Absolute maximum commandable step.
+        unsigned int max_step;
         /// Detected model of the laser.
         LaserModel detected_model;
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 507
          http://gearbox.svn.sourceforge.net/gearbox/?rev=507&view=rev
Author:   gbiggs
Date:     2010-08-05 01:26:43 +0000 (Thu, 05 Aug 2010)

Log Message:
-----------
Support new multi-echo detection commands.

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h
    gearbox/trunk/src/hokuyo_aist/test/example.cpp
    gearbox/trunk/src/hokuyo_aist/test/example.readme
    gearbox/trunk/src/hokuyo_aist/utils.h

Added Paths:
-----------
    gearbox/trunk/src/hokuyo_aist/test/example_urg_04lx.logr
    gearbox/trunk/src/hokuyo_aist/test/example_urg_04lx.logw
    gearbox/trunk/src/hokuyo_aist/test/example_utm_30lx.logr
    gearbox/trunk/src/hokuyo_aist/test/example_utm_30lx.logw

Removed Paths:
-------------
    gearbox/trunk/src/hokuyo_aist/test/example.logr
    gearbox/trunk/src/hokuyo_aist/test/example.logw

Modified: gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/getid/getid.cpp	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,15 +1,15 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo URG laser scanner driver.
  *
- * This distribution is licensed to you under the terms described in the LICENSE file included in
- * this distribution.
+ * This distribution is licensed to you under the terms described in the
+ * LICENSE file included in this distribution.
  *
- * This work is a product of the National Institute of Advanced Industrial Science and Technology,
- * Japan. Registration number: H22PRO-1086.
+ * This work is a product of the National Institute of Advanced Industrial
+ * Science and Technology, Japan. Registration number: H22PRO-1086.
  *
  * This file is part of hokuyo_aist.
  *
@@ -17,71 +17,73 @@
  * http://www.opensource.org/licenses/eclipse-1.0.txt
  */
 
-#include <stdio.h>
-#include <math.h>
+#include <cstdio>
+#include <cmath>
 #include <iostream>
-using namespace std;
 
 #include <hokuyo_aist/hokuyo_aist.h>
 #include <hokuyo_aist/hokuyo_errors.h>
 
 int main(int argc, char **argv)
 {
-	string portOptions = "type=serial,device=/dev/ttyACM0,timeout=1";
-	bool  verbose = false;
+    std::string port_options="type=serial,device=/dev/ttyACM0,timeout=1";
+    bool verbose=false;
 
 #if defined (WIN32)
-	portOptions = "type=serial,device=COM3,timeout=1";
+    port_options = "type=serial,device=COM3,timeout=1";
 #else
-	int opt;
-	// Get some options from the command line
-	while ((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:vh")) != -1)
-	{
-		switch (opt)
-		{
-			case 'o':
-				portOptions = optarg;
-				break;
-			case 'v':
-				verbose = true;
-				break;
-			case '?':
-			case 'h':
-			default:
-				cout << "Usage: " << argv[0] << " [options]" << endl << endl;
-				cout << "-o options\tPort options (see flexiport library)." << endl;
-				cout << "-v\t\tPut the hokuyo_aist library into verbose mode." << endl;
-				return 1;
-		}
-	}
+    int opt;
+    // Get some options from the command line
+    while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:vh")) != -1)
+    {
+        switch(opt)
+        {
+            case 'o':
+                port_options = optarg;
+                break;
+            case 'v':
+                verbose = true;
+                break;
+            case '?':
+            case 'h':
+            default:
+                std::cout << "Usage: " << argv[0] << " [options]\n\n";
+                std::cout <<
+                    "-o options\tPort options (see flexiport library).\n";
+                std::cout <<
+                    "-v\t\tPut the hokuyo_aist library into verbose mode.\n";
+                return 1;
+        }
+    }
 #endif // defined (WIN32)
 
-	try
-	{
-		hokuyo_aist::Sensor laser; // Laser scanner object
-		// Set the laser to verbose mode (so we see more information in the console)
-		if (verbose)
-			laser.set_verbose (true);
+    try
+    {
+        hokuyo_aist::Sensor laser; // Laser scanner object
+        // Set the laser to verbose mode (so we see more information in the
+        // console)
+        if(verbose)
+            laser.set_verbose (true);
 
-		// Open the laser
-		laser.open (portOptions);
-		// Turn the laser on
-		laser.set_power (true);
+        // Open the laser
+        laser.open (port_options);
+        // Turn the laser on
+        laser.set_power (true);
 
-		// Get some laser info
-		//cout << "Laser sensor information:" << endl;
-		hokuyo_aist::SensorInfo info;
-		laser.get_sensor_info (&info);
-		cout << info.serial << endl;
+        // Get some laser info
+        hokuyo_aist::SensorInfo info;
+        laser.get_sensor_info (&info);
+        std::cout << info.serial << '\n';
 
-		// Close the laser
-		laser.close ();
-	}
-	catch (hokuyo_aist::BaseError &e)
-	{
-		cerr << "Caught exception: " << e.what () << endl;
-		return 1;
-	}
+        // Close the laser
+        laser.close ();
+    }
+    catch(hokuyo_aist::BaseError &e)
+    {
+        std::cerr << "Caught exception: " << e.what () << '\n';
+        return 1;
+    }
 
-	return 0;
+    return 0;
 }
+

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.cpp	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -32,36 +32,36 @@
     std::stringstream ss;
 
     // Check for non-errors
-    if (error[0] == '0' && error[1] == '0')
+    if(error[0] == '0' && error[1] == '0')
         return "Status OK - 00";
-    else if (error[0] == '9' && error[1] == '9')
+    else if(error[0] == '9' && error[1] == '9')
         return "Status OK - 99";
 
     // Check for universal errors
-    if (error[0] == '0' && error[1] == 'A')
+    if(error[0] == '0' && error[1] == 'A')
         return "Unable to create transmission data or reply command internally";
-    else if (error[0] == '0' && error[1] == 'B')
+    else if(error[0] == '0' && error[1] == 'B')
         return "Buffer shortage or command repeated that is already processed";
-    else if (error[0] == '0' && error[1] == 'C')
+    else if(error[0] == '0' && error[1] == 'C')
         return "Command with insufficient parameters 1";
-    else if (error[0] == '0' && error[1] == 'D')
+    else if(error[0] == '0' && error[1] == 'D')
         return "Undefined command 1";
-    else if (error[0] == '0' && error[1] == 'E')
+    else if(error[0] == '0' && error[1] == 'E')
         return "Undefined command 2";
-    else if (error[0] == '0' && error[1] == 'F')
+    else if(error[0] == '0' && error[1] == 'F')
         return "Command with insufficient parameters 2";
-    else if (error[0] == '0' && error[1] == 'G')
+    else if(error[0] == '0' && error[1] == 'G')
         return "String character in command exceeds 16 letters";
-    else if (error[0] == '0' && error[1] == 'H')
+    else if(error[0] == '0' && error[1] == 'H')
         return "String character has invalid letters";
-    else if (error[0] == '0' && error[1] == 'I')
+    else if(error[0] == '0' && error[1] == 'I')
         return "Sensor is now in firmware update mode";
 
     int error_code = atoi(error);
 
-    if (cmd[0] == 'B' && cmd[1] == 'M')
+    if(cmd[0] == 'B' && cmd[1] == 'M')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Unable to control due to laser malfunction";
@@ -70,9 +70,9 @@
         }
     }
 // No info in the manual for this.
-//    else if (cmd[0] == 'Q' && cmd[1] == 'T')
+//    else if(cmd[0] == 'Q' && cmd[1] == 'T')
 //    {
-//        switch (error_code)
+//        switch(error_code)
 //        {
 //            default:
 //                std::stringstream ss;
@@ -81,11 +81,11 @@
 //                return ss.str ();
 //        }
 //    }
-    else if (((cmd[0] == 'G' || cmd[0] == 'H') &&
+    else if(((cmd[0] == 'G' || cmd[0] == 'H') &&
                 (cmd[1] == 'D' || cmd[1] == 'E')) ||
              (cmd[0] == 'G' && cmd[1] == 'S'))
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Starting step has non-numeric value";
@@ -100,7 +100,7 @@
             case 10:
                 return "Laser is off";
             default:
-                if (error_code >= 50)
+                if(error_code >= 50)
                     ss << "Hardware error: " << error_code;
                 else
                     ss << "Unknown error code " << error_code <<
@@ -109,11 +109,11 @@
                 return ss.str();
         }
     }
-    else if (((cmd[0] == 'M' || cmd[0] == 'N') && 
+    else if(((cmd[0] == 'M' || cmd[0] == 'N') && 
                 (cmd[1] == 'D' || cmd[1] == 'E')) ||
              (cmd[0] == 'M' && cmd[1] == 'S'))
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Starting step has non-numeric value";
@@ -130,14 +130,14 @@
             case 7:
                 return "Number of scans is non-numeric";
             default:
-                if (error_code >= 21 && error_code <= 49)
+                if(error_code >= 21 && error_code <= 49)
                 {
                     return "Processing stopped to verify error. "
                         "This function is not yet supported by hokuyo_aist.";
                 }
-                else if (error_code >= 50 && error_code <= 97)
+                else if(error_code >= 50 && error_code <= 97)
                     ss << "Hardware error: " << error_code;
-                else if (error_code == 98)
+                else if(error_code == 98)
                 {
                     return "Resumption of processing after confirming normal "
                         "laser opteration. This function is not yet supported "
@@ -150,9 +150,9 @@
                 return ss.str();
         }
     }
-    else if (cmd[0] == 'T' && cmd[1] == 'M')
+    else if(cmd[0] == 'T' && cmd[1] == 'M')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Invalid control code";
@@ -166,9 +166,9 @@
                 return "Adjust mode is off when requested time";
         }
     }
-    else if (cmd[0] == 'S' && cmd[1] == 'S')
+    else if(cmd[0] == 'S' && cmd[1] == 'S')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Baud rate has non-numeric value";
@@ -180,9 +180,9 @@
                 return "Not compatible with the sensor model";
         }
     }
-    else if (cmd[0] == 'C' && cmd[1] == 'R')
+    else if(cmd[0] == 'C' && cmd[1] == 'R')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Invalid speed";
@@ -194,9 +194,9 @@
                 return "Not compatible with the sensor model";
         }
     }
-    else if (cmd[0] == 'H' && cmd[1] == 'S')
+    else if(cmd[0] == 'H' && cmd[1] == 'S')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Parameter error";
@@ -207,9 +207,9 @@
         }
     }
 // No info in the manual for this.
-//    else if (cmd[0] == 'R' && cmd[1] == 'S')
+//    else if(cmd[0] == 'R' && cmd[1] == 'S')
 //    {
-//        switch (error_code)
+//        switch(error_code)
 //        {
 //            case :
 //                return "";
@@ -221,35 +221,35 @@
 //        }
 //    }
 // No info in the manual for this.
-//    else if (cmd[0] == 'V' && cmd[1] == 'V')
+//    else if(cmd[0] == 'V' && cmd[1] == 'V')
 //    {
-//        switch (error_code)
+//        switch(error_code)
 //        {
 //            case :
 //                return "";
 //        }
 //    }
 // No info in the manual for this.
-//    else if (cmd[0] == 'P' && cmd[1] == 'P')
+//    else if(cmd[0] == 'P' && cmd[1] == 'P')
 //    {
-//        switch (error_code)
+//        switch(error_code)
 //        {
 //            case :
 //                return "";
 //        }
 //    }
 // No info in the manual for this.
-//    else if (cmd[0] == 'I' && cmd[1] == 'I')
+//    else if(cmd[0] == 'I' && cmd[1] == 'I')
 //    {
-//        switch (error_code)
+//        switch(error_code)
 //        {
 //            case :
 //                return "";
 //        }
 //    }
-    else if (cmd[0] == 'D' && cmd[1] == 'B')
+    else if(cmd[0] == 'D' && cmd[1] == 'B')
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "Parameter error";

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_errors.h	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -77,7 +77,7 @@
 
         /** Formatted description of the error. */
         std::stringstream ss;
-        /** Static string representation of the error. */
+        /** String representation of the error. */
         char _error_type[32];
 }; //class BaseError
 
@@ -113,10 +113,6 @@
             : BaseError(desc_code, error_type)
         {}
         virtual ~RuntimeError() throw() {};
-
-    protected:
-        /** Static string representation of the error. */
-        static char const* _error_type;
 }; // class RuntimeError
 
 
@@ -449,7 +445,7 @@
 }; // class MissingFirmSpecError
 
 
-/// Bad response error
+/// Bad response error - may be sent in response to any command
 class HOKUYO_AIST_EXPORT ResponseError: public ProtocolError
 {
     public:

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -42,7 +42,7 @@
     : _error(error), _time(time), _model(model)
 {
     _length = length;
-    if (_length == 0)
+    if(_length == 0)
     {
         _ranges = NULL;
         _intensities = NULL;
@@ -53,7 +53,7 @@
         {
             _ranges = new uint32_t[_length];
         }
-        catch (std::bad_alloc &e)
+        catch(std::bad_alloc& e)
         {
             _length = 0;
             throw;
@@ -65,13 +65,13 @@
 }
 
 
-ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
+ScanData::ScanData(uint32_t* ranges, uint32_t* intensities,
         unsigned int length, bool error, unsigned int time,
         LaserModel model)
     : _error(error), _time(time), _model(model)
 {
     _length = length;
-    if (_length == 0)
+    if(_length == 0)
     {
         _ranges = NULL;
         _intensities = NULL;
@@ -82,7 +82,7 @@
         {
             _ranges = new uint32_t[_length];
         }
-        catch (std::bad_alloc &e)
+        catch(std::bad_alloc& e)
         {
             _length = 0;
             throw;
@@ -95,10 +95,10 @@
 }
 
 
-ScanData::ScanData(const ScanData &rhs)
+ScanData::ScanData(ScanData const& rhs)
 {
     _length = rhs.length();
-    if (_length == 0)
+    if(_length == 0)
         _ranges = NULL;
     else
     {
@@ -106,14 +106,14 @@
         {
             _ranges = new uint32_t[_length];
         }
-        catch (std::bad_alloc &e)
+        catch(std::bad_alloc& e)
         {
             _length = 0;
             throw;
         }
         memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
 
-        if (rhs.intensities() != NULL)
+        if(rhs.intensities() != NULL)
         {
             _intensities = new uint32_t[_length];
             memcpy(_intensities, rhs.intensities(),
@@ -135,9 +135,9 @@
 
 std::string ScanData::error_code_to_string(uint32_t error_code)
 {
-    if (_model == MODEL_UTM30LX)
+    if(_model == MODEL_UTM30LX)
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 1:
                 return "No object in the range.";
@@ -157,7 +157,7 @@
     }
     else
     {
-        switch (error_code)
+        switch(error_code)
         {
             case 0:
                 return "Detected object is possibly at 22m.";
@@ -210,15 +210,15 @@
 }
 
 
-ScanData& ScanData::operator=(const ScanData &rhs)
+ScanData& ScanData::operator=(ScanData const& rhs)
 {
-    if (rhs.length() == 0)
+    if(rhs.length() == 0)
     {
         _length = 0;
-        if (_ranges != NULL)
+        if(_ranges != NULL)
             delete[] _ranges;
         _ranges = NULL;
-        if (_intensities != NULL)
+        if(_intensities != NULL)
             delete[] _intensities;
         _intensities = NULL;
         _error = rhs.get_error_status();
@@ -228,37 +228,37 @@
     else
     {
         unsigned int rhslength = rhs.length();
-        uint32_t *newData;
-        if (rhslength != _length)
+        uint32_t* newData;
+        if(rhslength != _length)
         {
             // Copy the data into a temporary variable pointing to new space
             // (prevents dangling pointers on allocation error and prevents
             // self-assignment making a mess).
             newData = new uint32_t[rhslength];
             memcpy(newData, rhs.ranges(), sizeof(uint32_t) * rhslength);
-            if (_ranges != NULL)
+            if(_ranges != NULL)
                 delete[] _ranges;
             _ranges = newData;
             _length = rhs.length();
 
-            if (rhs.intensities() != NULL)
+            if(rhs.intensities() != NULL)
             {
                 try
                 {
                     newData = new uint32_t[rhslength];
                 }
-                catch (std::bad_alloc &e)
+                catch(std::bad_alloc& e)
                 {
                     // We have to remove any old intensity data or the length
                     // won't match
-                    if (_intensities != NULL)
+                    if(_intensities != NULL)
                         delete[] _intensities;
                     _intensities = NULL;
                     throw;
                 }
                 memcpy(newData, rhs.intensities(), sizeof(uint32_t) *
                         rhslength);
-                if (_intensities != NULL)
+                if(_intensities != NULL)
                     delete[] _intensities;
                 _intensities = newData;
             }
@@ -267,7 +267,7 @@
         {
             // If lengths are the same, no need to reallocate
             memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
-            if (rhs.intensities() != NULL)
+            if(rhs.intensities() != NULL)
                 memcpy(_intensities, rhs.intensities(), sizeof(uint32_t) *
                         _length);
         }
@@ -283,7 +283,7 @@
 
 uint32_t ScanData::operator[](unsigned int index)
 {
-    if (index >= _length)
+    if(index >= _length)
         throw IndexError();
     return _ranges[index];
 }
@@ -297,19 +297,19 @@
     ss << model_to_string(_model) << ":\n";
     for (unsigned int ii = 0; ii < _length; ii++)
         ss << _ranges[ii] << "\t";
-    if (_intensities != NULL)
+    if(_intensities != NULL)
     {
         ss << std::endl << _length << " intensities:\n";
         for (unsigned int ii = 0; ii < _length; ii++)
             ss << _intensities[ii] << "\t";
     }
     ss << '\n';
-    if (_error)
+    if(_error)
     {
         ss << "Detected data errors:\n";
         for (unsigned int ii = 0; ii < _length; ii++)
         {
-            if (_ranges[ii] < 20)
+            if(_ranges[ii] < 20)
                 ss << ii << ": " <<
                     error_code_to_string(_ranges[ii]) << '\n';
         }
@@ -324,10 +324,10 @@
 
 void ScanData::clean_up()
 {
-    if (_ranges != NULL)
+    if(_ranges != NULL)
         delete[] _ranges;
     _ranges = NULL;
-    if (_intensities != NULL)
+    if(_intensities != NULL)
         delete[] _intensities;
     _intensities = NULL;
     _length = 0;
@@ -339,13 +339,13 @@
 void ScanData::allocate_data(unsigned int length, bool include_intensities)
 {
     // If no data yet, allocate new
-    if (_ranges == NULL)
+    if(_ranges == NULL)
     {
         try
         {
             _ranges = new uint32_t[length];
         }
-        catch (std::bad_alloc &e)
+        catch(std::bad_alloc& e)
         {
             _length = 0;
             throw;
@@ -353,14 +353,14 @@
         _length = length;
     }
     // If there is data, reallocate only if the length is different
-    else if (length != _length)
+    else if(length != _length)
     {
         delete[] _ranges;
         try
         {
             _ranges = new uint32_t[length];
         }
-        catch (std::bad_alloc &e)
+        catch(std::bad_alloc& e)
         {
             _length = 0;
             throw;
@@ -369,22 +369,22 @@
     }
     // Else data is already allocated to the right length, so do nothing
 
-    if (include_intensities)
+    if(include_intensities)
     {
         // If no data yet, allocate new
-        if (_intensities == NULL)
+        if(_intensities == NULL)
         {
             _intensities = new uint32_t[length];
         }
         // If there is data, reallocate only if the length is different
-        else if (length != _length)
+        else if(length != _length)
         {
             delete[] _intensities;
             _intensities = new uint32_t[length];
         }
         // Else data is already allocated to the right length, so do nothing
     }
-    else if (_intensities != NULL)
+    else if(_intensities != NULL)
     {
         // If not told to allocate space for intensity data and it exists,
         // remove it

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -66,7 +66,7 @@
                 unsigned int length, bool error, unsigned int time,
                 LaserModel model);
         /// This copy constructor performs a deep copy of present data.
-        ScanData(const ScanData &rhs);
+        ScanData(ScanData const& rhs);
         ~ScanData();
 
         /** @brief Return a pointer to array of range readings in millimetres.
@@ -97,7 +97,7 @@
         LaserModel model() const { return _model; }
 
         /// @brief Assignment operator.
-        ScanData& operator=(const ScanData &rhs);
+        ScanData& operator=(ScanData const& rhs);
         /** @brief Subscript operator.
 
         Provides direct access to an element of the range data. */
@@ -110,8 +110,8 @@
         void clean_up();
 
     protected:
-        uint32_t *_ranges;
-        uint32_t *_intensities;
+        uint32_t* _ranges;
+        uint32_t* _intensities;
         unsigned int _length;
         bool _error;
         unsigned int _time;

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -27,6 +27,7 @@
 #include <flexiport/port.h>
 #include <flexiport/serialport.h>
 
+#include <cassert>
 #include <cstring>
 #include <stdarg.h>
 #include <stdlib.h>
@@ -45,9 +46,9 @@
 {
 
 // SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
+unsigned int const SCIP1_LINE_LENGTH = 66;
 // SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
+unsigned int const SCIP2_LINE_LENGTH = 67;
 
 ///////////////////////////////////////////////////////////////////////////////
 // SCIP protocol version 1 notes
@@ -213,7 +214,7 @@
 // Utility functions
 ///////////////////////////////////////////////////////////////////////////////
 
-unsigned int decode_2_byte_value(char *data)
+unsigned int decode_2_byte_value(char* data)
 {
     unsigned int byte1, byte2;
 
@@ -224,7 +225,7 @@
 }
 
 
-unsigned int decode_3_byte_value(char *data)
+unsigned int decode_3_byte_value(char* data)
 {
     unsigned int byte1, byte2, byte3;
 
@@ -236,7 +237,7 @@
 }
 
 
-unsigned int decode_4_byte_value(char *data)
+unsigned int decode_4_byte_value(char* data)
 {
     unsigned int byte1, byte2, byte3, byte4;
 
@@ -290,14 +291,14 @@
 
 Sensor::~Sensor()
 {
-    if (_port != NULL)
+    if(_port != NULL)
         delete _port;
 }
 
 
 void Sensor::open(std::string port_options)
 {
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() Creating and opening port using options: " <<
@@ -306,7 +307,7 @@
     _port = flexiport::CreatePort(port_options);
     _port->Open();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Connected using " <<
             _port->GetPortType() << " connection.\n";
@@ -324,7 +325,7 @@
 
 unsigned int Sensor::open_with_probing(std::string port_options)
 {
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() Creating and opening port using options: " << port_options <<
@@ -333,7 +334,7 @@
     _port = flexiport::CreatePort(port_options);
     _port->Open();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Connected using " <<
             _port->GetPortType() << " connection.\n";
@@ -349,20 +350,20 @@
         // Get some values we need for providing default ranges
         _get_defaults();
     }
-    catch (BaseError)
+    catch(BaseError)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Failed to connect at the default baud rate.\n";
         }
-        if (_port->GetPortType() == "serial")
+        if(_port->GetPortType() == "serial")
         {
             // Failed at the default baud rate, so try again at the other
             // rates. Note that a baud rate of 750000 or 250000 doesn't appear
             // to be supported on any common OS.
-            const unsigned int bauds[] = {500000, 115200, 57600, 38400, 19200};
-            const unsigned int numBauds = 5;
+            unsigned int const bauds[] = {500000, 115200, 57600, 38400, 19200};
+            unsigned int const numBauds(5);
             for (unsigned int ii = 0; ii < numBauds; ii++)
             {
                 reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(bauds[ii]);
@@ -372,19 +373,19 @@
                     _get_defaults();
                     // If the above two functions succeed, break out of the
                     // loop and be happy
-                    if (_verbose)
+                    if(_verbose)
                     {
                         _err_output << "Sensor::" << __func__ <<
                             "() Connected at " << bauds[ii] << '\n';
                     }
                     return bauds[ii];
                 }
-                catch (BaseError)
+                catch(BaseError)
                 {
-                    if (ii == numBauds - 1)
+                    if(ii == numBauds - 1)
                     {
                         // Last baud rate, give up and rethrow
-                        if (_verbose)
+                        if(_verbose)
                         {
                             _err_output << "Sensor::" << __func__ <<
                                 "() Failed to connect at any baud rate.\n";
@@ -397,7 +398,7 @@
         }
         else
         {
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Sensor::" << __func__ <<
                     "() Port is not serial, cannot probe.\n";
@@ -406,7 +407,7 @@
         }
     }
 
-    if (_port->GetPortType() == "serial")
+    if(_port->GetPortType() == "serial")
         return reinterpret_cast<flexiport::SerialPort*>(_port)->GetBaudRate();
     else
         return 0;
@@ -415,9 +416,9 @@
 
 void Sensor::close()
 {
-    if (!_port)
+    if(!_port)
         throw CloseError();
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
     delete _port;
     _port = NULL;
@@ -426,7 +427,7 @@
 
 bool Sensor::is_open() const
 {
-    if (_port != NULL)
+    if(_port != NULL)
         return _port->IsOpen();
     return false;
 }
@@ -434,36 +435,36 @@
 
 void Sensor::set_power(bool on)
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
-        if (on)
+        if(on)
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
             _send_command("L", "1", 1, NULL);
         }
         else
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
             _send_command("L", "0", 1, NULL);
         }
         _skip_lines(1);
     }
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (on)
+        if(on)
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
             _send_command("BM", NULL, 0, "02");
         }
         else
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
             _send_command("QT", NULL, 0, "02");
@@ -479,22 +480,20 @@
 // set to the same baud.
 void Sensor::set_baud(unsigned int baud)
 {
-    if (_port->GetPortType() != "serial")
-    {
+    if(_port->GetPortType() != "serial")
         throw NotSerialError();
-    }
 
     char newBaud[13];
     memset(newBaud, 0, sizeof(char) * 13);
 
-    if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
+    if(baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
         baud != 250000 && baud != 500000 && baud != 750000)
     {
         throw BaudrateError(baud);
     }
     number_to_string(baud, newBaud, 6);
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
         // Send the command to change baud rate
         _send_command("S", newBaud, 13, NULL);
@@ -502,7 +501,7 @@
         // Change the port's baud rate
         reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
     }
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
         // Send the command to change baud rate
         _send_command("SS", newBaud, 6, "03");
@@ -532,7 +531,7 @@
         '.' << subnet.fourth << ' ';
     params << gateway.first << '.' << gateway.second << '.' << gateway.third <<
         '.' << gateway.fourth;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() Setting IP information to $I" << params.str() << '\n';
@@ -540,18 +539,18 @@
     int status = _send_command(&command[0], params.str().c_str(), 48, NULL);
     // Skip the extra line feed
     _skip_lines(1);
-    if (status != 0)
+    if(status != 0)
         throw SetIPError();
 }
 
 
 void Sensor::reset()
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(7);
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
@@ -566,11 +565,11 @@
 
 void Sensor::semi_reset()
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(35);
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (_verbose)
+        if(_verbose)
             _err_output << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
         _send_command("RS", NULL, 0, NULL);
@@ -583,17 +582,17 @@
 
 void Sensor::set_motor_speed(unsigned int speed)
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(8);
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
         // Sanity check the value
-        if (speed > 10 && speed != 99)
+        if(speed > 10 && speed != 99)
             throw MotorSpeedError();
         char buffer[3];
-        if (speed == 0)
+        if(speed == 0)
         {
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Sensor::" << __func__ <<
                     "() Reseting motor speed to default.\n";
@@ -602,9 +601,9 @@
             buffer[1] = '0';
             buffer[2] = '\0';
         }
-        else if (speed == 99)
+        else if(speed == 99)
         {
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Sensor::" << __func__ <<
                     "() Reseting motor speed to default.\n";
@@ -615,7 +614,7 @@
         }
         else
         {
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Sensor::" << __func__ <<
                     "() Setting motor speed to ratio " << speed << '\n';
@@ -632,20 +631,20 @@
 
 void Sensor::set_high_sensitivity(bool on)
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(10);
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (on)
+        if(on)
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Switching to high sensitivity.\n";
             _send_command("HS", "1", 1, "02");
         }
         else
         {
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Sensor::" << __func__ <<
                     "() Switching to normal sensitivity.\n";
@@ -659,14 +658,14 @@
 }
 
 
-void Sensor::get_sensor_info(SensorInfo *info)
+void Sensor::get_sensor_info(SensorInfo* info)
 {
-    if (info == NULL)
+    if(info == NULL)
         throw NoDestinationError();
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Getting sensor information using SCIP version 1.\n";
@@ -697,7 +696,7 @@
         info->serial = &buffer[5];
         // Get either the status line or the end of message
         _read_line(buffer);
-        if (buffer[0] != '\0')
+        if(buffer[0] != '\0')
         {
             // Got a status line
             info->sensor_diagnostic = &buffer[5];
@@ -709,17 +708,17 @@
         // eg: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
         // Note that this example is right up against the maximum SCIP v1 line
         // length of 64 bytes.
-        if (atoi(info->firmware.c_str()) >= 3)
+        if(atoi(info->firmware.c_str()) >= 3)
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "SCIP1 Firmware line for parsing: " <<
                     info->firmware << '\n';
             // Now the fun part: parsing the line. It would be nice if we could
             // use the POSIX regex functions, but since MS doesn't believe in
             // POSIX we get to do it the hard way.
             // Start by finding the first (
-            const char *valueStart;
-            if ((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
+            char const* valueStart;
+            if((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
             {
                 // No bracket? Crud. Fail and use the hard-coded values from
                 // the manual.
@@ -731,13 +730,13 @@
                     "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info->min_range,
                     &info->max_range, &aperture, &info->first_step,
                     &info->last_step, &info->speed);
-            if (numFound != 6)
+            if(numFound != 6)
             {
                 // Didn't get enough values out, assume unknown format and fall
                 // back on the defaults
                 info->set_defaults();
                 info->calculate_values();
-                if (_verbose)
+                if(_verbose)
                 {
                     _err_output << "Retrieved sensor info (hard-coded, not "
                         "enough values):\n";
@@ -759,7 +758,7 @@
                 info->max_angle = (info->last_step - info->front_step) *
                     info->resolution;
 
-                if (_verbose)
+                if(_verbose)
                 {
                     _err_output << "Retrieved sensor info (from FIRM line):\n";
                     _err_output << info->as_string();
@@ -771,16 +770,16 @@
             // We're stuck with hard-coded defaults from the manual (already
             // set earlier).
             info->calculate_values();
-            if (_verbose)
+            if(_verbose)
             {
                 _err_output << "Retrieved sensor info (hard-coded):\n";
                 _err_output << info->as_string();
             }
         }
     }
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Getting sensor information using SCIP version 2.\n";
@@ -794,23 +793,23 @@
         // We need to send three commands to get all the info we want: VV, PP
         // and II
         _send_command("VV", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
+        while(_read_line_with_check(buffer, -1, true) != 0)
             _process_vv_line(buffer, info);
 
         // Next up, PP
         _send_command("PP", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
+        while(_read_line_with_check(buffer, -1, true) != 0)
             _process_pp_line(buffer, info);
 
         // Command II: Revenge of the Commands.
         _send_command("II", NULL, 0, NULL);
-        while (_read_line_with_check(buffer, -1, true) != 0)
+        while(_read_line_with_check(buffer, -1, true) != 0)
             _process_ii_line(buffer, info);
 
         _enable_checksum_workaround = false;
 
         info->calculate_values();
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Retrieved sensor info:\n";
             _err_output << info->as_string();
@@ -823,11 +822,11 @@
 
 unsigned int Sensor::get_time()
 {
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(12);
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (_verbose)
+        if(_verbose)
             _err_output << "Sensor::" << __func__ <<
                 "() Retrieving time from laser.\n";
         _send_command("TM", "0", 1, NULL);
@@ -846,22 +845,22 @@
 }
 
 
-unsigned int Sensor::get_ranges(ScanData *data, int start_step,
+unsigned int Sensor::get_ranges(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
 
     char buffer[11];
     memset(buffer, 0, sizeof(char) * 11);
 
-    if (start_step < 0)
+    if(start_step < 0)
         start_step = _first_step;
-    if (end_step < 0)
+    if(end_step < 0)
         end_step = _last_step;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges between " << start_step << " and " <<
@@ -869,7 +868,7 @@
             '\n';
     }
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
         // Send the command to ask for the most recent range data from
         // start_step to end_step
@@ -880,21 +879,21 @@
         // In SCIP1 mode we're going to get back 2-byte data
         _read_2_byte_range_data(data, num_steps);
     }
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
         // Send the command to ask for the most recent range data from
         // start_step to end_step
         number_to_string(start_step, buffer, 4);
         number_to_string(end_step, &buffer[4], 4);
         number_to_string(cluster_count, &buffer[8], 2);
-        if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+        if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
             _send_command("HD", buffer, 10, NULL);
         else
             _send_command("GD", buffer, 10, NULL);
         // There will be a timestamp before the data (if there is data)
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
-        if (_read_line_with_check(buffer) == 0)
+        if(_read_line_with_check(buffer) == 0)
             throw NoDataError();
         data->_time = decode_4_byte_value(buffer);
         // In SCIP2 mode we're going to get back 3-byte data because we're
@@ -908,10 +907,10 @@
 }
 
 
-unsigned int Sensor::get_ranges_by_angle(ScanData *data, double start_angle,
+unsigned int Sensor::get_ranges_by_angle(ScanData* data, double start_angle,
         double end_angle, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDataError();
 
     // Calculate the given angles in steps, rounding towards _front_step
@@ -920,12 +919,12 @@
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
+    if(start_step < _first_step || start_step > _last_step)
         throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
+    if(end_step < _first_step || end_step > _last_step)
         throw EndStepError();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
@@ -937,22 +936,22 @@
 }
 
 
-unsigned int Sensor::get_ranges_intensities(ScanData *data, int start_step,
+unsigned int Sensor::get_ranges_intensities(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
 
     char buffer[11];
     memset(buffer, 0, sizeof(char) * 11);
 
-    if (start_step < 0)
+    if(start_step < 0)
         start_step = _first_step;
-    if (end_step < 0)
+    if(end_step < 0)
         end_step = _last_step;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges between " << start_step << " and " <<
@@ -960,9 +959,9 @@
             '\n';
     }
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(36);
-    else if (_scip_version != 2)
+    else if(_scip_version != 2)
         throw UnknownScipVersionError();
 
     // Send the command to ask for the most recent data from
@@ -970,14 +969,14 @@
     number_to_string(start_step, buffer, 4);
     number_to_string(end_step, &buffer[4], 4);
     number_to_string(cluster_count, &buffer[8], 2);
-    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
         _send_command("HE", buffer, 10, NULL);
     else
         _send_command("GE", buffer, 10, NULL);
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if (_read_line_with_check(buffer) == 0)
+    if(_read_line_with_check(buffer) == 0)
         throw NoDataError();
     data->_time = decode_4_byte_value(buffer);
     // In SCIP2 mode we're going to get back 3-byte data because we're
@@ -988,10 +987,10 @@
 }
 
 
-unsigned int Sensor::get_ranges_intensities_by_angle(ScanData *data,
+unsigned int Sensor::get_ranges_intensities_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDataError();
 
     // Calculate the given angles in steps, rounding towards _front_step
@@ -1000,12 +999,12 @@
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
+    if(start_step < _first_step || start_step > _last_step)
         throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
+    if(end_step < _first_step || end_step > _last_step)
         throw EndStepError();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
@@ -1017,27 +1016,27 @@
 }
 
 
-unsigned int Sensor::get_new_ranges(ScanData *data, int start_step,
+unsigned int Sensor::get_new_ranges(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(16);
-    else if (_scip_version != 2)
+    else if(_scip_version != 2)
         throw UnknownScipVersionError();
 
     char buffer[14];
     memset(buffer, 0, sizeof(char) * 14);
 
-    if (start_step < 0)
+    if(start_step < 0)
         start_step = _first_step;
-    if (end_step < 0)
+    if(end_step < 0)
         end_step = _last_step;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " new ranges between " << start_step << " and " <<
@@ -1052,7 +1051,7 @@
     number_to_string(1, &buffer[10], 1);
     number_to_string(1, &buffer[11], 2);
     char command[3];
-    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
         command[0] = 'N';
     else
         command[0] = 'M';
@@ -1066,22 +1065,22 @@
     _skip_lines(1); // End of the command echo message
     _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
-    if (response[0] != command[0] || response[1] != command[1])
+    if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
     // Then compare the parameters
     buffer[12] = '0'; // There will be zero scans remaining after this one
-    if (memcmp(&response[2], buffer, 13) != 0)
+    if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
     _read_line_with_check(response, 4);
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() " << command << " data prefix status: " << response[0] <<
             response[1] << '\n';
     }
     // Check the status code is OK - should only get 99 here
-    if (response[0] != '9' || response[1] != '9')
+    if(response[0] != '9' || response[1] != '9')
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
@@ -1093,7 +1092,7 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if (_read_line_with_check(buffer) == 0)
+    if(_read_line_with_check(buffer) == 0)
         throw NoDataError();
     data->_time = decode_4_byte_value(buffer);
     // In SCIP2 mode we're going to get back 3-byte data because we're
@@ -1104,12 +1103,12 @@
 }
 
 
-unsigned int Sensor::get_new_ranges_by_angle(ScanData *data,
+unsigned int Sensor::get_new_ranges_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(16);
 
     // Calculate the given angles in steps, rounding towards _front_step
@@ -1118,12 +1117,12 @@
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
+    if(start_step < _first_step || start_step > _last_step)
         throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
+    if(end_step < _first_step || end_step > _last_step)
         throw EndStepError();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
@@ -1135,27 +1134,27 @@
 }
 
 
-unsigned int Sensor::get_new_ranges_intensities(ScanData *data,
+unsigned int Sensor::get_new_ranges_intensities(ScanData* data,
         int start_step, int end_step, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(17);
-    else if (_scip_version != 2)
+    else if(_scip_version != 2)
         throw UnknownScipVersionError();
 
     char buffer[14];
     memset(buffer, 0, sizeof(char) * 14);
 
-    if (start_step < 0)
+    if(start_step < 0)
         start_step = _first_step;
-    if (end_step < 0)
+    if(end_step < 0)
         end_step = _last_step;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " new ranges and intensities between " <<
@@ -1171,7 +1170,7 @@
     number_to_string(1, &buffer[10], 1);
     number_to_string(1, &buffer[11], 2);
     char command[3];
-    if (_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
         command[0] = 'N';
     else
         command[0] = 'M';
@@ -1185,21 +1184,22 @@
     _skip_lines(1); // End of the command echo message
     _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
-    if (response[0] != command[0] || response[1] != command[1])
+    if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
     // Then compare the parameters
-    if (memcmp(&response[2], buffer, 13) != 0)
+    buffer[12] = '0'; // There will be zero scans remaining after this one
+    if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
     _read_line_with_check(response, 4);
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() " << command << " data prefix status: " << response[0] <<
             response[1] << '\n';
     }
     // Check the status code is OK - should only get 99 here
-    if (response[0] != '9' || response[1] != '9')
+    if(response[0] != '9' || response[1] != '9')
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
@@ -1211,7 +1211,7 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if (_read_line_with_check(buffer) == 0)
+    if(_read_line_with_check(buffer) == 0)
         throw NoDataError();
     data->_time = decode_4_byte_value(buffer);
     // In SCIP2 mode we're going to get back 3-byte data because we're
@@ -1222,12 +1222,12 @@
 }
 
 
-unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData *data,
+unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if (data == NULL)
+    if(data == NULL)
         throw NoDestinationError();
-    if (_scip_version == 1)
+    if(_scip_version == 1)
         throw UnsupportedError(17);
 
     // Calculate the given angles in steps, rounding towards _front_step
@@ -1236,12 +1236,12 @@
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if (start_step < _first_step || start_step > _last_step)
+    if(start_step < _first_step || start_step > _last_step)
         throw StartStepError();
-    if (end_step < _first_step || end_step > _last_step)
+    if(end_step < _first_step || end_step > _last_step)
         throw EndStepError();
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
@@ -1269,7 +1269,7 @@
         (static_cast<double>(angle) / static_cast<double>(_resolution));
     // Round towards _front_step so that the step values are always inside the
     // angles given
-    if (resultF < _front_step)
+    if(resultF < _front_step)
         result = static_cast<int>(ceil(resultF));
     else
         result = static_cast<int>(floor(resultF));
@@ -1290,7 +1290,7 @@
 // timeout (which may be infinite).
 void Sensor::_clear_read_buffer()
 {
-    while (_port->BytesAvailableWait() > 0)
+    while(_port->BytesAvailableWait() > 0)
         _port->Flush();
 }
 
@@ -1303,46 +1303,46 @@
 // The line feed that terminates a line will be replaced with a NULL.
 // The return value is the number of bytes received, not including the NULL
 // byte or the line feed.
-int Sensor::_read_line(char *buffer, int expected_length)
+int Sensor::_read_line(char* buffer, int expected_length)
 {
     int linelength = 0;
 
-    if (expected_length == -1)
+    if(expected_length == -1)
     {
         int maxlength = (_scip_version == 1) ?
             SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ << "() Reading up to " <<
                 maxlength << " bytes.\n";
         }
         // We need to get at least 1 byte in a line: the line feed.
-        if ((linelength = _port->ReadLine(buffer, maxlength)) < 0)
+        if((linelength = _port->ReadLine(buffer, maxlength)) < 0)
             throw ReadError(0);
-        else if (linelength == 0)
+        else if(linelength == 0)
             throw ReadError(1);
         // Replace the line feed with a NULL
         buffer[linelength - 1] = '\0';
     }
     else
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Reading exactly " << expected_length << " bytes.\n";
         }
         // expected_length+1 for the NULL
-        if ((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
+        if((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
             throw ReadError(0);
-        else if (linelength == 0)
+        else if(linelength == 0)
             throw ReadError(1);
-        else if (linelength < expected_length)
+        else if(linelength < expected_length)
             throw LineLengthError(linelength, expected_length);
         // Replace the line feed with a NULL
         buffer[linelength - 1] = '\0';
     }
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
             " bytes.\n";
@@ -1370,11 +1370,11 @@
 // _model is MODEL_UTM30LX. In this case, if the checksum fails normally, it
 // scans the line for "<-" and recalculates the checksum on the bytes up to
 // that point. This only happens in SCIP v2.
-int Sensor::_read_line_with_check(char *buffer, int expected_length,
+int Sensor::_read_line_with_check(char* buffer, int expected_length,
         bool has_semicolon)
 {
     int linelength = _read_line(buffer, expected_length);
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
         // No checksums in SCIP version 1
         return linelength;
@@ -1382,20 +1382,20 @@
 
     // If the line is empty, assume it was a line-feed message terminator, in
     // which case there is no checksum to check.
-    if (linelength == 0)
+    if(linelength == 0)
         return 0;
 
     // Ignore the checksum itself, and possibly a semicolon (_read_line has
     // already chopped off the line feed for us).
     int bytesToConsider = linelength - 1 - (has_semicolon ? 1 : 0);
     int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Considering " <<
             bytesToConsider << " bytes for checksum from a line length of " <<
             linelength << " bytes.\n";
     }
-    if (bytesToConsider < 1)
+    if(bytesToConsider < 1)
         throw InsufficientBytesError(bytesToConsider, linelength);
 
     int checkSum = 0;
@@ -1404,23 +1404,23 @@
         checkSum = _confirm_checksum(buffer, bytesToConsider,
                 static_cast<int>(buffer[checksumIndex]));
     }
-    catch (ProtocolError &e)
+    catch(ProtocolError& e)
     {
-        if (_model == MODEL_UTM30LX && _enable_checksum_workaround)
+        if(_model == MODEL_UTM30LX && _enable_checksum_workaround)
         {
             // Here comes the UTM-30LX workaround
             char* hasComment = strstr(buffer, "<-");
-            if (hasComment != NULL)
+            if(hasComment != NULL)
             {
                 int newBytesToConsider = hasComment - buffer;
-                if (_verbose)
+                if(_verbose)
                 {
                     _err_output << "Sensor::" << __func__ <<
                         "() Performing UTM-30LX II response "
                         "checksum workaround: trying with " <<
                         newBytesToConsider << " bytes.\n";
                 }
-                if (newBytesToConsider < 1)
+                if(newBytesToConsider < 1)
                 {
                     throw InsufficientBytesError(newBytesToConsider,
                             linelength);
@@ -1448,10 +1448,10 @@
 // Reads lines until the number specified has passed.
 void Sensor::_skip_lines(int count)
 {
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
             " lines.\n";
-    if (_port->SkipUntil(0x0A, count) < 0)
+    if(_port->SkipUntil(0x0A, count) < 0)
         throw ReadError(18);
 }
 
@@ -1465,8 +1465,8 @@
 // If param_length is 0, no parameters will be sent or expected in the reply.
 // extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
 // Return value is the status code returned for the command.
-int Sensor::_send_command(const char *cmd, const char *param,
-        int param_length, const char *extra_ok)
+int Sensor::_send_command(char const* cmd, char const* param,
+        int param_length, char const* extra_ok)
 {
     int statusCode = -1;
     char response[17];
@@ -1474,23 +1474,23 @@
     // Flush first to clear out the dregs of any previous commands
     _port->Flush();
 
-    if (_scip_version == 1)
+    if(_scip_version == 1)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Writing in SCIP1 mode. Command is " << cmd[0] <<
                 ", parameters length is " << param_length << '\n';
         }
         // Write the command
-        if (_port->Write(cmd, 1) < 1)
+        if(_port->Write(cmd, 1) < 1)
             throw WriteError(19);
-        if (param_length > 0)
+        if(param_length > 0)
         {
-            if (_port->Write(param, param_length) < param_length)
+            if(_port->Write(param, param_length) < param_length)
                 throw WriteError(20);
         }
-        if (_port->Write("\n", 1) < 1)
+        if(_port->Write("\n", 1) < 1)
             throw WriteError(21);
 
         // Read back the response (should get at least 4 bytes , possibly up to
@@ -1500,7 +1500,7 @@
         _read_line(response, 2 + param_length);
         _read_line(&response[statusIndex], 2);
         // First make sure that the echoed command matches
-        if (response[0] != cmd[0])
+        if(response[0] != cmd[0])
         {
             char temp_cmd[2];
             temp_cmd[0] = cmd[0];
@@ -1511,9 +1511,9 @@
             throw CommandEchoError(temp_cmd, temp_echo);
         }
         // Then compare the parameters
-        if (param_length > 0)
+        if(param_length > 0)
         {
-            if (memcmp(&response[1], param, param_length) != 0)
+            if(memcmp(&response[1], param, param_length) != 0)
             {
                 char temp_cmd[2];
                 temp_cmd[0] = cmd[0];
@@ -1522,17 +1522,17 @@
             }
         }
         // Next up, check the status byte
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Command response status: " << response[statusIndex] <<
                 '\n';
         }
-        if (response[statusIndex] != '0')
+        if(response[statusIndex] != '0')
         {
-            if (extra_ok != NULL)
+            if(extra_ok != NULL)
             {
-                if (response[statusIndex] != extra_ok[0])
+                if(response[statusIndex] != extra_ok[0])
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
@@ -1551,40 +1551,40 @@
         statusCode = atoi(&response[statusIndex]);
         // All OK, data starts at beginning of port's buffer
     }
-    else if (_scip_version == 2)
+    else if(_scip_version == 2)
     {
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Writing in SCIP2 mode. Command is " << cmd <<
                 ", parameters length is " << param_length << '\n';
         }
         // Write the command
-        if (_port->Write(cmd, 2) < 2)
+        if(_port->Write(cmd, 2) < 2)
             throw WriteError(19);
-        if (param_length > 0)
+        if(param_length > 0)
         {
-            if (_port->Write(param, param_length) < param_length)
+            if(_port->Write(param, param_length) < param_length)
                 throw WriteError(20);
         }
-        if (_port->Write("\n", 1) < 1)
+        if(_port->Write("\n", 1) < 1)
             throw WriteError(21);
 
         // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
         _read_line(response, 3 + param_length);
         // Check the echo is correct
-        if (response[0] != cmd[0] || response[1] != cmd[1])
+        if(response[0] != cmd[0] || response[1] != cmd[1])
             throw CommandEchoError(cmd, response);
         // Then compare the parameters
-        if (param_length > 0)
+        if(param_length > 0)
         {
-            if (memcmp(&response[2], param, param_length) != 0)
+            if(memcmp(&response[2], param, param_length) != 0)
                 throw ParamEchoError(cmd);
         }
 
         // The next line should be the status line
         _read_line_with_check(response, 4);
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Command response status: " << response[0] << response[1] <<
@@ -1592,12 +1592,12 @@
         }
         // Check the status code is OK
         response[2] = '\0';
-        if (!(response[0] == '0' && response[1] == '0') &&
+        if(!(response[0] == '0' && response[1] == '0') &&
             !(response[0] == '9' && response[1] == '9'))
         {
-            if (extra_ok != NULL)
+            if(extra_ok != NULL)
             {
-                if (response[0] != extra_ok[0] || response[1] != extra_ok[1])
+                if(response[0] != extra_ok[0] || response[1] != extra_ok[1])
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
@@ -1625,20 +1625,20 @@
 
 void Sensor::_find_model(char const* buffer)
 {
-    if (strstr(buffer, "URG-04LX") != NULL)
+    if(strstr(buffer, "URG-04LX") != NULL)
         _model = MODEL_URG04LX;
-    else if (strstr(buffer, "UBG-04LX") != NULL)
+    else if(strstr(buffer, "UBG-04LX") != NULL)
         _model = MODEL_UBG04LXF01;
-    else if (strstr(buffer, "UHG-08LX") != NULL)
+    else if(strstr(buffer, "UHG-08LX") != NULL)
         _model = MODEL_UHG08LX;
-    else if (strstr(buffer, "UTM-30LX") != NULL)
+    else if(strstr(buffer, "UTM-30LX") != NULL)
     {
         _model = MODEL_UTM30LX;
         // Also enable the work around for a checksum problem in this
         // model.
         _enable_checksum_workaround = true;
     }
-    else if (strstr(buffer, "UXM-30LX") != NULL)
+    else if(strstr(buffer, "UXM-30LX") != NULL)
         _model = MODEL_UXM30LXE;
     else
         _model = MODEL_UNKNOWN;
@@ -1650,7 +1650,7 @@
     bool scip2Failed = false;
 
 
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ <<
             "() Testing SCIP protocol version.\n";
     // Try SCIP version 2 first by sending an info command
@@ -1658,16 +1658,16 @@
     {
         _send_command("VV", NULL, 0, NULL);
     }
-    catch (BaseError)
+    catch(BaseError)
     {
         // That didn't work too well...
-        if (_verbose)
+        if(_verbose)
             _err_output << "Sensor::" << __func__ <<
                 "() Initial SCIP version 2 test failed.\n";
         scip2Failed = true;
     }
 
-    if (scip2Failed)
+    if(scip2Failed)
     {
         // Currently using SCIP version 1
         // Get the firmware version and check if we can move to SCIP version 2
@@ -1678,7 +1678,7 @@
         {
             _send_command("V", NULL, 0, NULL);
         }
-        catch (BaseError)
+        catch(BaseError)
         {
             throw ScipVersionError();
         }
@@ -1689,7 +1689,7 @@
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
         _read_line(buffer);
 
-        if (strncmp(buffer, "FIRM:", 5) != 0)
+        if(strncmp(buffer, "FIRM:", 5) != 0)
             throw MissingFirmSpecError();
         // Pull out the major version number
         // Note that although lasers such as the UTM-30LX appear to use a
@@ -1697,9 +1697,9 @@
         // don't support SCIP v1 and so shouldn't get to this point anyway - if
         // they do, it's an uncaught error.
         int majorVer = strtol(&buffer[5], NULL, 10);
-        if (errno == ERANGE)
+        if(errno == ERANGE)
             throw FirmwareError();
-        if (_verbose)
+        if(_verbose)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Firmware major version is " << majorVer <<
@@ -1711,9 +1711,9 @@
 
         // If the firmware version is less than 3, we're stuck with SCIP
         // version 1.
-        if (majorVer < 3)
+        if(majorVer < 3)
         {
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Firmware does not support SCIP version 2; using SCIP "
                     "version 1.\n";
@@ -1732,9 +1732,9 @@
             {
                 _send_command("S", "CIP2.0", 6, NULL);
             }
-            catch (BaseError)
+            catch(BaseError)
             {
-                if (_verbose)
+                if(_verbose)
                     _err_output << "Sensor::" << __func__ <<
                         "() Could not change to SCIP version 2; using SCIP "
                         "version 1.\n";
@@ -1744,7 +1744,7 @@
             _skip_lines(1);
 
             // Changed to SCIP version 2
-            if (_verbose)
+            if(_verbose)
                 _err_output << "Sensor::" << __func__ <<
                     "() Using SCIP version 2.\n";
             _scip_version = 2;
@@ -1758,7 +1758,7 @@
 
         // Dump the rest of the result
         _skip_lines(6);
-        if (_verbose)
+        if(_verbose)
             _err_output << "Sensor::" << __func__ <<
                 "() Using SCIP version 2.\n";
         return;
@@ -1771,7 +1771,7 @@
 
 void Sensor::_get_defaults()
 {
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ <<
             "() Getting default values.\n";
 
@@ -1786,7 +1786,7 @@
     _last_step = info.last_step;
     _front_step = info.front_step;
     _max_range = info.max_range;
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() Got default values: " << _min_angle << " " << _max_angle <<
@@ -1796,79 +1796,79 @@
 }
 
 
-void Sensor::_process_vv_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_vv_line(char const* buffer, SensorInfo* info)
 {
-    if (strncmp(buffer, "VEND", 4) == 0)
+    if(strncmp(buffer, "VEND", 4) == 0)
         info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
-    else if (strncmp(buffer, "PROD", 4) == 0)
+    else if(strncmp(buffer, "PROD", 4) == 0)
     {
         info->product = &buffer[5]; // Product info
         // Find the product model
         _find_model(&buffer[5]);
         info->detected_model = _model;
     }
-    else if (strncmp(buffer, "FIRM", 4) == 0)
+    else if(strncmp(buffer, "FIRM", 4) == 0)
         info->firmware = &buffer[5]; // Firmware version
-    else if (strncmp(buffer, "PROT", 4) == 0)
+    else if(strncmp(buffer, "PROT", 4) == 0)
         info->protocol = &buffer[5]; // Protocol version
-    else if (strncmp(buffer, "SERI", 4) == 0)
+    else if(strncmp(buffer, "SERI", 4) == 0)
         info->serial = &buffer[5]; // Serial number
-    else if (!_ignore_unknowns)
+    else if(!_ignore_unknowns)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::_process_pp_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_pp_line(char const* buffer, SensorInfo* info)
 {
-    if (strncmp(buffer, "MODL", 4) == 0)
+    if(strncmp(buffer, "MODL", 4) == 0)
         info->model = &buffer[5];   // Model
     // On to the fun ones that require parsing
-    else if (strncmp(buffer, "DMIN", 4) == 0)
+    else if(strncmp(buffer, "DMIN", 4) == 0)
         info->min_range = atoi(&buffer[5]);
-    else if (strncmp(buffer, "DMAX", 4) == 0)
+    else if(strncmp(buffer, "DMAX", 4) == 0)
         info->max_range = atoi(&buffer[5]);
-    else if (strncmp(buffer, "ARES", 4) == 0)
+    else if(strncmp(buffer, "ARES", 4) == 0)
         info->steps = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AMIN", 4) == 0)
+    else if(strncmp(buffer, "AMIN", 4) == 0)
         info->first_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AMAX", 4) == 0)
+    else if(strncmp(buffer, "AMAX", 4) == 0)
         info->last_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "AFRT", 4) == 0)
+    else if(strncmp(buffer, "AFRT", 4) == 0)
         info->front_step = atoi(&buffer[5]);
-    else if (strncmp(buffer, "SCAN", 4) == 0)
+    else if(strncmp(buffer, "SCAN", 4) == 0)
         info->standard_speed = atoi(&buffer[5]);
     /* No example in the manual and sensor with support for this has not
      * arrived yet, so don't know what to look for.
-    else if (strncmp(buffer, "", 4) == 0)
+    else if(strncmp(buffer, "", 4) == 0)
     {
-        if (strstr(buffer, "CCW") != NULL)
+        if(strstr(buffer, "CCW") != NULL)
             info->rot_dir = COUNTERCLOCKWISE;
         else
             info->rot_dir = CLOCKWISE;
     }*/
-    else if (!_ignore_unknowns)
+    else if(!_ignore_unknowns)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::_process_ii_line(const char *buffer, SensorInfo *info)
+void Sensor::_process_ii_line(char const* buffer, SensorInfo* info)
 {
-    if (strncmp(buffer, "MODL", 4) == 0)
+    if(strncmp(buffer, "MODL", 4) == 0)
         // Do nothing here - we already know this value from PP
         return;
-    else if (strncmp(buffer, "LASR", 4) == 0)
+    else if(strncmp(buffer, "LASR", 4) == 0)
     {
-        if (strncmp(&buffer[5], "OFF", 3) == 0)
+        if(strncmp(&buffer[5], "OFF", 3) == 0)
             info->power = false;
         else
             info->power = true;
     }
-    else if (strncmp(buffer, "SCSP", 4) == 0)
+    else if(strncmp(buffer, "SCSP", 4) == 0)
     {
-        if (strncmp(&buffer[5], "Initial", 7) == 0)
+        if(strncmp(&buffer[5], "Initial", 7) == 0)
         {
             // Unchanged motor speed
-            if (sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
+            if(sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
             {
                 throw ParseError(buffer, "Motor speed");
             }
@@ -1878,42 +1878,76 @@
         {
             // Changed motor speed, format is:
             // <level>%<ignored string>(<speed>[rpm])
-            if (sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
+            if(sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
                         &info->speed) != 2)
             {
                 throw ParseError(buffer, "Motor speed");
             }
         }
     }
-    else if (strncmp(buffer, "MESM", 4) == 0)
+    else if(strncmp(buffer, "MESM", 4) == 0)
         info->measure_state = &buffer[5];
-    else if (strncmp(buffer, "SBPS", 4) == 0)
+    else if(strncmp(buffer, "SBPS", 4) == 0)
     {
-        if (strncmp(&buffer[5], "USB only", 8) == 0 ||
+        if(strncmp(&buffer[5], "USB only", 8) == 0 ||
             strncmp(&buffer[5], "USB Full Speed", 14) == 0)
         {
             // No baud rate for USB-only devices such as the UHG-08LX
             info->baud = 0;
         }
-        else if (sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
+        else if(sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
             throw ParseError(buffer, "Baud rate");
     }
-    else if (strncmp(buffer, "TIME", 4) == 0)
+    else if(strncmp(buffer, "TIME", 4) == 0)
     {
-        if (sscanf(buffer, "TIME:%x", &info->time) != 1)
+        if(sscanf(buffer, "TIME:%x", &info->time) != 1)
             throw ParseError(buffer, "Timestamp");
     }
-    else if (strncmp(buffer, "STAT", 4) == 0)
+    else if(strncmp(buffer, "STAT", 4) == 0)
         info->sensor_diagnostic = &buffer[5];
-    else if (!_ignore_unknowns)
+    else if(!_ignore_unknowns)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::_read_2_byte_range_data(ScanData *data, unsigned int num_steps)
+/// Combines up to three values from an echo buffer into a single value based
+/// on the setting of _multiecho_mode.
+void Sensor::_process_echo_buffer(int const* buffer, int num_echos,
+        uint32_t* dest)
 {
-    if (_verbose)
+    uint32_t sum = 0;
+    switch(_multiecho_mode)
     {
+        case ME_FRONT:
+            *dest = buffer[0];
+            break;
+        case ME_MIDDLE:
+            if(num_echos == 3)
+                *dest = buffer[1];
+            else
+                *dest = buffer[0];
+            break;
+        case ME_REAR:
+            *dest = buffer[num_echos - 1];
+            break;
+        case ME_AVERAGE:
+            for (int ii = 0; ii < num_echos; ii++)
+                sum += buffer[ii];
+            sum /= static_cast<float>(num_echos);
+            *dest = sum;
+            break;
+        case ME_OFF:
+        default:
+            *dest = buffer[0];
+            break;
+    }
+}
+
+
+void Sensor::_read_2_byte_range_data(ScanData* data, unsigned int num_steps)
+{
+    if(_verbose)
+    {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
     }
@@ -1926,45 +1960,45 @@
 
     // 2 byte data is easy since it fits neatly in a 64-byte block
     char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentStep = 0;
+    unsigned int current_step = 0;
     int numBytesInLine = 0;
-    while (true)
+    while(true)
     {
         // Read a line of data
         numBytesInLine = _read_line_with_check(buffer);
         // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
+        if(numBytesInLine == 0)
             break;
         // Process pairs of bytes until we encounter the end of the line
-        for (int ii = 0; ii < numBytesInLine; ii += 2, currentStep++)
+        for (int ii = 0; ii < numBytesInLine; ii += 2, current_step++)
         {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+            if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a data block? Why?
                 throw MisplacedLineFeedError();
             }
-            data->_ranges[currentStep] = decode_2_byte_value(&buffer[ii]);
-            if (data->_ranges[currentStep] < 20)
+            data->_ranges[current_step] = decode_2_byte_value(&buffer[ii]);
+            if(data->_ranges[current_step] < 20)
                 data->_error = true;
         }
         // End of this line. Go around again.
     }
 
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges.\n";
-    if (currentStep != num_steps)
+            current_step << " ranges.\n";
+    if(current_step != num_steps)
         throw DataCountError();
 }
 
 
-void Sensor::_read_3_byte_range_data(ScanData *data, unsigned int num_steps)
+void Sensor::_read_3_byte_range_data(ScanData* data, unsigned int num_steps)
 {
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
-        if (_multiecho_mode != ME_OFF)
+        if(_multiecho_mode != ME_OFF)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Multi-echo mode is set to " <<
@@ -1981,99 +2015,132 @@
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
     char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentStep = 0;
-    int numBytesInLine = 0, splitCount = 0;
-    char splitValue[3];
-    while (true)
+    unsigned int current_step = 0;
+    int numBytesInLine = 0, split_count = 0;
+    char split_value[3];
+    int echo_buffer[3] = {-1, -1, -1};
+    int echo_buf_ind = 0;
+    while(true)
     {
         // Read a line of data
         numBytesInLine = _read_line_with_check(buffer);
         // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
+        if(numBytesInLine == 0)
+        {
+            if(echo_buffer[0] != -1)
+            {
+                // Not the first value, so deal with the previous
+                _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+                        &data->_ranges[current_step]);
+                if(data->_ranges[current_step] > _max_range)
+                {
+                    _err_output << "WARNING: Sensor::" << __func__ <<
+                        "() Value at step " << current_step <<
+                        " beyond maximum range: " <<
+                        data->_ranges[current_step];
+                }
+                else if(data->_ranges[current_step] < 20)
+                    data->_error = true;
+                current_step++;
+            }
             break;
+        }
         // Process triplets of bytes until we encounter or overrun the end of
         // the line
         for (int ii = 0; ii < numBytesInLine;)
         {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+            if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a line? Why?
                 throw MisplacedLineFeedError();
             }
-            if (ii == numBytesInLine - 2)       // Short 1 byte
+            if(split_count == 0)
             {
-                splitValue[0] = buffer[ii];
-                splitValue[1] = buffer[ii + 1];
+                // Start of a value. Decide where to store the next value, and
+                // if the previous is complete.
+                if(buffer[ii] == '&')
+                {
+                    // Next echo
+                    echo_buf_ind++;
+                    ii++;
+                }
+                else if(echo_buffer[0] != -1)
+                {
+                    // Not the first value, so deal with the previous
+                    _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+                            &data->_ranges[current_step]);
+                    if(data->_ranges[current_step] > _max_range)
+                    {
+                        _err_output << "WARNING: Sensor::" << __func__ <<
+                            "() Value at step " << current_step <<
+                            " beyond maximum range: " <<
+                            data->_ranges[current_step];
+                    }
+                    else if(data->_ranges[current_step] < 20)
+                        data->_error = true;
+                    current_step++;
+                    echo_buf_ind = 0;
+                    echo_buffer[0] = -1;
+                }
+            }
+            if(ii == numBytesInLine - 2)       // Short 1 byte
+            {
+                split_value[0] = buffer[ii];
+                split_value[1] = buffer[ii + 1];
                 // Will be reset on the next iteration, after it's used
-                splitCount = 1;
+                split_count = 1;
                 ii += 2;
             }
-            else if (ii == numBytesInLine - 1)  // Short 2 bytes
+            else if(ii == numBytesInLine - 1)  // Short 2 bytes
             {
-                splitValue[0] = buffer[ii];
+                split_value[0] = buffer[ii];
                 // Will be reset on the next iteration, after it's used
-                splitCount = 2;
+                split_count = 2;
                 ii += 1;
             }
             else
             {
-                if (splitCount == 1)
+                if(split_count == 1)
                 {
-                    splitValue[2] = buffer[ii++];
-                    data->_ranges[currentStep] =
-                        decode_3_byte_value(splitValue);
+                    split_value[2] = buffer[ii++];
+                    echo_buffer[echo_buf_ind] =
+                        decode_3_byte_value(split_value);
                 }
-                else if (splitCount == 2)
+                else if(split_count == 2)
                 {
-                    splitValue[1] = buffer[ii++];
-                    splitValue[2] = buffer[ii++];
-                    data->_ranges[currentStep] =
-                        decode_3_byte_value(splitValue);
+                    split_value[1] = buffer[ii++];
+                    split_value[2] = buffer[ii++];
+                    echo_buffer[echo_buf_ind] =
+                        decode_3_byte_value(split_value);
                 }
                 else
                 {
-                    data->_ranges[currentStep] =
+                    echo_buffer[echo_buf_ind] =
                         decode_3_byte_value(&buffer[ii]);
                     ii += 3;
                 }
-                if (data->_ranges[currentStep] > _max_range)
-                {
-                    _err_output << "WARNING: Sensor::" << __func__ <<
-                        "() Value at step " << currentStep <<
-                        " beyond maximum range: " <<
-                        data->_ranges[currentStep] << " (raw bytes: ";
-                    if (splitCount != 0)
-                        _err_output << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")\n";
-                    else
-                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
-                            ")\n";
-                }
-                else if (data->_ranges[currentStep] < 20)
-                    data->_error = true;
-                currentStep++;
-                splitCount = 0;     // Reset this here now that it's been used
+                split_count = 0;     // Reset this here now that it's been used
             }
         }
         // End of this line. Go around again.
     }
 
-    if (_verbose)
+    if(_verbose)
         _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentStep << " ranges.\n";
-    if (currentStep != num_steps)
+            current_step << " ranges.\n";
+    if(current_step != num_steps)
         throw DataCountError();
 }
 
 
-void Sensor::_read_3_byte_range_and_intensity_data(ScanData *data,
+void Sensor::_read_3_byte_range_and_intensity_data(ScanData* data,
         unsigned int num_steps)
 {
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges and intensities.\n";
-        if (_multiecho_mode != ME_OFF)
+        if(_multiecho_mode != ME_OFF)
         {
             _err_output << "Sensor::" << __func__ <<
                 "() Multi-echo mode is set to " <<
@@ -2089,115 +2156,140 @@
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
     char buffer[SCIP2_LINE_LENGTH];
-    unsigned int currentRange = 0, currentIntensity = 0;
-    int numBytesInLine = 0, splitCount = 0;
-    char splitValue[3];
+    unsigned int current_range = 0, current_intensity = 0;
+    int numBytesInLine = 0, split_count = 0;
+    char split_value[3];
     bool nextIsIntensity = false;
-    while (true)
+    int echo_buffer[3] = {-1, -1, -1};
+    int echo_buf_ind = 0;
+    while(true)
     {
         // Read a line of data
         numBytesInLine = _read_line_with_check(buffer);
         // Check if we've reached the end of the data
-        if (numBytesInLine == 0)
+        if(numBytesInLine == 0)
+        {
+            // The last piece should always be an intensity value (if it isn't
+            // then the data count won't add up and an error will be thrown
+            // below anyway).
+            if(echo_buffer[0] != -1)
+            {
+                assert(nextIsIntensity == true);
+                _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
+                        &data->_intensities[current_intensity]);
+                current_intensity++;
+            }
             break;
+        }
         // Process triplets of bytes until we encounter or overrun the end of
         // the line
         for (int ii = 0; ii < numBytesInLine;)
         {
-            if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+            if(buffer[ii] == '\n' || buffer[ii + 1] == '\n')
             {
                 // Line feed in the middle of a line? Why?
                 throw MisplacedLineFeedError();
             }
-            if (ii == numBytesInLine - 2)       // Short 1 byte
+            if(split_count == 0)
             {
-                splitValue[0] = buffer[ii];
-                splitValue[1] = buffer[ii + 1];
+                // Start of a value. Decide where to store the next value, and
+                // if the previous is complete.
+                if(buffer[ii] == '&')
+                {
+                    // Next echo
+                    echo_buf_ind++;
+                    ii++;
+                }
+                else if(echo_buffer[0] != -1)
+                {
+                    // Not the first value, so deal with the previous
+                    uint32_t* dest;
+                    if(nextIsIntensity)
+                        dest = &data->_intensities[current_intensity];
+                    else
+                        dest = &data->_ranges[current_range];
+                    _process_echo_buffer(echo_buffer, echo_buf_ind + 1, dest);
+                    echo_buf_ind = 0;
+                    echo_buffer[0] = -1;
+                    if(data->_ranges[current_range] > _max_range &&
+                            !nextIsIntensity)
+                    {
+                        _err_output << "WARNING: Sensor::" << __func__ <<
+                            "() Value at step " << current_range <<
+                            " beyond maximum range: " <<
+                            data->_ranges[current_range] << " (raw bytes: ";
+                        if(split_count != 0)
+                            _err_output << split_value[0] << split_value[1] <<
+                                split_value[2] << ")\n";
+                        else
+                            _err_output << buffer[0] << buffer[1] << buffer[2] <<
+                                ")\n";
+                    }
+                    else if(data->_ranges[current_range] < 20)
+                        data->_error = true;
+                    if(nextIsIntensity)
+                        current_intensity++;
+                    else
+                        current_range++;
+                    // Alternate between range and intensity values
+                    nextIsIntensity = !nextIsIntensity;
+                }
+            }
+            if(ii == numBytesInLine - 2)       // Short 1 byte
+            {
+                split_value[0] = buffer[ii];
+                split_value[1] = buffer[ii + 1];
                 // Will be reset on the next iteration, after it's used
-                splitCount = 1;
+                split_count = 1;
                 ii += 2;
             }
-            else if (ii == numBytesInLine - 1)  // Short 2 bytes
+            else if(ii == numBytesInLine - 1)  // Short 2 bytes
             {
-                splitValue[0] = buffer[ii];
+                split_value[0] = buffer[ii];
                 // Will be reset on the next iteration, after it's used
-                splitCount = 2;
+                split_count = 2;
                 ii += 1;
             }
             else
             {
-                if (splitCount == 1)
+                if(split_count == 1)
                 {
-                    splitValue[2] = buffer[ii++];
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(splitValue);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(splitValue);
+                    split_value[2] = buffer[ii++];
+                    echo_buffer[echo_buf_ind] =
+                        decode_3_byte_value(split_value);
                 }
-                else if (splitCount == 2)
+                else if(split_count == 2)
                 {
-                    splitValue[1] = buffer[ii++];
-                    splitValue[2] = buffer[ii++];
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(splitValue);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(splitValue);
+                    split_value[1] = buffer[ii++];
+                    split_value[2] = buffer[ii++];
+                    echo_buffer[echo_buf_ind] =
+                        decode_3_byte_value(split_value);
                 }
                 else
                 {
-                    if (nextIsIntensity)
-                        data->_intensities[currentIntensity] =
-                            decode_3_byte_value(&buffer[ii]);
-                    else
-                        data->_ranges[currentRange] =
-                            decode_3_byte_value(&buffer[ii]);
+                    echo_buffer[echo_buf_ind] =
+                        decode_3_byte_value(&buffer[ii]);
                     ii += 3;
                 }
-                if (data->_ranges[currentRange] > _max_range &&
-                        !nextIsIntensity)
-                {
-                    _err_output << "WARNING: Sensor::" << __func__ <<
-                        "() Value at step " << currentRange <<
-                        " beyond maximum range: " <<
-                        data->_ranges[currentRange] << " (raw bytes: ";
-                    if (splitCount != 0)
-                        _err_output << splitValue[0] << splitValue[1] <<
-                            splitValue[2] << ")\n";
-                    else
-                        _err_output << buffer[0] << buffer[1] << buffer[2] <<
-                            ")\n";
-                }
-                else if (data->_ranges[currentRange] < 20)
-                    data->_error = true;
-                if (nextIsIntensity)
-                    currentIntensity++;
-                else
-                    currentRange++;
                 // Reset this here now that it's been used
-                splitCount = 0;
-                // Alternate between range and intensity values
-                nextIsIntensity = !nextIsIntensity;
+                split_count = 0;
             }
         }
         // End of this line. Go around again.
     }
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ << "() Read " <<
-            currentRange << " ranges and " << currentIntensity <<
+            current_range << " ranges and " << current_intensity <<
             " intensities (expected " << num_steps << ").\n";
     }
-    if (currentRange != num_steps || currentIntensity != num_steps)
+    if(current_range != num_steps || current_intensity != num_steps)
         throw DataCountError();
 }
 
 
-int Sensor::_confirm_checksum(const char *buffer, int length,
+int Sensor::_confirm_checksum(char const* buffer, int length,
         int expected_sum)
 {
     int checksum = 0;
@@ -2209,7 +2301,7 @@
     // Add 0x30
     checksum += 0x30;
 
-    if (_verbose)
+    if(_verbose)
     {
         _err_output << "Sensor::" << __func__ <<
             "() Calculated checksum = " << checksum << " (" <<
@@ -2217,7 +2309,7 @@
             expected_sum << " (" << static_cast<char> (expected_sum) <<
             ")\n";
     }
-    if (checksum != expected_sum)
+    if(checksum != expected_sum)
         throw ChecksumError(expected_sum, checksum);
 
     return checksum;

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-04 10:42:44 UTC (rev 506)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-05 01:26:43 UTC (rev 507)
@@ -1,7 +1,7 @@
 /*
  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
  *               http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
+ * Copyright (c) 2008-2010 Geoffrey Biggs
  *
  * hokuyo_aist Hokuyo laser scanner driver.
  *
@@ -50,11 +50,13 @@
 /// The Tough-URG features multiecho detection capability. To use this, set
 /// the sensor to use any mode other than ME_OFF.
 /// The sensor can register up to three echos for a single reading. The
-/// multiecho mode determines how these are combined into a single value. If
-/// ME_FRONT is set, only the closest reading will be used. If ME_MIDDLE is
-/// set, only the middle reading will be used, or the closest reading if there
-/// are only two echos. If ME_REAR is set, the furthest reading will be used.
-/// Setting ME_AVERAGE will use the average of all two or three echos.
+/// multiecho mode determines how these are combined into a single value:
+/// - ME_FRONT: only the closest reading will be used.
+/// - ME_MIDDLE: the middle reading will be used, or the closest
+///   reading if there are only two echos.
+/// - ME_REAR: the furthest reading will be used.
+/// - ME_AVERAGE: the average of all two or three echos will be used.
+/// In all cases, if there is only one echo, then this setting has no effect.
 enum MultiechoMode
 {
     ME_OFF,
@@ -187,7 +189,7 @@
         /** @brief Get various information about the scanner.
 
         Much of the information is not available with the SCIP v1 protocol. */
-        void get_sensor_info(SensorInfo *info);
+        void get_sensor_info(SensorInfo* info);
 
         /** @brief Get the value of the scanner's clock in milliseconds.
 
@@ -216,7 +218,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges(ScanData *data, int start_step = -1,
+        unsigned int get_ranges(ScanData* data, int start_step = -1,
                 int end_step = -1, unsigned int cluster_count = 1);
 
         /** @brief Get the latest scan data from the scanner.
@@ -233,7 +235,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_by_angle(ScanData *data, double start_angle,
+        unsigned int get_ranges_by_angle(ScanData* data, double start_angle,
                 double end_angle, unsigned int cluster_count = 1);
 
         /** @brief Get the latest scan data from the scanner with intensities.
@@ -258,7 +260,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_intensities(ScanData *data,
+        unsigned int get_ranges_intensities(ScanData* data,
                 int start_step = -1, int end_step = -1,
                 unsigned int cluster_count = 1);
 
@@ -276,7 +278,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_intensities_by_angle(ScanData *data,
+        unsigned int get_ranges_intensities_by_angle(ScanData* data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -308,7 +310,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges(ScanData *data, int start_step = -1,
+        unsigned int get_new_ranges(ScanData* data, int start_step = -1,
                 int end_step = -1, unsigned int cluster_count = 1);
 
         /** @brief Get a new scan from the scanner.
@@ -327,7 +329,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_by_angle(ScanData *data,
+        unsigned int get_new_ranges_by_angle(ScanData* data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -358,7 +360,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_intensities(ScanData *data,
+        unsigned int get_new_ranges_intensities(ScanData* data,
                 int start_step = -1, int end_step = -1,
                 unsigned int cluster_count = 1);
 
@@ -378,7 +380,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_intensities_by_angle(ScanData *data,
+        unsigned int get_new_ranges_intensities_by_angle(ScanData* data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -403,7 +405,7 @@
         unsigned int angle_to_step(double angle);
 
     private:
-        flexiport::Port *_port;
+        flexiport::Port* _port;
         std::ostream& _err_output;
 
         uint8_t _scip_version;
@@ -416,25 +418,27 @@
         unsigned int _max_range;
 
         void _clear_read_buffer();
-        int _read_line(char *buffer, int expected_length = -1);
-        int _read_line_with_check(char *buffer, int expected_length = -1,
+        int _read_line(char* buffer, int expected_length = -1);
+        int _read_line_with_check(char* buffer, int expected_length = -1,
                 bool has_semicolon = false);
         void _skip_lines(int count);
-        int _send_command(const char *cmd, const char *param, int param_length,
-                const char *extra_ok);
+        int _send_command(char const* cmd, char const* param, int param_length,
+                char const* extra_ok);
 

@@ Diff output truncated at 100000 characters. @@

This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 510
          http://gearbox.svn.sourceforge.net/gearbox/?rev=510&view=rev
Author:   gbiggs
Date:     2010-08-08 04:31:17 +0000 (Sun, 08 Aug 2010)

Log Message:
-----------
Added time calibration

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h
    gearbox/trunk/src/hokuyo_aist/utils.h

Modified: gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-05 05:15:27 UTC (rev 509)
+++ gearbox/trunk/src/hokuyo_aist/CMakeLists.txt	2010-08-08 04:31:17 UTC (rev 510)
@@ -30,8 +30,19 @@
             add_definitions (-DHOKUYO_AIST_STATIC -DFLEXIPORT_STATIC)
         endif (GBX_DEFAULT_LIB_TYPE STREQUAL SHARED)
     endif (WIN32)
+
+    include (CheckFunctionExists)
+    set (CMAKE_REQUIRED_INCLUDES time.h)
+    set (CMAKE_REQUIRED_LIBRARIES rt)
+    CHECK_FUNCTION_EXISTS (clock_gettime HAVE_CLOCK_GETTIME)
+    set (CMAKE_REQUIRED_INCLUDES)
+    set (CMAKE_REQUIRED_LIBRARIES)
+
     GBX_ADD_LIBRARY (${libName} DEFAULT ${libVersion} ${srcs})
     target_link_libraries (${libName} ${reqLibs})
+    if (HAVE_CLOCK_GETTIME)
+        target_link_libraries (${libName} rt)
+    endif (HAVE_CLOCK_GETTIME)
     GBX_ADD_PKGCONFIG (${libName} ${libDesc} "" reqLibs "" "" ${libVersion})
 
     GBX_ADD_HEADERS (${libName} ${hdrs})

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-05 05:15:27 UTC (rev 509)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-08 04:31:17 UTC (rev 510)
@@ -31,111 +31,88 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 ScanData::ScanData()
-    : _ranges(NULL), _intensities(NULL), _length(0),
-    _error(false), _time(0), _model(MODEL_UNKNOWN)
+    : ranges_(NULL), intensities_(NULL), ranges_length_(0),
+    intensities_length_(0), error_(false), laser_time_(0), system_time_(0),
+    model_(MODEL_UNKNOWN), buffers_provided_(false)
 {
 }
 
 
-ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
-        unsigned int time, LaserModel model)
-    : _error(error), _time(time), _model(model)
+ScanData::ScanData(uint32_t* const ranges_buffer,
+        unsigned int ranges_length, uint32_t* const intensities_buffer,
+        unsigned int intensities_length)
+    : ranges_(ranges_buffer), intensities_(intensities_buffer),
+    ranges_length_(ranges_length), intensities_length_(intensities_length),
+    error_(false), laser_time_(0), system_time_(0), model_(MODEL_UNKNOWN),
+    buffers_provided_(true)
 {
-    _length = length;
-    if(_length == 0)
-    {
-        _ranges = NULL;
-        _intensities = NULL;
-    }
+}
+
+
+ScanData::ScanData(ScanData const& rhs)
+{
+    ranges_length_ = rhs.ranges_length();
+    intensities_length_ = rhs.intensities_length();
+    if(ranges_length_ == 0)
+        ranges_ = NULL;
     else
     {
         try
         {
-            _ranges = new uint32_t[_length];
+            ranges_ = new uint32_t[ranges_length_];
         }
         catch(std::bad_alloc& e)
         {
-            _length = 0;
+            ranges_length_ = 0;
             throw;
         }
-        memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
-        _intensities = NULL;    // No intensity data to copy
+        memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * ranges_length_);
     }
-}
-
-
-ScanData::ScanData(uint32_t* ranges, uint32_t* intensities,
-        unsigned int length, bool error, unsigned int time,
-        LaserModel model)
-    : _error(error), _time(time), _model(model)
-{
-    _length = length;
-    if(_length == 0)
-    {
-        _ranges = NULL;
-        _intensities = NULL;
-    }
+    if(intensities_length_ == 0)
+        intensities_ = NULL;
     else
     {
         try
         {
-            _ranges = new uint32_t[_length];
+            intensities_ = new uint32_t[intensities_length_];
         }
         catch(std::bad_alloc& e)
         {
-            _length = 0;
+            intensities_length_ = 0;
             throw;
         }
-        memcpy(_ranges, ranges, sizeof(uint32_t) * _length);
-
-        _intensities = new uint32_t[_length];
-        memcpy(_intensities, intensities, sizeof(uint32_t) * _length);
+        memcpy(intensities_, rhs.ranges(),
+                sizeof(uint32_t) * intensities_length_);
     }
+    error_ = rhs.get_error_status();
+    laser_time_ = rhs.laser_time_stamp();
+    system_time_ = rhs.system_time_stamp();
+    model_ = rhs.model();
+    buffers_provided_ = rhs.buffers_provided();
 }
 
 
-ScanData::ScanData(ScanData const& rhs)
+ScanData::~ScanData()
 {
-    _length = rhs.length();
-    if(_length == 0)
-        _ranges = NULL;
-    else
+    if(!buffers_provided_)
     {
-        try
+        if (ranges_ != NULL)
         {
-            _ranges = new uint32_t[_length];
+            delete[] ranges_;
+            ranges_ = NULL;
         }
-        catch(std::bad_alloc& e)
+        if (intensities_ != NULL)
         {
-            _length = 0;
-            throw;
+            delete[] intensities_;
+            intensities_ = NULL;
         }
-        memcpy(_ranges, rhs.ranges(), sizeof(uint32_t) * _length);
-
-        if(rhs.intensities() != NULL)
-        {
-            _intensities = new uint32_t[_length];
-            memcpy(_intensities, rhs.intensities(),
-                    sizeof(uint32_t) * _length);
-        }
     }
-    _error = rhs.get_error_status();
-    _time = rhs.time_stamp();
-    _model = rhs.model();
 }
 
 
-ScanData::~ScanData()
-{
-    if(_ranges != NULL)
-        delete[] _ranges;
-}
-
-
 std::string ScanData::error_code_to_string(uint32_t error_code)
 {
-    if(_model == MODEL_UTM30LX)
+    if(model_ == MODEL_UTM30LX)
     {
         switch(error_code)
         {
@@ -212,80 +189,94 @@
 
 ScanData& ScanData::operator=(ScanData const& rhs)
 {
-    if(rhs.length() == 0)
+    unsigned int rhslength = rhs.ranges_length();
+    if(rhslength == 0)
     {
-        _length = 0;
-        if(_ranges != NULL)
-            delete[] _ranges;
-        _ranges = NULL;
-        if(_intensities != NULL)
-            delete[] _intensities;
-        _intensities = NULL;
-        _error = rhs.get_error_status();
-        _time = rhs.time_stamp();
-        _model = rhs.model();
+        ranges_ = NULL;
+        ranges_length_ = 0;
     }
     else
     {
-        unsigned int rhslength = rhs.length();
-        uint32_t* newData;
-        if(rhslength != _length)
+        if(rhslength != ranges_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)
+            if(!buffers_provided_ && ranges_ != 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;
+                // Just copy
+                memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * rhslength);
             }
+            else
+            {
+                // Copy the data into a temporary variable pointing to new space
+                // (prevents dangling pointers on allocation error and prevents
+                // self-assignment making a mess).
+                uint32_t* new_data = new uint32_t[rhslength];
+                memcpy(new_data, rhs.ranges(), sizeof(uint32_t) * rhslength);
+                if(ranges_ != NULL)
+                    delete[] ranges_;
+                ranges_ = new_data;
+                ranges_length_ = rhs.ranges_length();
+            }
         }
         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) * rhslength);
         }
+    }
 
-        _error = rhs.get_error_status();
-        _time = rhs.time_stamp();
-        _model = rhs.model();
+    rhslength = rhs.intensities_length();
+    if(rhslength == 0)
+    {
+        intensities_ = NULL;
+        intensities_length_ = 0;
     }
+    else
+    {
+        if(rhslength != intensities_length_)
+        {
+            if(!buffers_provided_ && intensities_ != NULL)
+            {
+                // Just copy
+                memcpy(intensities_, rhs.intensities(),
+                        sizeof(uint32_t) * rhslength);
+            }
+            else
+            {
+                // Copy the data into a temporary variable pointing to new
+                // space (prevents dangling pointers on allocation error and
+                // prevents self-assignment making a mess).
+                uint32_t* new_data = new uint32_t[rhslength];
+                memcpy(new_data, rhs.intensities(),
+                        sizeof(uint32_t) * rhslength);
+                if(intensities_ != NULL)
+                    delete[] intensities_;
+                intensities_ = new_data;
+                intensities_length_ = rhs.intensities_length();
+            }
+        }
+        else
+        {
+            // If lengths are the same, no need to reallocate
+            memcpy(intensities_, rhs.intensities(),
+                    sizeof(uint32_t) * rhslength);
+        }
+    }
 
+    error_ = rhs.get_error_status();
+    laser_time_ = rhs.laser_time_stamp();
+    system_time_ = rhs.system_time_stamp();
+    model_ = rhs.model();
+    buffers_provided_ = rhs.buffers_provided();
+
     return *this;
 }
 
 
 uint32_t ScanData::operator[](unsigned int index)
 {
-    if(index >= _length)
+    if(index >= ranges_length_)
         throw IndexError();
-    return _ranges[index];
+    return ranges_[index];
 }
 
 
@@ -293,30 +284,36 @@
 {
     std::stringstream ss;
 
-    ss << _length << " readings from model ";
-    ss << model_to_string(_model) << ":\n";
-    for (unsigned int ii = 0; ii < _length; ii++)
-        ss << _ranges[ii] << "\t";
-    if(_intensities != NULL)
+    if(ranges_ != NULL)
     {
-        ss << std::endl << _length << " intensities:\n";
-        for (unsigned int ii = 0; ii < _length; ii++)
-            ss << _intensities[ii] << "\t";
+        ss << ranges_length_ << " ranges from model ";
+        ss << model_to_string(model_) << ":\n";
+        for(unsigned int ii(0); ii < ranges_length_; ii++)
+            ss << ranges_[ii] << '\t';
+        ss << '\n';
     }
-    ss << '\n';
-    if(_error)
+    if(intensities_ != NULL)
     {
+        ss << intensities_length_ << " intensities from model ";
+        ss << model_to_string(model_) << ":\n";
+        for(unsigned int ii(0); ii < intensities_length_; ii++)
+            ss << intensities_[ii] << '\t';
+        ss << '\n';
+    }
+
+    if(error_)
+    {
         ss << "Detected data errors:\n";
-        for (unsigned int ii = 0; ii < _length; ii++)
+        for(unsigned int ii = 0; ii < ranges_length_; ii++)
         {
-            if(_ranges[ii] < 20)
-                ss << ii << ": " <<
-                    error_code_to_string(_ranges[ii]) << '\n';
+            if(ranges_[ii] < 20)
+                ss << ii << ": " << error_code_to_string(ranges_[ii]) << '\n';
         }
     }
     else
         ss << "No data errors.\n";
-    ss << "Time stamp: " << _time << '\n';
+    ss << "Laser time stamp: " << laser_time_ << '\n';
+    ss << "System time stamp: " << system_time_ << '\n';
 
     return ss.str();
 }
@@ -324,72 +321,126 @@
 
 void ScanData::clean_up()
 {
-    if(_ranges != NULL)
-        delete[] _ranges;
-    _ranges = NULL;
-    if(_intensities != NULL)
-        delete[] _intensities;
-    _intensities = NULL;
-    _length = 0;
-    _error = false;
-    _time = 0;
+    if(!buffers_provided_)
+    {
+        if(ranges_ != NULL)
+            delete[] ranges_;
+        ranges_ = NULL;
+        if(intensities_ != NULL)
+            delete[] intensities_;
+        intensities_ = NULL;
+    }
+    ranges_length_ = 0;
+    intensities_length_ = 0;
+    error_ = false;
+    laser_time_ = 0;
+    system_time_ = 0;
 }
 
 
 void ScanData::allocate_data(unsigned int length, bool include_intensities)
 {
+    // If buffers have been provided, automatic allocation is off.
+    if(buffers_provided_)
+        return;
+
     // If no data yet, allocate new
-    if(_ranges == NULL)
+    if(ranges_ == NULL)
     {
         try
         {
-            _ranges = new uint32_t[length];
+            ranges_ = new uint32_t[length];
         }
         catch(std::bad_alloc& e)
         {
-            _length = 0;
+            ranges_length_ = 0;
             throw;
         }
-        _length = length;
+        ranges_length_ = length;
     }
     // If there is data, reallocate only if the length is different
-    else if(length != _length)
+    else if(length != ranges_length_)
     {
-        delete[] _ranges;
+        delete[] ranges_;
         try
         {
-            _ranges = new uint32_t[length];
+            ranges_ = new uint32_t[length];
         }
         catch(std::bad_alloc& e)
         {
-            _length = 0;
+            ranges_length_ = 0;
             throw;
         }
-        _length = length;
+        ranges_length_ = length;
     }
     // Else data is already allocated to the right length, so do nothing
 
     if(include_intensities)
     {
         // If no data yet, allocate new
-        if(_intensities == NULL)
+        if(intensities_ == NULL)
         {
-            _intensities = new uint32_t[length];
+            try
+            {
+                intensities_ = new uint32_t[length];
+            }
+            catch(std::bad_alloc& e)
+            {
+                intensities_length_ = 0;
+                throw;
+            }
+            intensities_length_ = length;
         }
         // If there is data, reallocate only if the length is different
-        else if(length != _length)
+        else if(length != intensities_length_)
         {
-            delete[] _intensities;
-            _intensities = new uint32_t[length];
+            delete[] intensities_;
+            try
+            {
+                intensities_ = new uint32_t[length];
+            }
+            catch(std::bad_alloc& e)
+            {
+                intensities_length_ = 0;
+                throw;
+            }
+            intensities_length_ = length;
         }
         // Else data is already allocated to the right length, so do nothing
     }
-    else if(_intensities != NULL)
+    else if(intensities_ != NULL)
     {
         // If not told to allocate space for intensity data and it exists,
         // remove it
-        delete[] _intensities;
-        _intensities = NULL;
+        delete[] intensities_;
+        intensities_ = NULL;
+        intensities_length_ = 0;
     }
 }
 
+
+void ScanData::write_range_(unsigned int index, uint32_t value)
+{
+    if(ranges_ != NULL)
+    {
+        if(index >= ranges_length_)
+            throw IndexError();
+        ranges_[index] = value;
+        if(ranges_[index] < 20)
+            error_ = true;
+    }
+}
+
+
+void ScanData::write_intensity_(unsigned int index, uint32_t value)
+{
+    if(intensities_ != NULL)
+    {
+        if(index >= intensities_length_)
+            throw IndexError();
+        intensities_[index] = value;
+        if(intensities_[index] < 20)
+            error_ = true;
+    }
+}
+

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-05 05:15:27 UTC (rev 509)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-08 04:31:17 UTC (rev 510)
@@ -57,14 +57,26 @@
         /// This constructor creates an empty ScanData with no data currently
         /// allocated.
         ScanData();
-        /// This constructor performs a deep copy of existing range data.
-        ScanData(uint32_t* ranges, unsigned int length, bool error,
-                unsigned int time, LaserModel model);
-        /// This constructor performs a deep copy of existing range and
-        /// intensity data.
-        ScanData(uint32_t* ranges, uint32_t* intensities,
-                unsigned int length, bool error, unsigned int time,
-                LaserModel model);
+        /// This constructor uses a provided data buffer rather than allocating
+        /// automatically.
+        ///
+        /// If the intensity pointer is NULL, no data will be provided of that
+        /// type.
+        ///
+        /// @param ranges_buffer A pointer to a data area to store range data
+        /// in. It is the caller's responsibility to ensure that it is big
+        /// enough.
+        /// @param ranges_length The size of the ranges buffer. Used only for
+        /// copy constructor and similar.
+        /// @param intensities_buffer A pointer to a data area to store
+        /// intensity data in. It is the caller's responsibility to ensure that
+        /// it is big enough.
+        /// @param intensities_length The size of the intensities buffer. Used
+        /// only for copy constructor and similar.
+        ScanData(uint32_t* const ranges_buffer,
+                unsigned int ranges_length,
+                uint32_t* const intensities_buffer=NULL,
+                unsigned int intensities_length=0);
         /// This copy constructor performs a deep copy of present data.
         ScanData(ScanData const& rhs);
         ~ScanData();
@@ -75,28 +87,43 @@
         data to see a probable cause for the error. Most of the time, it will
         just be an out-of-range reading. */
         const uint32_t* ranges() const
-            { return _ranges; }
+            { return ranges_; }
         /// @brief Return a pointer to an array of intensity readings.
         const uint32_t* intensities() const
-            { return _intensities; }
-        /// @brief Get the number of samples in the data.
-        unsigned int length() const { return _length; }
+            { return intensities_; }
+        /// @brief Get the number of range samples in the data.
+        unsigned int ranges_length() const { return ranges_length_; }
+        /// @brief Get the number of intensity samples in the data.
+        unsigned int intensities_length() const { return intensities_length_; }
         /** @brief Indicates if one or more steps had an error.
 
         A step's value will be less than 20 if it had an error. Use @ref
         error_code_to_string to get a textual representation of the error. */
-        bool get_error_status() const { return _error; }
+        bool get_error_status() const { return error_; }
         /// @brief Return a string representing the error for the given error
         /// code.
         std::string error_code_to_string(uint32_t error_code);
-        /** @brief Get the time stamp of the data in milliseconds
+        /** @brief Get the raw time stamp of the data in milliseconds.
 
         This value is only available using SCIP version 2). */
-        unsigned int time_stamp() const { return _time; }
+        unsigned int laser_time_stamp() const { return laser_time_; }
+        /** @brief Get the system time stamp of the data in milliseconds.
+
+        This value is only available using SCIP version 2). */
+        unsigned int system_time_stamp() const { return system_time_; }
         /// Get the model of the laser that produced this scan.
-        LaserModel model() const { return _model; }
+        LaserModel model() const { return model_; }
+        /// Check if the buffers are being provided instead of automatic.
+        bool buffers_provided() const { return buffers_provided_; }
 
         /// @brief Assignment operator.
+        ///
+        /// If the rhs has provided buffers, the lhs will not receive the same
+        /// buffers. Instead, it will copy the data into its own buffers.
+        /// If the lhs has provided buffers, it is the caller's responsibility
+        /// to ensure they will be big enough to receive the data from the rhs,
+        /// except in the case of NULL buffers (no data will be copied for NULL
+        /// buffers).
         ScanData& operator=(ScanData const& rhs);
         /** @brief Subscript operator.
 
@@ -110,15 +137,20 @@
         void clean_up();
 
     protected:
-        uint32_t* _ranges;
-        uint32_t* _intensities;
-        unsigned int _length;
-        bool _error;
-        unsigned int _time;
-        LaserModel _model;
+        uint32_t* ranges_;
+        uint32_t* intensities_;
+        unsigned int ranges_length_;
+        unsigned int intensities_length_;
+        bool error_;
+        unsigned int laser_time_;
+        unsigned int system_time_;
+        LaserModel model_;
+        bool buffers_provided_;
 
         void allocate_data(unsigned int length,
                 bool include_intensities = false);
+        void write_range_(unsigned int index, uint32_t value);
+        void write_intensity_(unsigned int index, uint32_t value);
 }; // class ScanData
 
 } // namespace hokuyo_aist

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-05 05:15:27 UTC (rev 509)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-08 04:31:17 UTC (rev 510)
@@ -29,14 +29,17 @@
 
 #include <cassert>
 #include <cstring>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <errno.h>
-#include <math.h>
+#include <cstdarg>
+#include <cstdlib>
+#include <cstdio>
+#include <cerrno>
+#include <cmath>
+#include <unistd.h>
 #include <sstream>
 #include <iostream>
 #include <iomanip>
+#include <ctime>
+#include <fstream>
 
 #if defined(WIN32)
     #define __func__    __FUNCTION__
@@ -271,93 +274,94 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 Sensor::Sensor()
-    : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
-    _enable_checksum_workaround(false), _ignore_unknowns(false),
-    _multiecho_mode(ME_OFF), _min_angle(0.0), _max_angle(0.0),
-    _resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
+    : port_(NULL), err_output_(std::cerr), scip_version_(2), verbose_(false),
+    enable_checksum_workaround_(false), ignore_unknowns_(false),
+    multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0),
+    resolution_(0.0), first_step_(0), last_step_(0), front_step_(0),
+    max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0)
 {
 }
 
 
 Sensor::Sensor(std::ostream& err_output)
-    : _port(NULL), _err_output(err_output), _scip_version(2),
-    _verbose(false), _enable_checksum_workaround(false),
-    _ignore_unknowns(false), _multiecho_mode(ME_OFF), _min_angle(0.0),
-    _max_angle(0.0), _resolution(0.0), _first_step(0), _last_step(0),
-    _front_step(0)
+    : port_(NULL), err_output_(err_output), scip_version_(2), verbose_(false),
+    enable_checksum_workaround_(false), ignore_unknowns_(false),
+    multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0),
+    resolution_(0.0), first_step_(0), last_step_(0), front_step_(0),
+    max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0)
 {
 }
 
 
 Sensor::~Sensor()
 {
-    if(_port != NULL)
-        delete _port;
+    if(port_ != NULL)
+     delete port_;
 }
 
 
 void Sensor::open(std::string port_options)
 {
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
+        err_output_ << "Sensor::" << __func__ <<
             "() Creating and opening port using options: " <<
             port_options << '\n';
     }
-    _port = flexiport::CreatePort(port_options);
-    _port->Open();
+    port_ = flexiport::CreatePort(port_options);
+    port_->Open();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection.\n";
-        _err_output << _port->GetStatus();
+        err_output_ << "Sensor::" << __func__ << "() Connected using " <<
+            port_->GetPortType() << " connection.\n";
+        err_output_ << port_->GetStatus();
     }
-    _port->Flush();
+    port_->Flush();
 
     // Figure out the SCIP version currently in use and switch to a higher one
     // if possible
-    _get_and_set_scip_version();
+    get_and_set_scip_version_();
     // Get some values we need for providing default ranges
-    _get_defaults();
+    get_defaults_();
 }
 
 
 unsigned int Sensor::open_with_probing(std::string port_options)
 {
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
+        err_output_ << "Sensor::" << __func__ <<
             "() Creating and opening port using options: " << port_options <<
             '\n';
     }
-    _port = flexiport::CreatePort(port_options);
-    _port->Open();
+    port_ = flexiport::CreatePort(port_options);
+    port_->Open();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Connected using " <<
-            _port->GetPortType() << " connection.\n";
-        _err_output << _port->GetStatus();
+        err_output_ << "Sensor::" << __func__ << "() Connected using " <<
+            port_->GetPortType() << " connection.\n";
+        err_output_ << port_->GetStatus();
     }
-    _port->Flush();
+    port_->Flush();
 
     try
     {
         // Figure out the SCIP version currently in use and switch to a higher
         // one if possible
-        _get_and_set_scip_version();
+        get_and_set_scip_version_();
         // Get some values we need for providing default ranges
-        _get_defaults();
+        get_defaults_();
     }
     catch(BaseError)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Failed to connect at the default baud rate.\n";
         }
-        if(_port->GetPortType() == "serial")
+        if(port_->GetPortType() == "serial")
         {
             // Failed at the default baud rate, so try again at the other
             // rates. Note that a baud rate of 750000 or 250000 doesn't appear
@@ -366,16 +370,16 @@
             unsigned int const numBauds(5);
             for (unsigned int ii = 0; ii < numBauds; ii++)
             {
-                reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(bauds[ii]);
+                reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(bauds[ii]);
                 try
                 {
-                    _get_and_set_scip_version();
-                    _get_defaults();
+                    get_and_set_scip_version_();
+                    get_defaults_();
                     // If the above two functions succeed, break out of the
                     // loop and be happy
-                    if(_verbose)
+                    if(verbose_)
                     {
-                        _err_output << "Sensor::" << __func__ <<
+                        err_output_ << "Sensor::" << __func__ <<
                             "() Connected at " << bauds[ii] << '\n';
                     }
                     return bauds[ii];
@@ -385,9 +389,9 @@
                     if(ii == numBauds - 1)
                     {
                         // Last baud rate, give up and rethrow
-                        if(_verbose)
+                        if(verbose_)
                         {
-                            _err_output << "Sensor::" << __func__ <<
+                            err_output_ << "Sensor::" << __func__ <<
                                 "() Failed to connect at any baud rate.\n";
                         }
                         throw;
@@ -398,17 +402,17 @@
         }
         else
         {
-            if(_verbose)
+            if(verbose_)
             {
-                _err_output << "Sensor::" << __func__ <<
+                err_output_ << "Sensor::" << __func__ <<
                     "() Port is not serial, cannot probe.\n";
             }
             throw;
         }
     }
 
-    if(_port->GetPortType() == "serial")
-        return reinterpret_cast<flexiport::SerialPort*>(_port)->GetBaudRate();
+    if(port_->GetPortType() == "serial")
+        return reinterpret_cast<flexiport::SerialPort*>(port_)->GetBaudRate();
     else
         return 0;
 }
@@ -416,60 +420,60 @@
 
 void Sensor::close()
 {
-    if(!_port)
+    if(!port_)
         throw CloseError();
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ << "() Closing connection.\n";
-    delete _port;
-    _port = NULL;
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ << "() Closing connection.\n";
+    delete port_;
+    port_ = NULL;
 }
 
 
 bool Sensor::is_open() const
 {
-    if(_port != NULL)
-        return _port->IsOpen();
+    if(port_ != NULL)
+        return port_->IsOpen();
     return false;
 }
 
 
 void Sensor::set_power(bool on)
 {
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
     {
         if(on)
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            _send_command("L", "1", 1, NULL);
+            send_command_("L", "1", 1, NULL);
         }
         else
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            _send_command("L", "0", 1, NULL);
+            send_command_("L", "0", 1, NULL);
         }
-        _skip_lines(1);
+        skip_lines_(1);
     }
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
         if(on)
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            _send_command("BM", NULL, 0, "02");
+            send_command_("BM", NULL, 0, "02");
         }
         else
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            _send_command("QT", NULL, 0, "02");
+            send_command_("QT", NULL, 0, "02");
         }
-        _skip_lines(1);
+        skip_lines_(1);
     }
     else
         throw UnknownScipVersionError();
@@ -480,7 +484,7 @@
 // set to the same baud.
 void Sensor::set_baud(unsigned int baud)
 {
-    if(_port->GetPortType() != "serial")
+    if(port_->GetPortType() != "serial")
         throw NotSerialError();
 
     char newBaud[13];
@@ -493,21 +497,21 @@
     }
     number_to_string(baud, newBaud, 6);
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
     {
         // Send the command to change baud rate
-        _send_command("S", newBaud, 13, NULL);
-        _skip_lines(1);
+        send_command_("S", newBaud, 13, NULL);
+        skip_lines_(1);
         // Change the port's baud rate
-        reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
+        reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud);
     }
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
         // Send the command to change baud rate
-        _send_command("SS", newBaud, 6, "03");
-        _skip_lines(1);
+        send_command_("SS", newBaud, 6, "03");
+        skip_lines_(1);
         // Change the port's baud rate
-        reinterpret_cast<flexiport::SerialPort*>(_port)->SetBaudRate(baud);
+        reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud);
     }
     else
         throw UnknownScipVersionError();
@@ -519,7 +523,7 @@
 {
     // Command is "$IP"(3) + IP(15) + ' ' + subnet(15) + ' ' + gateway(15),
     // +1 for the null byte, but split two bytes off the front for sending.
-    // Hijack the _send_command function, treating the rest of the command as
+    // Hijack the send_command_ function, treating the rest of the command as
     // parameters.
     char const command[3] = "$I";
     std::stringstream params;
@@ -531,14 +535,14 @@
         '.' << subnet.fourth << ' ';
     params << gateway.first << '.' << gateway.second << '.' << gateway.third <<
         '.' << gateway.fourth;
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
+        err_output_ << "Sensor::" << __func__ <<
             "() Setting IP information to $I" << params.str() << '\n';
     }
-    int status = _send_command(&command[0], params.str().c_str(), 48, NULL);
+    int status = send_command_(&command[0], params.str().c_str(), 48, NULL);
     // Skip the extra line feed
-    _skip_lines(1);
+    skip_lines_(1);
     if(status != 0)
         throw SetIPError();
 }
@@ -546,17 +550,17 @@
 
 void Sensor::reset()
 {
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(7);
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
         }
-        _send_command("RS", NULL, 0, NULL);
-        _skip_lines(1);
+        send_command_("RS", NULL, 0, NULL);
+        skip_lines_(1);
     }
     else
         throw UnknownScipVersionError();
@@ -565,15 +569,15 @@
 
 void Sensor::semi_reset()
 {
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(35);
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
-        if(_verbose)
-            _err_output << "Sensor::" << __func__ <<
+        if(verbose_)
+            err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
-        _send_command("RS", NULL, 0, NULL);
-        _skip_lines(1);
+        send_command_("RS", NULL, 0, NULL);
+        skip_lines_(1);
     }
     else
         throw UnknownScipVersionError();
@@ -582,9 +586,9 @@
 
 void Sensor::set_motor_speed(unsigned int speed)
 {
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(8);
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
         // Sanity check the value
         if(speed > 10 && speed != 99)
@@ -592,9 +596,9 @@
         char buffer[3];
         if(speed == 0)
         {
-            if(_verbose)
+            if(verbose_)
             {
-                _err_output << "Sensor::" << __func__ <<
+                err_output_ << "Sensor::" << __func__ <<
                     "() Reseting motor speed to default.\n";
             }
             buffer[0] = '0';
@@ -603,9 +607,9 @@
         }
         else if(speed == 99)
         {
-            if(_verbose)
+            if(verbose_)
             {
-                _err_output << "Sensor::" << __func__ <<
+                err_output_ << "Sensor::" << __func__ <<
                     "() Reseting motor speed to default.\n";
             }
             buffer[0] = '9';
@@ -614,15 +618,15 @@
         }
         else
         {
-            if(_verbose)
+            if(verbose_)
             {
-                _err_output << "Sensor::" << __func__ <<
+                err_output_ << "Sensor::" << __func__ <<
                     "() Setting motor speed to ratio " << speed << '\n';
             }
             number_to_string(speed, buffer, 2);
         }
-        _send_command("CR", buffer, 2, "03");
-        _skip_lines(1);
+        send_command_("CR", buffer, 2, "03");
+        skip_lines_(1);
     }
     else
         throw UnknownScipVersionError();
@@ -631,27 +635,27 @@
 
 void Sensor::set_high_sensitivity(bool on)
 {
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(10);
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
         if(on)
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Switching to high sensitivity.\n";
-            _send_command("HS", "1", 1, "02");
+            send_command_("HS", "1", 1, "02");
         }
         else
         {
-            if(_verbose)
+            if(verbose_)
             {
-                _err_output << "Sensor::" << __func__ <<
+                err_output_ << "Sensor::" << __func__ <<
                     "() Switching to normal sensitivity.\n";
             }
-            _send_command("HS", "0", 1, "02");
+            send_command_("HS", "0", 1, "02");
         }
-        _skip_lines(1);
+        skip_lines_(1);
     }
     else
         throw UnknownScipVersionError();
@@ -663,44 +667,44 @@
     if(info == NULL)
         throw NoDestinationError();
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Getting sensor information using SCIP version 1.\n";
         }
 
-        info->set_defaults();
+        info->set_defaults_();
 
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
 
-        _send_command("V", NULL, 0, NULL);
+        send_command_("V", NULL, 0, NULL);
         // Get the vendor info line
-        _read_line(buffer);
+        read_line_(buffer);
         info->vendor = &buffer[5]; // Chop off the "VEND:" tag
         // Get the product info line
-        _read_line(buffer);
+        read_line_(buffer);
         info->product = &buffer[5];
         // Only the URG-04LX supports SCIP1
-        _model = MODEL_URG04LX;
+        model_ = MODEL_URG04LX;
         // Get the firmware line
-        _read_line(buffer);
+        read_line_(buffer);
         info->firmware = &buffer[5];
         // Get the protocol version line
-        _read_line(buffer);
+        read_line_(buffer);
         info->protocol = &buffer[5];
         // Get the serial number
-        _read_line(buffer);
+        read_line_(buffer);
         info->serial = &buffer[5];
         // Get either the status line or the end of message
-        _read_line(buffer);
+        read_line_(buffer);
         if(buffer[0] != '\0')
         {
             // Got a status line
             info->sensor_diagnostic = &buffer[5];
-            _skip_lines(1);
+            skip_lines_(1);
         }
 
         // Check the firmware version major number. If it's >=3 there is
@@ -710,8 +714,8 @@
         // length of 64 bytes.
         if(atoi(info->firmware.c_str()) >= 3)
         {
-            if(_verbose)
-                _err_output << "SCIP1 Firmware line for parsing: " <<
+            if(verbose_)
+                err_output_ << "SCIP1 Firmware line for parsing: " <<
                     info->firmware << '\n';
             // Now the fun part: parsing the line. It would be nice if we could
             // use the POSIX regex functions, but since MS doesn't believe in
@@ -722,7 +726,7 @@
             {
                 // No bracket? Crud. Fail and use the hard-coded values from
                 // the manual.
-                info->calculate_values();
+                info->calculate_values_();
             }
             // Now put it through sscanf and hope...
             int aperture;
@@ -734,13 +738,13 @@
             {
                 // Didn't get enough values out, assume unknown format and fall
                 // back on the defaults
-                info->set_defaults();
-                info->calculate_values();
-                if(_verbose)
+                info->set_defaults_();
+                info->calculate_values_();
+                if(verbose_)
                 {
-                    _err_output << "Retrieved sensor info (hard-coded, not "
+                    err_output_ << "Retrieved sensor info (hard-coded, not "
                         "enough values):\n";
-                    _err_output << info->as_string();
+                    err_output_ << info->as_string();
                 }
             }
             else
@@ -758,10 +762,10 @@
                 info->max_angle = (info->last_step - info->front_step) *
                     info->resolution;
 
-                if(_verbose)
+                if(verbose_)
                 {
-                    _err_output << "Retrieved sensor info (from FIRM line):\n";
-                    _err_output << info->as_string();
+                    err_output_ << "Retrieved sensor info (from FIRM line):\n";
+                    err_output_ << info->as_string();
                 }
             }
         }
@@ -769,50 +773,50 @@
         {
             // We're stuck with hard-coded defaults from the manual (already
             // set earlier).
-            info->calculate_values();
-            if(_verbose)
+            info->calculate_values_();
+            if(verbose_)
             {
-                _err_output << "Retrieved sensor info (hard-coded):\n";
-                _err_output << info->as_string();
+                err_output_ << "Retrieved sensor info (hard-coded):\n";
+                err_output_ << info->as_string();
             }
         }
     }
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Getting sensor information using SCIP version 2.\n";
         }
 
-        info->set_defaults();
+        info->set_defaults_();
 
         char buffer[SCIP2_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
 
         // We need to send three commands to get all the info we want: VV, PP
         // and II
-        _send_command("VV", NULL, 0, NULL);
-        while(_read_line_with_check(buffer, -1, true) != 0)
-            _process_vv_line(buffer, info);
+        send_command_("VV", NULL, 0, NULL);
+        while(read_line_with_check_(buffer, -1, true) != 0)
+            process_vv_line_(buffer, info);
 
         // Next up, PP
-        _send_command("PP", NULL, 0, NULL);
-        while(_read_line_with_check(buffer, -1, true) != 0)
-            _process_pp_line(buffer, info);
+        send_command_("PP", NULL, 0, NULL);
+        while(read_line_with_check_(buffer, -1, true) != 0)
+            process_pp_line_(buffer, info);
 
         // Command II: Revenge of the Commands.
-        _send_command("II", NULL, 0, NULL);
-        while(_read_line_with_check(buffer, -1, true) != 0)
-            _process_ii_line(buffer, info);
+        send_command_("II", NULL, 0, NULL);
+        while(read_line_with_check_(buffer, -1, true) != 0)
+            process_ii_line_(buffer, info);
 
-        _enable_checksum_workaround = false;
+        enable_checksum_workaround_ = false;
 
-        info->calculate_values();
-        if(_verbose)
+        info->calculate_values_();
+        if(verbose_)
         {
-            _err_output << "Retrieved sensor info:\n";
-            _err_output << info->as_string();
+            err_output_ << "Retrieved sensor info:\n";
+            err_output_ << info->as_string();
         }
     }
     else
@@ -822,19 +826,25 @@
 
 unsigned int Sensor::get_time()
 {
-    if(_scip_version == 1)
+    return offset_timestamp_(wrap_timestamp_(get_raw_time()));
+}
+
+
+unsigned int Sensor::get_raw_time()
+{
+    if(scip_version_ == 1)
         throw UnsupportedError(12);
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
-        if(_verbose)
-            _err_output << "Sensor::" << __func__ <<
+        if(verbose_)
+            err_output_ << "Sensor::" << __func__ <<
                 "() Retrieving time from laser.\n";
-        _send_command("TM", "0", 1, NULL);
-        _send_command("TM", "1", 1, NULL);
+        send_command_("TM", "0", 1, NULL);
+        send_command_("TM", "1", 1, NULL);
         char buffer[7];
-        _read_line_with_check(buffer, 6);
-        _send_command("TM", "2", 1, NULL);
-        _skip_lines(1);
+        read_line_with_check_(buffer, 6);
+        send_command_("TM", "2", 1, NULL);
+        skip_lines_(1);
         // We need to decode the time value that's in the buffer
         return decode_4_byte_value(buffer);
     }
@@ -845,6 +855,134 @@
 }
 
 
+unsigned int Sensor::calibrate_time(unsigned int samples)
+{
+    enter_timing_mode_();
+
+    std::ofstream df("times.txt", std::ofstream::app);
+
+    // Measure the latency with 1000 plain TM commands
+    std::vector<unsigned long long> latencies;
+    for(unsigned int ii = 0; ii < samples; ii++)
+    {
+        unsigned long long start_time(get_computer_time_());
+        get_timing_mode_time_("");
+        unsigned long long end_time(get_computer_time_());
+        latencies.push_back(end_time - start_time);
+    }
+    // Calculate the median two-way latency
+    unsigned long long two_way_latency = median(latencies);
+    df << two_way_latency << '\t';
+
+    // Measure the latency with 1000 TM commands + 32 bytes
+    std::string bytes = "12345678901234567890123456789012";
+    latencies.clear();
+    for(unsigned int ii = 0; ii < samples; ii++)
+    {
+        unsigned long long start_time(get_computer_time_());
+        get_timing_mode_time_(bytes);
+        unsigned long long end_time(get_computer_time_());
+        latencies.push_back(end_time - start_time);
+    }
+    // Calculate the median two-way latency
+    unsigned long long latency_32bytes = median(latencies);
+    df << latency_32bytes << '\t';
+    // Subtract the no-data latency to remove the command transmission time
+    latency_32bytes -= two_way_latency;
+    df << latency_32bytes << '\t';
+    // Divide by 32 to get the per-byte transmission time
+    unsigned long long one_byte_latency32 = latency_32bytes / 32;
+    df << one_byte_latency32 << '\t';
+
+    // Measure the latency with 1000 TM commands + 64 bytes
+    bytes = "1234567890123456789012345678901234567890123456789012345678901234";
+    latencies.clear();
+    for(unsigned int ii = 0; ii < samples; ii++)
+    {
+        unsigned long long start_time(get_computer_time_());
+        get_timing_mode_time_(bytes);
+        unsigned long long end_time(get_computer_time_());
+        latencies.push_back(end_time - start_time);
+    }
+    // Calculate the median two-way latency
+    unsigned long long latency_64bytes = median(latencies);
+    df << latency_64bytes << '\t';
+    // Subtract the no-data latency to remove the command transmission time
+    latency_64bytes -= two_way_latency;
+    df << latency_64bytes << '\t';
+    // Divide by 32 to get the per-byte transmission time
+    unsigned long long one_byte_latency64 = latency_64bytes / 64;
+    df << one_byte_latency64 << '\t';
+
+    // Get the time from the laser
+    // To get the most accurate result, we cannot do any processing while
+    // receiving. We want to know the time that reception finished as exactly
+    // as possible. To achieve this, we do not use send_command_, but instead
+    // send the command manually and receive the entire expected reply at once,
+    // then check/decode it later.
+    char response[17];
+    if(port_->Write("TM1\n", 4) < 4)
+        throw WriteError(19);
+    unsigned int line_length = port_->ReadLine(response, 16);
+    unsigned long long end_time(get_computer_time_());
+    df << end_time << '\t';
+    // Process the response to confirm it is correct
+    if(line_length < 0)
+        throw ReadError(0);
+    else if(line_length == 0)
+        throw ReadError(1);
+    else if(line_length < 15)
+        throw LineLengthError(line_length, 15);
+    response[line_length - 1] = '\0';
+    if(response[0] != 'T' || response[1] != 'M' || response[2] != '1')
+        throw CommandEchoError("TM", response);
+    response[7] = '\0';
+    if(response[4] != '0' || response[5] != '0' || response[6] != 'P')
+        throw ResponseError(response, "TM");
+    // Check the checksum on the time stamp is accurate
+    confirm_checksum_(&response[8], 4, response[12]);
+    // Decode the time stamp
+    unsigned int timestamp =
+        wrap_timestamp_(decode_4_byte_value(&response[8]));
+    df << timestamp << '\t';
+    // The laser had to send 15 bytes to send back the time stamp. This means
+    // that there was approximately a 15 byte delay between the time stamp's
+    // occurance in real time and when we finished receiving the data.
+    // The offset from computer time to laser time is:
+    // (computer time - comms time) - laser time
+    // Not forgetting that the computer times need to be converted to
+    // milliseconds.
+    time_offset_ = ((end_time - (one_byte_latency64 * 15)) / 1e6) - timestamp;
+    df << time_offset_ << '\n';
+
+    // All done.
+    leave_timing_mode_();
+    df.close();
+    return time_offset_;
+}
+
+
+unsigned int Sensor::wrap_timestamp_(unsigned int timestamp)
+{
+    unsigned int result;
+    if(timestamp < last_timestamp_)
+    {
+        wrap_count_++;
+        result = timestamp + wrap_count_ * 0x1000; // 24-bit value + 1
+    }
+    else
+        result = timestamp;
+    last_timestamp_ = timestamp;
+    return result;
+}
+
+
+unsigned int Sensor::offset_timestamp_(unsigned int timestamp)
+{
+    return timestamp + time_offset_;
+}
+
+
 unsigned int Sensor::get_ranges(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
@@ -855,55 +993,56 @@
     memset(buffer, 0, sizeof(char) * 11);
 
     if(start_step < 0)
-        start_step = _first_step;
+        start_step = first_step_;
     if(end_step < 0)
-        end_step = _last_step;
+        end_step = last_step_;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges between " << start_step << " and " <<
             end_step << " with a cluster count of " << cluster_count <<
             '\n';
     }
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
     {
         // Send the command to ask for the most recent range data from
         // start_step to end_step
         number_to_string(start_step, buffer, 3);
         number_to_string(end_step, &buffer[3], 3);
         number_to_string(cluster_count, &buffer[6], 2);
-        _send_command("G", buffer, 8, NULL);
+        send_command_("G", buffer, 8, NULL);
         // In SCIP1 mode we're going to get back 2-byte data
-        _read_2_byte_range_data(data, num_steps);
+        read_2_byte_range_data_(data, num_steps);
     }
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
         // Send the command to ask for the most recent range data from
         // start_step to end_step
         number_to_string(start_step, buffer, 4);
         number_to_string(end_step, &buffer[4], 4);
         number_to_string(cluster_count, &buffer[8], 2);
-        if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
-            _send_command("HD", buffer, 10, NULL);
+        if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
+            send_command_("HD", buffer, 10, NULL);
         else
-            _send_command("GD", buffer, 10, NULL);
+            send_command_("GD", buffer, 10, NULL);
         // There will be a timestamp before the data (if there is data)
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
-        if(_read_line_with_check(buffer) == 0)
+        if(read_line_with_check_(buffer) == 0)
             throw NoDataError();
-        data->_time = decode_4_byte_value(buffer);
+        data->laser_time_ = decode_4_byte_value(buffer);
+        data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the GD command
-        _read_3_byte_range_data(data, num_steps);
+        read_3_byte_range_data_(data, num_steps);
     }
     else
         throw UnknownScipVersionError();
 
-    return data->_length;
+    return data->ranges_length_;
 }
 
 
@@ -913,20 +1052,20 @@
     if(data == NULL)
         throw NoDataError();
 
-    // Calculate the given angles in steps, rounding towards _front_step
+    // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if(start_step < _first_step || start_step > _last_step)
+    if(start_step < first_step_ || start_step > last_step_)
         throw StartStepError();
-    if(end_step < _first_step || end_step > _last_step)
+    if(end_step < first_step_ || end_step > last_step_)
         throw EndStepError();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
+        err_output_ << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
             end_angle << " is step " << end_step << '\n';
     }
@@ -946,22 +1085,22 @@
     memset(buffer, 0, sizeof(char) * 11);
 
     if(start_step < 0)
-        start_step = _first_step;
+        start_step = first_step_;
     if(end_step < 0)
-        end_step = _last_step;
+        end_step = last_step_;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges between " << start_step << " and " <<
             end_step << " with a cluster count of " << cluster_count <<
             '\n';
     }
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(36);
-    else if(_scip_version != 2)
+    else if(scip_version_ != 2)
         throw UnknownScipVersionError();
 
     // Send the command to ask for the most recent data from
@@ -969,21 +1108,22 @@
     number_to_string(start_step, buffer, 4);
     number_to_string(end_step, &buffer[4], 4);
     number_to_string(cluster_count, &buffer[8], 2);
-    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
-        _send_command("HE", buffer, 10, NULL);
+    if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
+        send_command_("HE", buffer, 10, NULL);
     else
-        _send_command("GE", buffer, 10, NULL);
+        send_command_("GE", buffer, 10, NULL);
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(_read_line_with_check(buffer) == 0)
+    if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->_time = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer);
+    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the GE command
-    _read_3_byte_range_data(data, num_steps);
+    read_3_byte_range_data_(data, num_steps);
 
-    return data->_length;
+    return data->ranges_length_;
 }
 
 
@@ -993,20 +1133,20 @@
     if(data == NULL)
         throw NoDataError();
 
-    // Calculate the given angles in steps, rounding towards _front_step
+    // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if(start_step < _first_step || start_step > _last_step)
+    if(start_step < first_step_ || start_step > last_step_)
         throw StartStepError();
-    if(end_step < _first_step || end_step > _last_step)
+    if(end_step < first_step_ || end_step > last_step_)
         throw EndStepError();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
+        err_output_ << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
             end_angle << " is step " << end_step << '\n';
     }
@@ -1022,23 +1162,23 @@
     if(data == NULL)
         throw NoDestinationError();
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(16);
-    else if(_scip_version != 2)
+    else if(scip_version_ != 2)
         throw UnknownScipVersionError();
 
     char buffer[14];
     memset(buffer, 0, sizeof(char) * 14);
 
     if(start_step < 0)
-        start_step = _first_step;
+        start_step = first_step_;
     if(end_step < 0)
-        end_step = _last_step;
+        end_step = last_step_;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " new ranges between " << start_step << " and " <<
             end_step << " with a cluster count of " << cluster_count <<
             '\n';
@@ -1051,19 +1191,19 @@
     number_to_string(1, &buffer[10], 1);
     number_to_string(1, &buffer[11], 2);
     char command[3];
-    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+    if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
         command[0] = 'N';
     else
         command[0] = 'M';
     command[1] = 'D';
     command[2] = '\0';
-    _send_command(command, buffer, 13, NULL);
+    send_command_(command, buffer, 13, NULL);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
     char response[17];
-    _skip_lines(1); // End of the command echo message
-    _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
+    skip_lines_(1); // End of the command echo message
+    read_line_(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
     if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
@@ -1072,10 +1212,10 @@
     if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
-    _read_line_with_check(response, 4);
-    if(_verbose)
+    read_line_with_check_(response, 4);
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
+        err_output_ << "Sensor::" << __func__ <<
             "() " << command << " data prefix status: " << response[0] <<
             response[1] << '\n';
     }
@@ -1084,7 +1224,7 @@
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
-        _skip_lines(1);
+        skip_lines_(1);
         throw ResponseError(response, command);
     }
 
@@ -1092,14 +1232,15 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(_read_line_with_check(buffer) == 0)
+    if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->_time = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer);
+    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the MD command
-    _read_3_byte_range_data(data, num_steps);
+    read_3_byte_range_data_(data, num_steps);
 
-    return data->_length;
+    return data->ranges_length_;
 }
 
 
@@ -1108,23 +1249,23 @@
 {
     if(data == NULL)
         throw NoDestinationError();
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(16);
 
-    // Calculate the given angles in steps, rounding towards _front_step
+    // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if(start_step < _first_step || start_step > _last_step)
+    if(start_step < first_step_ || start_step > last_step_)
         throw StartStepError();
-    if(end_step < _first_step || end_step > _last_step)
+    if(end_step < first_step_ || end_step > last_step_)
         throw EndStepError();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
+        err_output_ << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
             end_angle << " is step " << end_step << '\n';
     }
@@ -1140,23 +1281,23 @@
     if(data == NULL)
         throw NoDestinationError();
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(17);
-    else if(_scip_version != 2)
+    else if(scip_version_ != 2)
         throw UnknownScipVersionError();
 
     char buffer[14];
     memset(buffer, 0, sizeof(char) * 14);
 
     if(start_step < 0)
-        start_step = _first_step;
+        start_step = first_step_;
     if(end_step < 0)
-        end_step = _last_step;
+        end_step = last_step_;
 
     unsigned int num_steps = (end_step - start_step + 1) / cluster_count;
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " new ranges and intensities between " <<
             start_step << " and " << end_step <<
             " with a cluster count of " << cluster_count << '\n';
@@ -1170,19 +1311,19 @@
     number_to_string(1, &buffer[10], 1);
     number_to_string(1, &buffer[11], 2);
     char command[3];
-    if(_model == MODEL_UXM30LXE && _multiecho_mode != ME_OFF)
+    if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
         command[0] = 'N';
     else
         command[0] = 'M';
     command[1] = 'E';
     command[2] = '\0';
-    _send_command(command, buffer, 13, NULL);
+    send_command_(command, buffer, 13, NULL);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
     char response[17];
-    _skip_lines(1); // End of the command echo message
-    _read_line(response, 16); // Size is command(2)+params(13)+new line(1)
+    skip_lines_(1); // End of the command echo message
+    read_line_(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
     if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
@@ -1191,10 +1332,10 @@
     if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
-    _read_line_with_check(response, 4);
-    if(_verbose)
+    read_line_with_check_(response, 4);
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
+        err_output_ << "Sensor::" << __func__ <<
             "() " << command << " data prefix status: " << response[0] <<
             response[1] << '\n';
     }
@@ -1203,7 +1344,7 @@
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
-        _skip_lines(1);
+        skip_lines_(1);
         throw ResponseError(response, command);
     }
 
@@ -1211,14 +1352,15 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(_read_line_with_check(buffer) == 0)
+    if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->_time = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer);
+    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the ME command
-    _read_3_byte_range_and_intensity_data(data, num_steps);
+    read_3_byte_range_and_intensity_data_(data, num_steps);
 
-    return data->_length;
+    return data->ranges_length_;
 }
 
 
@@ -1227,23 +1369,23 @@
 {
     if(data == NULL)
         throw NoDestinationError();
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
         throw UnsupportedError(17);
 
-    // Calculate the given angles in steps, rounding towards _front_step
+    // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
     end_step = angle_to_step(end_angle);
 
     // Check the steps are within the allowable range
-    if(start_step < _first_step || start_step > _last_step)
+    if(start_step < first_step_ || start_step > last_step_)
         throw StartStepError();
-    if(end_step < _first_step || end_step > _last_step)
+    if(end_step < first_step_ || end_step > last_step_)
         throw EndStepError();
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Start angle " <<
+        err_output_ << "Sensor::" << __func__ << "() Start angle " <<
             start_angle << " is step " << start_step << ", end angle " <<
             end_angle << " is step " << end_step << '\n';
     }
@@ -1256,8 +1398,8 @@
 
 double Sensor::step_to_angle(unsigned int step)
 {
-    return (static_cast<int>(step) - static_cast<int>(_front_step)) *
-        _resolution;
+    return (static_cast<int>(step) - static_cast<int>(front_step_)) *
+        resolution_;
 }
 
 
@@ -1265,11 +1407,11 @@
 {
     unsigned int result;
     double resultF;
-    resultF = _front_step +
-        (static_cast<double>(angle) / static_cast<double>(_resolution));
-    // Round towards _front_step so that the step values are always inside the
+    resultF = front_step_ +
+        (static_cast<double>(angle) / static_cast<double>(resolution_));
+    // Round towards front_step_ so that the step values are always inside the
     // angles given
-    if(resultF < _front_step)
+    if(resultF < front_step_)
         result = static_cast<int>(ceil(resultF));
     else
         result = static_cast<int>(floor(resultF));
@@ -1288,10 +1430,10 @@
 // keep flushing until the port reports there is no data left after a timeout.
 // This shouldn't be called too much, as it introduces a delay as big as the
 // timeout (which may be infinite).
-void Sensor::_clear_read_buffer()
+void Sensor::clear_read_buffer_()
 {
-    while(_port->BytesAvailableWait() > 0)
-        _port->Flush();
+    while(port_->BytesAvailableWait() > 0)
+        port_->Flush();
 }
 
 
@@ -1303,21 +1445,21 @@
 // The line feed that terminates a line will be replaced with a NULL.
 // The return value is the number of bytes received, not including the NULL
 // byte or the line feed.
-int Sensor::_read_line(char* buffer, int expected_length)
+int Sensor::read_line_(char* buffer, int expected_length)
 {
     int linelength = 0;
 
     if(expected_length == -1)
     {
-        int maxlength = (_scip_version == 1) ?
+        int maxlength = (scip_version_ == 1) ?
             SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ << "() Reading up to " <<
+            err_output_ << "Sensor::" << __func__ << "() Reading up to " <<
                 maxlength << " bytes.\n";
         }
         // We need to get at least 1 byte in a line: the line feed.
-        if((linelength = _port->ReadLine(buffer, maxlength)) < 0)
+        if((linelength = port_->ReadLine(buffer, maxlength)) < 0)
             throw ReadError(0);
         else if(linelength == 0)
             throw ReadError(1);
@@ -1326,13 +1468,13 @@
     }
     else
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Reading exactly " << expected_length << " bytes.\n";
         }
         // expected_length+1 for the NULL
-        if((linelength = _port->ReadLine(buffer, expected_length + 1)) < 0)
+        if((linelength = port_->ReadLine(buffer, expected_length + 1)) < 0)
             throw ReadError(0);
         else if(linelength == 0)
             throw ReadError(1);
@@ -1342,11 +1484,11 @@
         buffer[linelength - 1] = '\0';
     }
 
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Read " << linelength <<
+        err_output_ << "Sensor::" << __func__ << "() Read " << linelength <<
             " bytes.\n";
-        _err_output << "Sensor::" << __func__ << "() Line is " << buffer <<
+        err_output_ << "Sensor::" << __func__ << "() Line is " << buffer <<
             '\n';
     }
     return linelength - 1; // Line feed not included
@@ -1356,7 +1498,7 @@
 // This function will read a line and then calculate its checksum, comparing it
 // with the checksum at the end of the line. The checksum will be removed
 // (along with the semi-colon, if present).  buffer and expected_length args are
-// as for _read_line().
+// as for read_line_().
 // If has_semicolon is true, the byte before the checksum is assumed to be the
 // semi-colon separator and so not a part of the checksum. If it's not a
 // semi-colon, an exception is thrown.
@@ -1367,14 +1509,14 @@
 // BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
 // whereby it does not include anything after and including "<-" in the
 // checksum calculation. A workaround is enabled in this function when
-// _model is MODEL_UTM30LX. In this case, if the checksum fails normally, it
+// model_ is MODEL_UTM30LX. In this case, if the checksum fails normally, it
 // scans the line for "<-" and recalculates the checksum on the bytes up to
 // that point. This only happens in SCIP v2.
-int Sensor::_read_line_with_check(char* buffer, int expected_length,
+int Sensor::read_line_with_check_(char* buffer, int expected_length,
         bool has_semicolon)
 {
-    int linelength = _read_line(buffer, expected_length);
-    if(_scip_version == 1)
+    int linelength = read_line_(buffer, expected_length);
+    if(scip_version_ == 1)
     {
         // No checksums in SCIP version 1
         return linelength;
@@ -1385,13 +1527,13 @@
     if(linelength == 0)
         return 0;
 
-    // Ignore the checksum itself, and possibly a semicolon (_read_line has
+    // Ignore the checksum itself, and possibly a semicolon (read_line_ has
     // already chopped off the line feed for us).
     int bytesToConsider = linelength - 1 - (has_semicolon ? 1 : 0);
     int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0);
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Considering " <<
+        err_output_ << "Sensor::" << __func__ << "() Considering " <<
             bytesToConsider << " bytes for checksum from a line length of " <<
             linelength << " bytes.\n";
     }
@@ -1401,21 +1543,21 @@
     int checkSum = 0;
     try
     {
-        checkSum = _confirm_checksum(buffer, bytesToConsider,
+        checkSum = confirm_checksum_(buffer, bytesToConsider,
                 static_cast<int>(buffer[checksumIndex]));
     }
     catch(ProtocolError& e)
     {
-        if(_model == MODEL_UTM30LX && _enable_checksum_workaround)
+        if(model_ == MODEL_UTM30LX && enable_checksum_workaround_)
         {
             // Here comes the UTM-30LX workaround
             char* hasComment = strstr(buffer, "<-");
             if(hasComment != NULL)
             {
                 int newBytesToConsider = hasComment - buffer;
-                if(_verbose)
+                if(verbose_)
                 {
-                    _err_output << "Sensor::" << __func__ <<
+                    err_output_ << "Sensor::" << __func__ <<
                         "() Performing UTM-30LX II response "
                         "checksum workaround: trying with " <<
                         newBytesToConsider << " bytes.\n";
@@ -1426,7 +1568,7 @@
                             linelength);
                 }
 
-                checkSum = _confirm_checksum(buffer, newBytesToConsider,
+                checkSum = confirm_checksum_(buffer, newBytesToConsider,
                         static_cast<int>(buffer[checksumIndex]));
             }
             else
@@ -1446,12 +1588,12 @@
 
 
 // Reads lines until the number specified has passed.
-void Sensor::_skip_lines(int count)
+void Sensor::skip_lines_(int count)
 {
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ << "() Skipping " << count <<
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ << "() Skipping " << count <<
             " lines.\n";
-    if(_port->SkipUntil(0x0A, count) < 0)
+    if(port_->SkipUntil(0x0A, count) < 0)
         throw ReadError(18);
 }
 
@@ -1465,40 +1607,40 @@
 // If param_length is 0, no parameters will be sent or expected in the reply.
 // extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
 // Return value is the status code returned for the command.
-int Sensor::_send_command(char const* cmd, char const* param,
+int Sensor::send_command_(char const* cmd, char const* param,
         int param_length, char const* extra_ok)
 {
     int statusCode = -1;
     char response[17];
 
     // Flush first to clear out the dregs of any previous commands
-    _port->Flush();
+    port_->Flush();
 
-    if(_scip_version == 1)
+    if(scip_version_ == 1)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Writing in SCIP1 mode. Command is " << cmd[0] <<
                 ", parameters length is " << param_length << '\n';
         }
         // Write the command
-        if(_port->Write(cmd, 1) < 1)
+        if(port_->Write(cmd, 1) < 1)
             throw WriteError(19);
         if(param_length > 0)
         {
-            if(_port->Write(param, param_length) < param_length)
+            if(port_->Write(param, param_length) < param_length)
                 throw WriteError(20);
         }
-        if(_port->Write("\n", 1) < 1)
+        if(port_->Write("\n", 1) < 1)
             throw WriteError(21);
 
         // Read back the response (should get at least 4 bytes , possibly up to
         // 16 including \n's depending on the parameters): cmd[0] params \n
         // status \n
         int statusIndex = 2 + param_length;
-        _read_line(response, 2 + param_length);
-        _read_line(&response[statusIndex], 2);
+        read_line_(response, 2 + param_length);
+        read_line_(&response[statusIndex], 2);
         // First make sure that the echoed command matches
         if(response[0] != cmd[0])
         {
@@ -1522,9 +1664,9 @@
             }
         }
         // Next up, check the status byte
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Command response status: " << response[statusIndex] <<
                 '\n';
         }
@@ -1536,7 +1678,7 @@
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
-                    _skip_lines(1);
+                    skip_lines_(1);
                     throw Scip1ResponseError(response[statusIndex], cmd[0]);
                 }
             }
@@ -1544,34 +1686,34 @@
             {
                 // There is an extra line feed after an error status
                 // (signalling end of message)
-                _skip_lines(1);
+                skip_lines_(1);
                 throw Scip1ResponseError(response[statusIndex], cmd[0]);
             }
         }
         statusCode = atoi(&response[statusIndex]);
         // All OK, data starts at beginning of port's buffer
     }
-    else if(_scip_version == 2)
+    else if(scip_version_ == 2)
     {
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Writing in SCIP2 mode. Command is " << cmd <<
                 ", parameters length is " << param_length << '\n';
         }
         // Write the command
-        if(_port->Write(cmd, 2) < 2)
+        if(port_->Write(cmd, 2) < 2)
             throw WriteError(19);
         if(param_length > 0)
         {
-            if(_port->Write(param, param_length) < param_length)
+            if(port_->Write(param, param_length) < param_length)
                 throw WriteError(20);
         }
-        if(_port->Write("\n", 1) < 1)
+        if(port_->Write("\n", 1) < 1)
             throw WriteError(21);
 
         // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        _read_line(response, 3 + param_length);
+        read_line_(response, 3 + param_length);
         // Check the echo is correct
         if(response[0] != cmd[0] || response[1] != cmd[1])
             throw CommandEchoError(cmd, response);
@@ -1583,10 +1725,10 @@
         }
 
         // The next line should be the status line
-        _read_line_with_check(response, 4);
-        if(_verbose)
+        read_line_with_check_(response, 4);
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Command response status: " << response[0] << response[1] <<
                 '\n';
         }
@@ -1601,7 +1743,7 @@
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
-                    _skip_lines(1);
+                    skip_lines_(1);
                     throw ResponseError(response, cmd);
                 }
             }
@@ -1609,7 +1751,7 @@
             {
                 // There is an extra line feed after an error status
                 // (signalling end of message)
-                _skip_lines(1);
+                skip_lines_(1);
                 throw ResponseError(response, cmd);
             }
         }
@@ -1623,46 +1765,95 @@
 }
 
 
-void Sensor::_find_model(char const* buffer)
+/// Puts the laser into the timing mode.
+void Sensor::enter_timing_mode_()
 {
+    send_command_("TM", "0", 1, NULL);
+}
+
+
+/// Get the timestamp from the laser.
+unsigned int Sensor::get_timing_mode_time_(std::string const& data)
+{
+    std::string command = "TM1";
+    if(data != "")
+    {
+        send_command_((command + data).c_str(), NULL,
+                command.size() + data.size(), NULL);
+    }
+    else
+        send_command_("TM", "1", 1, NULL);
+
+    char buffer[7];
+    read_line_with_check_(buffer, 6);
+    return decode_4_byte_value(buffer);
+}
+
+
+/// Get the computer's time as accurately as possible.
+unsigned long long Sensor::get_computer_time_()
+{
+#if defined(_POSIX_TIMERS)
+    struct timespec ts;
+    clock_gettime(CLOCK_REALTIME, &ts);
+    return ts.tv_sec * 1e9 + ts.tv_nsec;
+#else
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    return tv.tv_sec * 1e9 + tv.tv_usec * 1e3;
+#endif
+}
+
+
+/// Take the laser out of timing mode.
+void Sensor::leave_timing_mode_()
+{
+    send_command_("TM", "2", 1, NULL);
+    skip_lines_(1);
+}
+
+
+/// Search a string for the laser's model.
+void Sensor::find_model_(char const* buffer)
+{
     if(strstr(buffer, "URG-04LX") != NULL)
-        _model = MODEL_URG04LX;
+        model_ = MODEL_URG04LX;
     else if(strstr(buffer, "UBG-04LX") != NULL)
-        _model = MODEL_UBG04LXF01;
+        model_ = MODEL_UBG04LXF01;
     else if(strstr(buffer, "UHG-08LX") != NULL)
-        _model = MODEL_UHG08LX;
+        model_ = MODEL_UHG08LX;
     else if(strstr(buffer, "UTM-30LX") != NULL)
     {
-        _model = MODEL_UTM30LX;
+        model_ = MODEL_UTM30LX;
         // Also enable the work around for a checksum problem in this
         // model.
-        _enable_checksum_workaround = true;
+        enable_checksum_workaround_ = true;
     }
     else if(strstr(buffer, "UXM-30LX") != NULL)
-        _model = MODEL_UXM30LXE;
+        model_ = MODEL_UXM30LXE;
     else
-        _model = MODEL_UNKNOWN;
+        model_ = MODEL_UNKNOWN;
 }
 
 
-void Sensor::_get_and_set_scip_version()
+void Sensor::get_and_set_scip_version_()
 {
     bool scip2Failed = false;
 
 
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ <<
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ <<
             "() Testing SCIP protocol version.\n";
     // Try SCIP version 2 first by sending an info command
     try
     {
-        _send_command("VV", NULL, 0, NULL);
+        send_command_("VV", NULL, 0, NULL);
     }
     catch(BaseError)
     {
         // That didn't work too well...
-        if(_verbose)
-            _err_output << "Sensor::" << __func__ <<
+        if(verbose_)
+            err_output_ << "Sensor::" << __func__ <<
                 "() Initial SCIP version 2 test failed.\n";
         scip2Failed = true;
     }
@@ -1671,23 +1862,23 @@
     {
         // Currently using SCIP version 1
         // Get the firmware version and check if we can move to SCIP version 2
-        _scip_version = 1;
+        scip_version_ = 1;
 
-        _port->Flush();
+        port_->Flush();
         try
         {
-            _send_command("V", NULL, 0, NULL);
+            send_command_("V", NULL, 0, NULL);
         }
         catch(BaseError)
         {
             throw ScipVersionError();
         }
         // Skip the vendor and product info
-        _skip_lines(2);
+        skip_lines_(2);
         // Get the firmware line
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
-        _read_line(buffer);
+        read_line_(buffer);
 
         if(strncmp(buffer, "FIRM:", 5) != 0)
             throw MissingFirmSpecError();
@@ -1699,22 +1890,22 @@
         int majorVer = strtol(&buffer[5], NULL, 10);
         if(errno == ERANGE)
             throw FirmwareError();
-        if(_verbose)
+        if(verbose_)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Firmware major version is " << majorVer <<
                 '\n';
         }
         // Dump the rest of the V command result (one of these will be the
         // empty last line)
-        _skip_lines(3);
+        skip_lines_(3);
 
         // If the firmware version is less than 3, we're stuck with SCIP
         // version 1.
         if(majorVer < 3)
         {
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Firmware does not support SCIP version 2; using SCIP "
                     "version 1.\n";
             return;
@@ -1722,44 +1913,44 @@
         // Otherwise we can try SCIP version 2
         else
         {
-            _port->Flush();
-            // We'll hijack the _send_command function a bit here. Normally it
+            port_->Flush();
+            // We'll hijack the send_command_ function a bit here. Normally it
             // takes 1-byte commands, (we're currently using SCIP version 1,
             // remember), but the command to change to SCIP version 2 is 7
             // bytes long (why did they have to do it that way?). So send the
             // first byte as the command and the other 6 as parameters.
             try
             {
-                _send_command("S", "CIP2.0", 6, NULL);
+                send_command_("S", "CIP2.0", 6, NULL);
             }
             catch(BaseError)
             {
-                if(_verbose)
-                    _err_output << "Sensor::" << __func__ <<
+                if(verbose_)
+                    err_output_ << "Sensor::" << __func__ <<
                         "() Could not change to SCIP version 2; using SCIP "
                         "version 1.\n";
                 return;
             }
             // There'll be a trailing line on the end
-            _skip_lines(1);
+            skip_lines_(1);
 
             // Changed to SCIP version 2
-            if(_verbose)
-                _err_output << "Sensor::" << __func__ <<
+            if(verbose_)
+                err_output_ << "Sensor::" << __func__ <<
                     "() Using SCIP version 2.\n";
-            _scip_version = 2;
+            scip_version_ = 2;
             return;
         }
     }
     else
     {
         // Currently using SCIP version 2
-        _scip_version = 2;
+        scip_version_ = 2;
 
         // Dump the rest of the result
-        _skip_lines(6);
-        if(_verbose)
-            _err_output << "Sensor::" << __func__ <<
+        skip_lines_(6);
+        if(verbose_)
+            err_output_ << "Sensor::" << __func__ <<
                 "() Using SCIP version 2.\n";
         return;
     }
@@ -1769,34 +1960,34 @@
 }
 
 
-void Sensor::_get_defaults()
+void Sensor::get_defaults_()
 {
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ <<
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ <<
             "() Getting default values.\n";
 
     // Get the laser's info
     SensorInfo info;
     get_sensor_info(&info);
 
-    _min_angle = info.min_angle;
-    _max_angle = info.max_angle;
-    _resolution = info.resolution;
-    _first_step = info.first_step;
-    _last_step = info.last_step;
-    _front_step = info.front_step;
-    _max_range = info.max_range;
-    if(_verbose)
+    min_angle_ = info.min_angle;
+    max_angle_ = info.max_angle;
+    resolution_ = info.resolution;
+    first_step_ = info.first_step;
+    last_step_ = info.last_step;
+    front_step_ = info.front_step;
+    max_range_ = info.max_range;
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ <<
-            "() Got default values: " << _min_angle << " " << _max_angle <<
-            " " << _resolution << " " << _first_step << " " << _last_step <<
-            " " << _front_step << " " << _max_range << '\n';
+        err_output_ << "Sensor::" << __func__ <<
+            "() Got default values: " << min_angle_ << " " << max_angle_ <<
+            " " << resolution_ << " " << first_step_ << " " << last_step_ <<
+            " " << front_step_ << " " << max_range_ << '\n';
     }
 }
 
 
-void Sensor::_process_vv_line(char const* buffer, SensorInfo* info)
+void Sensor::process_vv_line_(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "VEND", 4) == 0)
         info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
@@ -1804,8 +1995,8 @@
     {
         info->product = &buffer[5]; // Product info
         // Find the product model
-        _find_model(&buffer[5]);
-        info->detected_model = _model;
+        find_model_(&buffer[5]);
+        info->detected_model = model_;
     }
     else if(strncmp(buffer, "FIRM", 4) == 0)
         info->firmware = &buffer[5]; // Firmware version
@@ -1813,12 +2004,12 @@
         info->protocol = &buffer[5]; // Protocol version
     else if(strncmp(buffer, "SERI", 4) == 0)
         info->serial = &buffer[5]; // Serial number
-    else if(!_ignore_unknowns)
+    else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::_process_pp_line(char const* buffer, SensorInfo* info)
+void Sensor::process_pp_line_(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
         info->model = &buffer[5];   // Model
@@ -1846,12 +2037,12 @@
         else
             info->rot_dir = CLOCKWISE;
     }*/
-    else if(!_ignore_unknowns)
+    else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::_process_ii_line(char const* buffer, SensorInfo* info)
+void Sensor::process_ii_line_(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
         // Do nothing here - we already know this value from PP
@@ -1905,58 +2096,57 @@
     }
     else if(strncmp(buffer, "STAT", 4) == 0)
         info->sensor_diagnostic = &buffer[5];
-    else if(!_ignore_unknowns)
+    else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }
 
 
 /// Combines up to three values from an echo buffer into a single value based
-/// on the setting of _multiecho_mode.
-void Sensor::_process_echo_buffer(int const* buffer, int num_echos,
-        uint32_t* dest)
+/// on the setting of multiecho_mode_.
+uint32_t Sensor::process_echo_buffer_(int const* buffer, int num_echos)
 {
     uint32_t sum = 0;
-    switch(_multiecho_mode)
+    switch(multiecho_mode_)
     {
         case ME_FRONT:
-            *dest = buffer[0];
+            return buffer[0];
             break;
         case ME_MIDDLE:
             if(num_echos == 3)
-                *dest = buffer[1];
+                return buffer[1];
             else
-                *dest = buffer[0];
+                return buffer[0];
             break;
         case ME_REAR:
-            *dest = buffer[num_echos - 1];
+            return buffer[num_echos -1 ];
             break;
         case ME_AVERAGE:
             for (int ii = 0; ii < num_echos; ii++)
                 sum += buffer[ii];
             sum /= static_cast<float>(num_echos);
-            *dest = sum;
+            return sum;
             break;
         case ME_OFF:
         default:
-            *dest = buffer[0];
+            return buffer[0];
             break;
     }
 }
 
 
-void Sensor::_read_2_byte_range_data(ScanData* data, unsigned int num_steps)
+void Sensor::read_2_byte_range_data_(ScanData* data, unsigned int num_steps)
 {
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
     }
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps);
-    data->_model = _model;
-    data->_error = false;
+    data->model_ = model_;
+    data->error_ = false;
 
     // 2 byte data is easy since it fits neatly in a 64-byte block
     char buffer[SCIP2_LINE_LENGTH];
@@ -1965,7 +2155,7 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
+        numBytesInLine = read_line_with_check_(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
             break;
@@ -1977,40 +2167,38 @@
                 // Line feed in the middle of a data block? Why?
                 throw MisplacedLineFeedError();
             }
-            data->_ranges[current_step] = decode_2_byte_value(&buffer[ii]);
-            if(data->_ranges[current_step] < 20)
-                data->_error = true;
+            data->write_range_(current_step, decode_2_byte_value(&buffer[ii]));
         }
         // End of this line. Go around again.
     }
 
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ << "() Read " <<
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ << "() Read " <<
             current_step << " ranges.\n";
     if(current_step != num_steps)
         throw DataCountError();
 }
 
 
-void Sensor::_read_3_byte_range_data(ScanData* data, unsigned int num_steps)
+void Sensor::read_3_byte_range_data_(ScanData* data, unsigned int num_steps)
 {
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges.\n";
-        if(_multiecho_mode != ME_OFF)
+        if(multiecho_mode_ != ME_OFF)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Multi-echo mode is set to " <<
-                multiecho_mode_to_string(_multiecho_mode) << '\n';
+                multiecho_mode_to_string(multiecho_mode_) << '\n';
         }
     }
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps);
-    data->_model = _model;
-    data->_error = false;
+    data->model_ = model_;
+    data->error_ = false;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
@@ -2023,24 +2211,25 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
+        numBytesInLine = read_line_with_check_(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
         {
             if(echo_buffer[0] != -1)
             {
                 // Not the first value, so deal with the previous
-                _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
-                        &data->_ranges[current_step]);
-                if(data->_ranges[current_step] > _max_range)
+                data->write_range_(current_step,
+                        process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
+                if(data->ranges_)
                 {
-                    _err_output << "WARNING: Sensor::" << __func__ <<
-                        "() Value at step " << current_step <<
-                        " beyond maximum range: " <<
-                        data->_ranges[current_step];
+                    if(data->ranges_[current_step] > max_range_)
+                    {
+                        err_output_ << "WARNING: Sensor::" << __func__ <<
+                            "() Value at step " << current_step <<
+                            " beyond maximum range: " <<
+                            data->ranges_[current_step];
+                    }
                 }
-                else if(data->_ranges[current_step] < 20)
-                    data->_error = true;
                 current_step++;
             }
             break;
@@ -2067,17 +2256,19 @@
                 else if(echo_buffer[0] != -1)
                 {
                     // Not the first value, so deal with the previous
-                    _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
-                            &data->_ranges[current_step]);
-                    if(data->_ranges[current_step] > _max_range)
+                    data->write_range_(current_step,
+                            process_echo_buffer_(echo_buffer,
+                                echo_buf_ind + 1));
+                    if(data->ranges_)
                     {
-                        _err_output << "WARNING: Sensor::" << __func__ <<
-                            "() Value at step " << current_step <<
-                            " beyond maximum range: " <<
-                            data->_ranges[current_step];
+                        if(data->ranges_[current_step] > max_range_)
+                        {
+                            err_output_ << "WARNING: Sensor::" << __func__ <<
+                                "() Value at step " << current_step <<
+                                " beyond maximum range: " <<
+                                data->ranges_[current_step];
+                        }
                     }
-                    else if(data->_ranges[current_step] < 20)
-                        data->_error = true;
                     current_step++;
                     echo_buf_ind = 0;
                     echo_buffer[0] = -1;
@@ -2125,33 +2316,33 @@
         // End of this line. Go around again.
     }
 
-    if(_verbose)
-        _err_output << "Sensor::" << __func__ << "() Read " <<
+    if(verbose_)
+        err_output_ << "Sensor::" << __func__ << "() Read " <<
             current_step << " ranges.\n";
     if(current_step != num_steps)
         throw DataCountError();
 }
 
 
-void Sensor::_read_3_byte_range_and_intensity_data(ScanData* data,
+void Sensor::read_3_byte_range_and_intensity_data_(ScanData* data,
         unsigned int num_steps)
 {
-    if(_verbose)
+    if(verbose_)
     {
-        _err_output << "Sensor::" << __func__ << "() Reading " <<
+        err_output_ << "Sensor::" << __func__ << "() Reading " <<
             num_steps << " ranges and intensities.\n";
-        if(_multiecho_mode != ME_OFF)
+        if(multiecho_mode_ != ME_OFF)
         {
-            _err_output << "Sensor::" << __func__ <<
+            err_output_ << "Sensor::" << __func__ <<
                 "() Multi-echo mode is set to " <<
-                multiecho_mode_to_string(_multiecho_mode) << '\n';
+                multiecho_mode_to_string(multiecho_mode_) << '\n';
         }
     }
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
     data->allocate_data(num_steps, true);
-    data->_model = _model;
+    data->model_ = model_;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
@@ -2165,7 +2356,7 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = _read_line_with_check(buffer);
+        numBytesInLine = read_line_with_check_(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
         {
@@ -2175,8 +2366,8 @@
             if(echo_buffer[0] != -1)
             {
                 assert(nextIsIntensity == true);
-                _process_echo_buffer(echo_buffer, echo_buf_ind + 1,
-                        &data->_intensities[current_intensity]);
+                data->write_intensity_(current_intensity,
+                    process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
                 current_intensity++;
             }
             break;
@@ -2203,30 +2394,37 @@
                 else if(echo_buffer[0] != -1)
                 {
                     // Not the first value, so deal with the previous
-                    uint32_t* dest;
                     if(nextIsIntensity)
-                        dest = &data->_intensities[current_intensity];
+                    {
+                        data->write_intensity_(current_intensity,
+                                process_echo_buffer_(echo_buffer,
+                                    echo_buf_ind + 1));
+                    }
                     else
-                        dest = &data->_ranges[current_range];
-                    _process_echo_buffer(echo_buffer, echo_buf_ind + 1, dest);
+                    {
+                        data->write_range_(current_range,
+                                process_echo_buffer_(echo_buffer,
+                                    echo_buf_ind + 1));
+                    }
                     echo_buf_ind = 0;
                     echo_buffer[0] = -1;
-                    if(data->_ranges[current_range] > _max_range &&
-                            !nextIsIntensity)
+                    if(data->ranges_)
                     {
-                        _err_output << "WARNING: Sensor::" << __func__ <<
-                            "() Value at step " << current_range <<
-                            " beyond maximum range: " <<
-                            data->_ranges[current_range] << " (raw bytes: ";
-                        if(split_count != 0)
-                            _err_output << split_value[0] << split_value[1] <<
-                                split_value[2] << ")\n";
-                        else
-                            _err_output << buffer[0] << buffer[1] << buffer[2] <<
-                                ")\n";
+                        if(data->ranges_[current_range] > max_range_ &&
+                                !nextIsIntensity)
+                        {
+                            err_output_ << "WARNING: Sensor::" << __func__ <<
+                                "() Value at step " << current_range <<
+                                " beyond maximum range: " <<
+                                data->ranges_[current_range] << " (raw bytes: ";
+                            if(split_count != 0)
+                                err_output_ << split_value[0] << split_value[1] <<
+                                    split_value[2] << ")\n";
+                            else
+                                err_output_ << buffer[0] << buffer[1] << buffer[2] <<
+                                    ")\n";
+                        }
                     }
-                    else if(data->_ranges[current_range] < 20)
-                        data->_error = true;
                     if(nextIsIntensity)
                         current_intensity++;
                     else
@@ -2278,9 +2476,9 @@
         // End of this line. Go around again.
     }
 
-    if(_verbose)
+    if(verbose_)
     {

@@ Diff output truncated at 100000 characters. @@

This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 511
          http://gearbox.svn.sourceforge.net/gearbox/?rev=511&view=rev
Author:   gbiggs
Date:     2010-08-11 00:49:53 +0000 (Wed, 11 Aug 2010)

Log Message:
-----------
Better clock calibration, including skew and drift adjustment.

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h
    gearbox/trunk/src/hokuyo_aist/test/example.cpp
    gearbox/trunk/src/hokuyo_aist/utils.h

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-11 00:49:53 UTC (rev 511)
@@ -31,7 +31,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 ScanData::ScanData()
-    : ranges_(NULL), intensities_(NULL), ranges_length_(0),
+    : ranges_(0), intensities_(0), ranges_length_(0),
     intensities_length_(0), error_(false), laser_time_(0), system_time_(0),
     model_(MODEL_UNKNOWN), buffers_provided_(false)
 {
@@ -54,7 +54,7 @@
     ranges_length_ = rhs.ranges_length();
     intensities_length_ = rhs.intensities_length();
     if(ranges_length_ == 0)
-        ranges_ = NULL;
+        ranges_ = 0;
     else
     {
         try
@@ -69,7 +69,7 @@
         memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * ranges_length_);
     }
     if(intensities_length_ == 0)
-        intensities_ = NULL;
+        intensities_ = 0;
     else
     {
         try
@@ -96,15 +96,15 @@
 {
     if(!buffers_provided_)
     {
-        if (ranges_ != NULL)
+        if (ranges_ != 0)
         {
             delete[] ranges_;
-            ranges_ = NULL;
+            ranges_ = 0;
         }
-        if (intensities_ != NULL)
+        if (intensities_ != 0)
         {
             delete[] intensities_;
-            intensities_ = NULL;
+            intensities_ = 0;
         }
     }
 }
@@ -192,14 +192,14 @@
     unsigned int rhslength = rhs.ranges_length();
     if(rhslength == 0)
     {
-        ranges_ = NULL;
+        ranges_ = 0;
         ranges_length_ = 0;
     }
     else
     {
         if(rhslength != ranges_length_)
         {
-            if(!buffers_provided_ && ranges_ != NULL)
+            if(!buffers_provided_ && ranges_ != 0)
             {
                 // Just copy
                 memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * rhslength);
@@ -211,7 +211,7 @@
                 // self-assignment making a mess).
                 uint32_t* new_data = new uint32_t[rhslength];
                 memcpy(new_data, rhs.ranges(), sizeof(uint32_t) * rhslength);
-                if(ranges_ != NULL)
+                if(ranges_ != 0)
                     delete[] ranges_;
                 ranges_ = new_data;
                 ranges_length_ = rhs.ranges_length();
@@ -227,14 +227,14 @@
     rhslength = rhs.intensities_length();
     if(rhslength == 0)
     {
-        intensities_ = NULL;
+        intensities_ = 0;
         intensities_length_ = 0;
     }
     else
     {
         if(rhslength != intensities_length_)
         {
-            if(!buffers_provided_ && intensities_ != NULL)
+            if(!buffers_provided_ && intensities_ != 0)
             {
                 // Just copy
                 memcpy(intensities_, rhs.intensities(),
@@ -248,7 +248,7 @@
                 uint32_t* new_data = new uint32_t[rhslength];
                 memcpy(new_data, rhs.intensities(),
                         sizeof(uint32_t) * rhslength);
-                if(intensities_ != NULL)
+                if(intensities_ != 0)
                     delete[] intensities_;
                 intensities_ = new_data;
                 intensities_length_ = rhs.intensities_length();
@@ -284,7 +284,7 @@
 {
     std::stringstream ss;
 
-    if(ranges_ != NULL)
+    if(ranges_ != 0)
     {
         ss << ranges_length_ << " ranges from model ";
         ss << model_to_string(model_) << ":\n";
@@ -292,7 +292,7 @@
             ss << ranges_[ii] << '\t';
         ss << '\n';
     }
-    if(intensities_ != NULL)
+    if(intensities_ != 0)
     {
         ss << intensities_length_ << " intensities from model ";
         ss << model_to_string(model_) << ":\n";
@@ -323,12 +323,12 @@
 {
     if(!buffers_provided_)
     {
-        if(ranges_ != NULL)
+        if(ranges_ != 0)
             delete[] ranges_;
-        ranges_ = NULL;
-        if(intensities_ != NULL)
+        ranges_ = 0;
+        if(intensities_ != 0)
             delete[] intensities_;
-        intensities_ = NULL;
+        intensities_ = 0;
     }
     ranges_length_ = 0;
     intensities_length_ = 0;
@@ -345,7 +345,7 @@
         return;
 
     // If no data yet, allocate new
-    if(ranges_ == NULL)
+    if(ranges_ == 0)
     {
         try
         {
@@ -378,7 +378,7 @@
     if(include_intensities)
     {
         // If no data yet, allocate new
-        if(intensities_ == NULL)
+        if(intensities_ == 0)
         {
             try
             {
@@ -408,12 +408,12 @@
         }
         // Else data is already allocated to the right length, so do nothing
     }
-    else if(intensities_ != NULL)
+    else if(intensities_ != 0)
     {
         // If not told to allocate space for intensity data and it exists,
         // remove it
         delete[] intensities_;
-        intensities_ = NULL;
+        intensities_ = 0;
         intensities_length_ = 0;
     }
 }
@@ -421,7 +421,7 @@
 
 void ScanData::write_range_(unsigned int index, uint32_t value)
 {
-    if(ranges_ != NULL)
+    if(ranges_ != 0)
     {
         if(index >= ranges_length_)
             throw IndexError();
@@ -434,7 +434,7 @@
 
 void ScanData::write_intensity_(unsigned int index, uint32_t value)
 {
-    if(intensities_ != NULL)
+    if(intensities_ != 0)
     {
         if(index >= intensities_length_)
             throw IndexError();

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-11 00:49:53 UTC (rev 511)
@@ -60,7 +60,7 @@
         /// This constructor uses a provided data buffer rather than allocating
         /// automatically.
         ///
-        /// If the intensity pointer is NULL, no data will be provided of that
+        /// If the intensity pointer is 0, no data will be provided of that
         /// type.
         ///
         /// @param ranges_buffer A pointer to a data area to store range data
@@ -75,7 +75,7 @@
         /// only for copy constructor and similar.
         ScanData(uint32_t* const ranges_buffer,
                 unsigned int ranges_length,
-                uint32_t* const intensities_buffer=NULL,
+                uint32_t* const intensities_buffer=0,
                 unsigned int intensities_length=0);
         /// This copy constructor performs a deep copy of present data.
         ScanData(ScanData const& rhs);
@@ -122,7 +122,7 @@
         /// buffers. Instead, it will copy the data into its own buffers.
         /// If the lhs has provided buffers, it is the caller's responsibility
         /// to ensure they will be big enough to receive the data from the rhs,
-        /// except in the case of NULL buffers (no data will be copied for NULL
+        /// except in the case of 0 buffers (no data will be copied for 0
         /// buffers).
         ScanData& operator=(ScanData const& rhs);
         /** @brief Subscript operator.

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-11 00:49:53 UTC (rev 511)
@@ -48,9 +48,9 @@
 namespace hokuyo_aist
 {
 
-// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
+// SCIP1: 66 bytes (64 bytes of data + line feed + 0)
 unsigned int const SCIP1_LINE_LENGTH = 66;
-// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
+// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + 0)
 unsigned int const SCIP2_LINE_LENGTH = 67;
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -274,28 +274,30 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 Sensor::Sensor()
-    : port_(NULL), err_output_(std::cerr), scip_version_(2), verbose_(false),
+    : port_(0), err_output_(std::cerr), scip_version_(2), verbose_(false),
     enable_checksum_workaround_(false), ignore_unknowns_(false),
     multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0),
     resolution_(0.0), first_step_(0), last_step_(0), front_step_(0),
-    max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0)
+    max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0),
+    wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0)
 {
 }
 
 
 Sensor::Sensor(std::ostream& err_output)
-    : port_(NULL), err_output_(err_output), scip_version_(2), verbose_(false),
+    : port_(0), err_output_(err_output), scip_version_(2), verbose_(false),
     enable_checksum_workaround_(false), ignore_unknowns_(false),
     multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0),
     resolution_(0.0), first_step_(0), last_step_(0), front_step_(0),
-    max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0)
+    max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0),
+    wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0)
 {
 }
 
 
 Sensor::~Sensor()
 {
-    if(port_ != NULL)
+    if(port_ != 0)
      delete port_;
 }
 
@@ -425,13 +427,13 @@
     if(verbose_)
         err_output_ << "Sensor::" << __func__ << "() Closing connection.\n";
     delete port_;
-    port_ = NULL;
+    port_ = 0;
 }
 
 
 bool Sensor::is_open() const
 {
-    if(port_ != NULL)
+    if(port_ != 0)
         return port_->IsOpen();
     return false;
 }
@@ -446,14 +448,14 @@
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            send_command_("L", "1", 1, NULL);
+            send_command_("L", "1", 1, 0);
         }
         else
         {
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            send_command_("L", "0", 1, NULL);
+            send_command_("L", "0", 1, 0);
         }
         skip_lines_(1);
     }
@@ -464,14 +466,14 @@
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            send_command_("BM", NULL, 0, "02");
+            send_command_("BM", 0, 0, "02");
         }
         else
         {
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            send_command_("QT", NULL, 0, "02");
+            send_command_("QT", 0, 0, "02");
         }
         skip_lines_(1);
     }
@@ -500,7 +502,7 @@
     if(scip_version_ == 1)
     {
         // Send the command to change baud rate
-        send_command_("S", newBaud, 13, NULL);
+        send_command_("S", newBaud, 13, 0);
         skip_lines_(1);
         // Change the port's baud rate
         reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud);
@@ -540,7 +542,7 @@
         err_output_ << "Sensor::" << __func__ <<
             "() Setting IP information to $I" << params.str() << '\n';
     }
-    int status = send_command_(&command[0], params.str().c_str(), 48, NULL);
+    int status = send_command_(&command[0], params.str().c_str(), 48, 0);
     // Skip the extra line feed
     skip_lines_(1);
     if(status != 0)
@@ -559,7 +561,7 @@
             err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
         }
-        send_command_("RS", NULL, 0, NULL);
+        send_command_("RS", 0, 0, 0);
         skip_lines_(1);
     }
     else
@@ -576,7 +578,7 @@
         if(verbose_)
             err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
-        send_command_("RS", NULL, 0, NULL);
+        send_command_("RS", 0, 0, 0);
         skip_lines_(1);
     }
     else
@@ -664,7 +666,7 @@
 
 void Sensor::get_sensor_info(SensorInfo* info)
 {
-    if(info == NULL)
+    if(info == 0)
         throw NoDestinationError();
 
     if(scip_version_ == 1)
@@ -680,7 +682,7 @@
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
 
-        send_command_("V", NULL, 0, NULL);
+        send_command_("V", 0, 0, 0);
         // Get the vendor info line
         read_line_(buffer);
         info->vendor = &buffer[5]; // Chop off the "VEND:" tag
@@ -722,7 +724,7 @@
             // POSIX we get to do it the hard way.
             // Start by finding the first (
             char const* valueStart;
-            if((valueStart = strchr(info->firmware.c_str(), '(')) == NULL)
+            if((valueStart = strchr(info->firmware.c_str(), '(')) == 0)
             {
                 // No bracket? Crud. Fail and use the hard-coded values from
                 // the manual.
@@ -796,17 +798,17 @@
 
         // We need to send three commands to get all the info we want: VV, PP
         // and II
-        send_command_("VV", NULL, 0, NULL);
+        send_command_("VV", 0, 0, 0);
         while(read_line_with_check_(buffer, -1, true) != 0)
             process_vv_line_(buffer, info);
 
         // Next up, PP
-        send_command_("PP", NULL, 0, NULL);
+        send_command_("PP", 0, 0, 0);
         while(read_line_with_check_(buffer, -1, true) != 0)
             process_pp_line_(buffer, info);
 
         // Command II: Revenge of the Commands.
-        send_command_("II", NULL, 0, NULL);
+        send_command_("II", 0, 0, 0);
         while(read_line_with_check_(buffer, -1, true) != 0)
             process_ii_line_(buffer, info);
 
@@ -839,11 +841,11 @@
         if(verbose_)
             err_output_ << "Sensor::" << __func__ <<
                 "() Retrieving time from laser.\n";
-        send_command_("TM", "0", 1, NULL);
-        send_command_("TM", "1", 1, NULL);
+        send_command_("TM", "0", 1, 0);
+        send_command_("TM", "1", 1, 0);
         char buffer[7];
         read_line_with_check_(buffer, 6);
-        send_command_("TM", "2", 1, NULL);
+        send_command_("TM", "2", 1, 0);
         skip_lines_(1);
         // We need to decode the time value that's in the buffer
         return decode_4_byte_value(buffer);
@@ -855,138 +857,92 @@
 }
 
 
-unsigned int Sensor::calibrate_time(unsigned int samples)
+long long Sensor::calibrate_time(unsigned int skew_sleep_time,
+        unsigned int samples)
 {
+    if(verbose_)
+        err_output_ << "Entering timing mode.\n";
     enter_timing_mode_();
 
-    std::ofstream df("times.txt", std::ofstream::app);
-
-    // Measure the latency with 1000 plain TM commands
-    std::vector<unsigned long long> latencies;
+    // From A. Carballo, Y. Hara, H. Kawata, T.
+    // Yoshida, A. Ohya, S. Yuta, “Time synchronisation
+    // between SOKUIKI sensor and host computer using
+    // timestamps”, Proceedings of the JSME Conference on
+    // Robotics and Mechatronics ROBOMEC 2007, Akita,
+    // Japan, 2007, Paper 1P1-K05.
+    //
+    // Calibration is performed by calculating the offset between the laser's
+    // clock and the computer's clock. The algorithm is:
+    // Offset = Comp time before -
+    //      (Laser time - (Comp time before - Comp time after) / 2)
+    // This is calculated samples times, and the median taken.
+    if(verbose_)
+        err_output_ << "Gathering " << samples << " offset values.\n";
+    std::vector<long long> offsets;
     for(unsigned int ii = 0; ii < samples; ii++)
     {
-        unsigned long long start_time(get_computer_time_());
-        get_timing_mode_time_("");
-        unsigned long long end_time(get_computer_time_());
-        latencies.push_back(end_time - start_time);
+        unsigned long long end_time(0);
+        unsigned long long start_time = get_computer_time_();
+        unsigned long long laser_time =
+            wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6;
+        offsets.push_back(start_time -
+                (laser_time - (end_time - start_time) / 2));
     }
-    // Calculate the median two-way latency
-    unsigned long long two_way_latency = median(latencies);
-    df << two_way_latency << '\t';
+    // Calculate the median offset
+    time_offset_ = median(offsets);
+    if(verbose_)
+        err_output_ << "Calculated offset is " << time_offset_ << '\n';
 
-    // Measure the latency with 1000 TM commands + 32 bytes
-    std::string bytes = "12345678901234567890123456789012";
-    latencies.clear();
-    for(unsigned int ii = 0; ii < samples; ii++)
+    if(skew_sleep_time > 0)
     {
-        unsigned long long start_time(get_computer_time_());
-        get_timing_mode_time_(bytes);
-        unsigned long long end_time(get_computer_time_());
-        latencies.push_back(end_time - start_time);
-    }
-    // Calculate the median two-way latency
-    unsigned long long latency_32bytes = median(latencies);
-    df << latency_32bytes << '\t';
-    // Subtract the no-data latency to remove the command transmission time
-    latency_32bytes -= two_way_latency;
-    df << latency_32bytes << '\t';
-    // Divide by 32 to get the per-byte transmission time
-    unsigned long long one_byte_latency32 = latency_32bytes / 32;
-    df << one_byte_latency32 << '\t';
+        // Sleep, then do it again to approximate a skew line
+        struct timespec sleep_time = {0, 0};
+        sleep_time.tv_sec = skew_sleep_time;
+        if(verbose_)
+            err_output_ << "Sleeping for " << skew_sleep_time << "s.\n";
+        nanosleep(&sleep_time, NULL);
 
-    // Measure the latency with 1000 TM commands + 64 bytes
-    bytes = "1234567890123456789012345678901234567890123456789012345678901234";
-    latencies.clear();
-    for(unsigned int ii = 0; ii < samples; ii++)
-    {
-        unsigned long long start_time(get_computer_time_());
-        get_timing_mode_time_(bytes);
-        unsigned long long end_time(get_computer_time_());
-        latencies.push_back(end_time - start_time);
+        if(verbose_)
+            err_output_ << "Gathering " << samples << " offset values.\n";
+        offsets.clear();
+        for(unsigned int ii = 0; ii < samples; ii++)
+        {
+            unsigned long long end_time(0);
+            unsigned long long start_time = get_computer_time_();
+            unsigned long long laser_time =
+                wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6;
+            offsets.push_back(start_time -
+                    (laser_time - (end_time - start_time) / 2));
+        }
+        // Calculate the median offset
+        long long offset2 = median(offsets);
+        if(verbose_)
+        {
+            err_output_ << "Calculated second offset is " << offset2 <<
+                '\n';
+        }
+
+        // Approximate the line slope as (offset2 - offset1) / sleep_time
+        time_skew_alpha_ = (offset2 - time_offset_) /
+            static_cast<double>(skew_sleep_time * 1e9);
+        if(verbose_)
+        {
+            err_output_ << "Calculated alpha is " << time_skew_alpha_ << '\n';
+        }
     }
-    // Calculate the median two-way latency
-    unsigned long long latency_64bytes = median(latencies);
-    df << latency_64bytes << '\t';
-    // Subtract the no-data latency to remove the command transmission time
-    latency_64bytes -= two_way_latency;
-    df << latency_64bytes << '\t';
-    // Divide by 32 to get the per-byte transmission time
-    unsigned long long one_byte_latency64 = latency_64bytes / 64;
-    df << one_byte_latency64 << '\t';
 
-    // Get the time from the laser
-    // To get the most accurate result, we cannot do any processing while
-    // receiving. We want to know the time that reception finished as exactly
-    // as possible. To achieve this, we do not use send_command_, but instead
-    // send the command manually and receive the entire expected reply at once,
-    // then check/decode it later.
-    char response[17];
-    if(port_->Write("TM1\n", 4) < 4)
-        throw WriteError(19);
-    unsigned int line_length = port_->ReadLine(response, 16);
-    unsigned long long end_time(get_computer_time_());
-    df << end_time << '\t';
-    // Process the response to confirm it is correct
-    if(line_length < 0)
-        throw ReadError(0);
-    else if(line_length == 0)
-        throw ReadError(1);
-    else if(line_length < 15)
-        throw LineLengthError(line_length, 15);
-    response[line_length - 1] = '\0';
-    if(response[0] != 'T' || response[1] != 'M' || response[2] != '1')
-        throw CommandEchoError("TM", response);
-    response[7] = '\0';
-    if(response[4] != '0' || response[5] != '0' || response[6] != 'P')
-        throw ResponseError(response, "TM");
-    // Check the checksum on the time stamp is accurate
-    confirm_checksum_(&response[8], 4, response[12]);
-    // Decode the time stamp
-    unsigned int timestamp =
-        wrap_timestamp_(decode_4_byte_value(&response[8]));
-    df << timestamp << '\t';
-    // The laser had to send 15 bytes to send back the time stamp. This means
-    // that there was approximately a 15 byte delay between the time stamp's
-    // occurance in real time and when we finished receiving the data.
-    // The offset from computer time to laser time is:
-    // (computer time - comms time) - laser time
-    // Not forgetting that the computer times need to be converted to
-    // milliseconds.
-    time_offset_ = ((end_time - (one_byte_latency64 * 15)) / 1e6) - timestamp;
-    df << time_offset_ << '\n';
-
     // All done.
+    if(verbose_)
+        err_output_ << "Leaving timing mode.\n";
     leave_timing_mode_();
-    df.close();
     return time_offset_;
 }
 
 
-unsigned int Sensor::wrap_timestamp_(unsigned int timestamp)
-{
-    unsigned int result;
-    if(timestamp < last_timestamp_)
-    {
-        wrap_count_++;
-        result = timestamp + wrap_count_ * 0x1000; // 24-bit value + 1
-    }
-    else
-        result = timestamp;
-    last_timestamp_ = timestamp;
-    return result;
-}
-
-
-unsigned int Sensor::offset_timestamp_(unsigned int timestamp)
-{
-    return timestamp + time_offset_;
-}
-
-
 unsigned int Sensor::get_ranges(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
 
     char buffer[11];
@@ -1013,7 +969,7 @@
         number_to_string(start_step, buffer, 3);
         number_to_string(end_step, &buffer[3], 3);
         number_to_string(cluster_count, &buffer[6], 2);
-        send_command_("G", buffer, 8, NULL);
+        send_command_("G", buffer, 8, 0);
         // In SCIP1 mode we're going to get back 2-byte data
         read_2_byte_range_data_(data, num_steps);
     }
@@ -1025,15 +981,16 @@
         number_to_string(end_step, &buffer[4], 4);
         number_to_string(cluster_count, &buffer[8], 2);
         if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
-            send_command_("HD", buffer, 10, NULL);
+            send_command_("HD", buffer, 10, 0);
         else
-            send_command_("GD", buffer, 10, NULL);
+            send_command_("GD", buffer, 10, 0);
         // There will be a timestamp before the data (if there is data)
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
         if(read_line_with_check_(buffer) == 0)
             throw NoDataError();
-        data->laser_time_ = decode_4_byte_value(buffer);
+        data->laser_time_ = decode_4_byte_value(buffer) +
+            step_to_time_offset_(start_step);
         data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the GD command
@@ -1049,7 +1006,7 @@
 unsigned int Sensor::get_ranges_by_angle(ScanData* data, double start_angle,
         double end_angle, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDataError();
 
     // Calculate the given angles in steps, rounding towards front_step_
@@ -1078,7 +1035,7 @@
 unsigned int Sensor::get_ranges_intensities(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
 
     char buffer[11];
@@ -1109,15 +1066,16 @@
     number_to_string(end_step, &buffer[4], 4);
     number_to_string(cluster_count, &buffer[8], 2);
     if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
-        send_command_("HE", buffer, 10, NULL);
+        send_command_("HE", buffer, 10, 0);
     else
-        send_command_("GE", buffer, 10, NULL);
+        send_command_("GE", buffer, 10, 0);
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer) +
+        step_to_time_offset_(start_step);
     data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the GE command
@@ -1130,7 +1088,7 @@
 unsigned int Sensor::get_ranges_intensities_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDataError();
 
     // Calculate the given angles in steps, rounding towards front_step_
@@ -1159,7 +1117,7 @@
 unsigned int Sensor::get_new_ranges(ScanData* data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
 
     if(scip_version_ == 1)
@@ -1197,7 +1155,7 @@
         command[0] = 'M';
     command[1] = 'D';
     command[2] = '\0';
-    send_command_(command, buffer, 13, NULL);
+    send_command_(command, buffer, 13, 0);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
@@ -1234,7 +1192,8 @@
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer) +
+        step_to_time_offset_(start_step);
     data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the MD command
@@ -1247,7 +1206,7 @@
 unsigned int Sensor::get_new_ranges_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
     if(scip_version_ == 1)
         throw UnsupportedError(16);
@@ -1278,7 +1237,7 @@
 unsigned int Sensor::get_new_ranges_intensities(ScanData* data,
         int start_step, int end_step, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
 
     if(scip_version_ == 1)
@@ -1317,7 +1276,7 @@
         command[0] = 'M';
     command[1] = 'E';
     command[2] = '\0';
-    send_command_(command, buffer, 13, NULL);
+    send_command_(command, buffer, 13, 0);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
@@ -1354,7 +1313,8 @@
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer);
+    data->laser_time_ = decode_4_byte_value(buffer) +
+        step_to_time_offset_(start_step);
     data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the ME command
@@ -1367,7 +1327,7 @@
 unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData* data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == NULL)
+    if(data == 0)
         throw NoDestinationError();
     if(scip_version_ == 1)
         throw UnsupportedError(17);
@@ -1438,12 +1398,12 @@
 
 
 // If expected_length is not -1, it should include the terminating line feed but
-// not the NULL (although the buffer still has to include this).
+// not the 0 (although the buffer still has to include this).
 // If expected_length is -1, this function expects buffer to be a certain length
 // to allow up to the maximum line length to be read. See SCIP1_LINE_LENGTH and
 // SCIP2_LINE_LENGTH.
-// The line feed that terminates a line will be replaced with a NULL.
-// The return value is the number of bytes received, not including the NULL
+// The line feed that terminates a line will be replaced with a 0.
+// The return value is the number of bytes received, not including the 0
 // byte or the line feed.
 int Sensor::read_line_(char* buffer, int expected_length)
 {
@@ -1463,7 +1423,7 @@
             throw ReadError(0);
         else if(linelength == 0)
             throw ReadError(1);
-        // Replace the line feed with a NULL
+        // Replace the line feed with a 0
         buffer[linelength - 1] = '\0';
     }
     else
@@ -1473,14 +1433,14 @@
             err_output_ << "Sensor::" << __func__ <<
                 "() Reading exactly " << expected_length << " bytes.\n";
         }
-        // expected_length+1 for the NULL
+        // expected_length+1 for the 0
         if((linelength = port_->ReadLine(buffer, expected_length + 1)) < 0)
             throw ReadError(0);
         else if(linelength == 0)
             throw ReadError(1);
         else if(linelength < expected_length)
             throw LineLengthError(linelength, expected_length);
-        // Replace the line feed with a NULL
+        // Replace the line feed with a 0
         buffer[linelength - 1] = '\0';
     }
 
@@ -1552,7 +1512,7 @@
         {
             // Here comes the UTM-30LX workaround
             char* hasComment = strstr(buffer, "<-");
-            if(hasComment != NULL)
+            if(hasComment != 0)
             {
                 int newBytesToConsider = hasComment - buffer;
                 if(verbose_)
@@ -1602,7 +1562,7 @@
 // command and parameters sent are correct, and that the returned status code
 // is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two
 // bytes of extra_ok (for SCIP2).
-// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string
+// cmd must be a 1 byte string for SCIP1 and a 2-byte 0-terminated string
 // for SCIP2.
 // If param_length is 0, no parameters will be sent or expected in the reply.
 // extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
@@ -1672,7 +1632,7 @@
         }
         if(response[statusIndex] != '0')
         {
-            if(extra_ok != NULL)
+            if(extra_ok != 0)
             {
                 if(response[statusIndex] != extra_ok[0])
                 {
@@ -1737,7 +1697,7 @@
         if(!(response[0] == '0' && response[1] == '0') &&
             !(response[0] == '9' && response[1] == '9'))
         {
-            if(extra_ok != NULL)
+            if(extra_ok != 0)
             {
                 if(response[0] != extra_ok[0] || response[1] != extra_ok[1])
                 {
@@ -1768,25 +1728,54 @@
 /// Puts the laser into the timing mode.
 void Sensor::enter_timing_mode_()
 {
-    send_command_("TM", "0", 1, NULL);
+    send_command_("TM", "0", 1, 0);
+    skip_lines_(1);
 }
 
 
+/// Take the laser out of timing mode.
+void Sensor::leave_timing_mode_()
+{
+    send_command_("TM", "2", 1, 0);
+    skip_lines_(1);
+}
+
+
 /// Get the timestamp from the laser.
-unsigned int Sensor::get_timing_mode_time_(std::string const& data)
+/// If @ref reception_time is not 0, it will be filled with the time at
+/// which data reception was completed.
+unsigned int Sensor::get_timing_mode_time_(unsigned long long* reception_time)
 {
-    std::string command = "TM1";
-    if(data != "")
-    {
-        send_command_((command + data).c_str(), NULL,
-                command.size() + data.size(), NULL);
-    }
-    else
-        send_command_("TM", "1", 1, NULL);
-
-    char buffer[7];
-    read_line_with_check_(buffer, 6);
-    return decode_4_byte_value(buffer);
+    // To get the most accurate result, we cannot do any processing while
+    // receiving. We want to know the time that reception finished as exactly
+    // as possible. To achieve this, we do not use send_command_, but instead
+    // send the command manually and receive the entire expected reply at once,
+    // then check/decode it later.
+    char response[17];
+    if(port_->Write("TM1\n", 4) < 4)
+        throw WriteError(19);
+    unsigned int line_length = port_->Read(response, 16);
+    if(reception_time)
+        *reception_time = get_computer_time_();
+    // Process the response to confirm it is correct
+    if(line_length < 0)
+        throw ReadError(0);
+    else if(line_length == 0)
+        throw ReadError(1);
+    else if(line_length < 15)
+        throw LineLengthError(line_length, 15);
+    response[line_length - 1] = '\0';
+    if(response[0] != 'T' || response[1] != 'M' || response[2] != '1')
+        throw CommandEchoError("TM", response);
+    response[7] = '\0';
+    if(response[4] != '0' || response[5] != '0' || response[6] != 'P')
+        throw ResponseError(response, "TM");
+    // Check the checksum on the time stamp is accurate
+    confirm_checksum_(&response[8], 4, response[12]);
+    // Decode the time stamp
+    unsigned int timestamp =
+        decode_4_byte_value(&response[8]);
+    return timestamp;
 }
 
 
@@ -1799,37 +1788,59 @@
     return ts.tv_sec * 1e9 + ts.tv_nsec;
 #else
     struct timeval tv;
-    gettimeofday(&tv, NULL);
+    gettimeofday(&tv, 0);
     return tv.tv_sec * 1e9 + tv.tv_usec * 1e3;
 #endif
 }
 
 
-/// Take the laser out of timing mode.
-void Sensor::leave_timing_mode_()
+/// timestamp must be in milliseconds. The result is in milliseconds.
+unsigned int Sensor::wrap_timestamp_(unsigned int timestamp)
 {
-    send_command_("TM", "2", 1, NULL);
-    skip_lines_(1);
+    if(timestamp < last_timestamp_)
+    {
+        wrap_count_++;
+    }
+    last_timestamp_ = timestamp;
+
+    return timestamp + wrap_count_ * 0x01000000; // 24-bit value + 1
 }
 
 
+/// timestamp must be in milliseconds. The result is in nanoseconds.
+unsigned long long Sensor::offset_timestamp_(unsigned int timestamp)
+{
+    return ((1 - time_drift_rate_) * timestamp * 1e6 + time_offset_) /
+        (1 - time_skew_alpha_);
+}
+
+
+unsigned int Sensor::step_to_time_offset_(int start_step)
+{
+    if(start_step < 0)
+        return first_step_ * time_resolution_;
+    else
+        return start_step * time_resolution_;
+}
+
+
 /// Search a string for the laser's model.
 void Sensor::find_model_(char const* buffer)
 {
-    if(strstr(buffer, "URG-04LX") != NULL)
+    if(strstr(buffer, "URG-04LX") != 0)
         model_ = MODEL_URG04LX;
-    else if(strstr(buffer, "UBG-04LX") != NULL)
+    else if(strstr(buffer, "UBG-04LX") != 0)
         model_ = MODEL_UBG04LXF01;
-    else if(strstr(buffer, "UHG-08LX") != NULL)
+    else if(strstr(buffer, "UHG-08LX") != 0)
         model_ = MODEL_UHG08LX;
-    else if(strstr(buffer, "UTM-30LX") != NULL)
+    else if(strstr(buffer, "UTM-30LX") != 0)
     {
         model_ = MODEL_UTM30LX;
         // Also enable the work around for a checksum problem in this
         // model.
         enable_checksum_workaround_ = true;
     }
-    else if(strstr(buffer, "UXM-30LX") != NULL)
+    else if(strstr(buffer, "UXM-30LX") != 0)
         model_ = MODEL_UXM30LXE;
     else
         model_ = MODEL_UNKNOWN;
@@ -1847,7 +1858,7 @@
     // Try SCIP version 2 first by sending an info command
     try
     {
-        send_command_("VV", NULL, 0, NULL);
+        send_command_("VV", 0, 0, 0);
     }
     catch(BaseError)
     {
@@ -1867,7 +1878,7 @@
         port_->Flush();
         try
         {
-            send_command_("V", NULL, 0, NULL);
+            send_command_("V", 0, 0, 0);
         }
         catch(BaseError)
         {
@@ -1887,7 +1898,7 @@
         // different firmware version format, that doesn't matter because they
         // don't support SCIP v1 and so shouldn't get to this point anyway - if
         // they do, it's an uncaught error.
-        int majorVer = strtol(&buffer[5], NULL, 10);
+        int majorVer = strtol(&buffer[5], 0, 10);
         if(errno == ERANGE)
             throw FirmwareError();
         if(verbose_)
@@ -1921,7 +1932,7 @@
             // first byte as the command and the other 6 as parameters.
             try
             {
-                send_command_("S", "CIP2.0", 6, NULL);
+                send_command_("S", "CIP2.0", 6, 0);
             }
             catch(BaseError)
             {
@@ -2032,7 +2043,7 @@
      * arrived yet, so don't know what to look for.
     else if(strncmp(buffer, "", 4) == 0)
     {
-        if(strstr(buffer, "CCW") != NULL)
+        if(strstr(buffer, "CCW") != 0)
             info->rot_dir = COUNTERCLOCKWISE;
         else
             info->rot_dir = CLOCKWISE;
@@ -2064,6 +2075,7 @@
                 throw ParseError(buffer, "Motor speed");
             }
             info->speed_level = 0;
+            time_resolution_ = info->speed;
         }
         else
         {
@@ -2074,6 +2086,7 @@
             {
                 throw ParseError(buffer, "Motor speed");
             }
+            time_resolution_ = info->speed;
         }
     }
     else if(strncmp(buffer, "MESM", 4) == 0)

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-11 00:49:53 UTC (rev 511)
@@ -218,12 +218,62 @@
         approximately every 9.5 hours. This class will detect wrapped time
         stamps and adjust them accordingly.
 
+        @param skew_sleep_time Whether to approximate the laser clock skew.
+        This requires sleeping for the specified time, then calibrating again.
+        The results of the two calibrations are used to approximate the clock
+        skew as a straight line between the two points. This is very
+        approximate; in order to overcome noise in the signal you will need to
+        calibrate over a period of up to several minutes. Set this to 0 to not
+        approximate the skew.
         @param samples The number of samples to use in calculating latencies.
-        @return The calculated offset in milliseconds.
+        @return The calculated offset in nanoseconds.
         */
 
-        unsigned int calibrate_time(unsigned int samples=10);
+        long long calibrate_time(unsigned int skew_sleep_time=0,
+                unsigned int samples=10);
 
+        /// Retrieve the calculated time offset (0 if not calibrated).
+        long long time_offset() const { return time_offset_; }
+
+        /// Set the time offset (if the calculated value is bad).
+        void set_time_offset(long long time_offset)
+            { time_offset_ = time_offset; }
+        /// Retrieve the current clock drift rate (0 if not set).
+        float drift_rate() const { return time_drift_rate_; }
+        /** Set the current clock drift rate.
+
+        The drift rate is used when correcting laser time stamps to computer
+        time. It is factored into the equation as:
+            Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha)
+        where Tc is the computer time and Tl is the laser time. See @ref
+        set_skew_alpha for the values of alpha and beta.
+
+        If the drift rate is zero, it means the laser's clock does not drift.
+        This is extremely unlikely, but the laser's drift may not matter for
+        your application.
+
+        Drift should usually be provided by the manufacturer. If it is not,
+        you can calculate it by calibrating the laser many times over a long
+        period and looking at the change in the calculated offset. */
+        void set_drift_rate(float drift_rate)
+            { time_drift_rate_ = drift_rate; }
+
+        /// Get the calculated skew line slope (default: 0).
+        float skew_alpha() const { return time_skew_alpha_; }
+        /** Set a skew line slope value.
+
+        The skew is used when correcting laser time stamps to computer time.
+        It is factored into the equation as:
+            Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha)
+        where Tc is the computer time and Tl is the laser time. See @ref
+        drift_rate for the value of the drift. Beta, the line's y crossing, is
+        assumed to be 0 since we always use a two-point line fit instead of
+        something like least squares.
+
+        The effect of this is to cancel out the skew caused by the different
+        frequencies of the computer clock and the laser clock. */
+        void set_skew_alpha(float alpha) { time_skew_alpha_ = alpha; }
+
         /** @brief Get the latest scan data from the scanner.
 
         This function requires a pointer to a @ref ScanData object. It will
@@ -444,8 +494,20 @@
         double min_angle_, max_angle_, resolution_;
         int first_step_, last_step_, front_step_;
         unsigned int max_range_;
-        unsigned int time_offset_;
-        unsigned int last_timestamp_, wrap_count_;
+        /// The time between two points in a scan, in milliseconds.
+        unsigned int time_resolution_;
+        /// The offset from the laser's clock to the computer's clock in
+        /// nanoseconds.
+        long long time_offset_;
+        /// The previous received timestamp from the laser, in laser time and
+        /// in milliseconds.
+        unsigned int last_timestamp_;
+        /// The number of times the laser's clock has wrapped.
+        unsigned int wrap_count_;
+        /// The drift rate of the laser's clock
+        float time_drift_rate_;
+        /// The clock skew alpha value.
+        float time_skew_alpha_;
 
         void clear_read_buffer_();
         int read_line_(char* buffer, int expected_length = -1);
@@ -455,12 +517,22 @@
         int send_command_(char const* cmd, char const* param, int param_length,
                 char const* extra_ok);
 
+    public:
         void enter_timing_mode_();
-        unsigned int get_timing_mode_time_(std::string const& data);
+        void leave_timing_mode_();
+        /// Get the laser time in milliseconds.
+        unsigned int get_timing_mode_time_(unsigned long long* reception_time=0);
+        /// Get the computer time in nanoseconds.
         unsigned long long get_computer_time_();
-        void leave_timing_mode_();
+        /// Adjust a wrapped laser timestamp, in milliseconds.
         unsigned int wrap_timestamp_(unsigned int timestamp);
-        unsigned int offset_timestamp_(unsigned int timestamp);
+        /// Offset a laser timestamp, in milliseconds, into computer time, in
+        /// nanoseconds.
+        unsigned long long offset_timestamp_(unsigned int timestamp);
+    private:
+        /// Convert a step value into its time offset from the start of a scan,
+        /// in milliseconds.
+        unsigned int step_to_time_offset_(int start_step);
 
         void find_model_(char const* buffer);
         void get_and_set_scip_version_();

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-11 00:49:53 UTC (rev 511)
@@ -32,7 +32,7 @@
     : min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
     front_step(0), standard_speed(0), rot_dir(COUNTERCLOCKWISE), power(false),
     speed(0), speed_level(0), baud(0), time(0), min_angle(0.0), max_angle(0.0),
-    resolution(0.0), scanable_steps(0), max_step(0),
+    resolution(0.0), time_resolution(0), scanable_steps(0), max_step(0),
     detected_model(MODEL_UNKNOWN)
 {
 }
@@ -113,6 +113,7 @@
     max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) *
         resolution;
     scanable_steps = last_step - first_step + 1;
+    time_resolution = 60000 / speed;
 }
 
 

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-11 00:49:53 UTC (rev 511)
@@ -196,6 +196,8 @@
         double max_angle;
         /// Angle between two scan points (radians).
         double resolution;
+        /// Time between two scan points (milliseconds).
+        unsigned int time_resolution;
         /// Total number of steps in a full scan (lastStep - firstStep).
         unsigned int scanable_steps;
         /// Absolute maximum commandable step.

Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-11 00:49:53 UTC (rev 511)
@@ -130,6 +130,10 @@
         {
             std::cerr << "Failed to change baud rate: " << e.what() << '\n';
         }
+        catch(...)
+        {
+            std::cerr << "Failed to change baud rate\n";
+        }
         // Set the motor speed
         try
         {

Modified: gearbox/trunk/src/hokuyo_aist/utils.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/utils.h	2010-08-08 04:31:17 UTC (rev 510)
+++ gearbox/trunk/src/hokuyo_aist/utils.h	2010-08-11 00:49:53 UTC (rev 511)
@@ -24,7 +24,10 @@
 #include <string>
 #include <vector>
 #include <algorithm>
+#include <cassert>
 
+#include <iostream>
+
 #if defined(WIN32)
     typedef unsigned char           uint8_t;
     typedef unsigned int            uint32_t;
@@ -65,6 +68,7 @@
     }
 #endif
 
+
 /// Find the median value of a std::vector.
 template<typename T>
 inline T median(std::vector<T>& v)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 512
          http://gearbox.svn.sourceforge.net/gearbox/?rev=512&view=rev
Author:   gbiggs
Date:     2010-08-16 01:36:43 +0000 (Mon, 16 Aug 2010)

Log Message:
-----------
Changed data pointer to reference

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/test/example.cpp

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-11 00:49:53 UTC (rev 511)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-16 01:36:43 UTC (rev 512)
@@ -939,12 +939,9 @@
 }
 
 
-unsigned int Sensor::get_ranges(ScanData* data, int start_step,
+unsigned int Sensor::get_ranges(ScanData& data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
-
     char buffer[11];
     memset(buffer, 0, sizeof(char) * 11);
 
@@ -989,9 +986,9 @@
         // timestamp back if there was no data.
         if(read_line_with_check_(buffer) == 0)
             throw NoDataError();
-        data->laser_time_ = decode_4_byte_value(buffer) +
+        data.laser_time_ = decode_4_byte_value(buffer) +
             step_to_time_offset_(start_step);
-        data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
+        data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the GD command
         read_3_byte_range_data_(data, num_steps);
@@ -999,16 +996,13 @@
     else
         throw UnknownScipVersionError();
 
-    return data->ranges_length_;
+    return data.ranges_length_;
 }
 
 
-unsigned int Sensor::get_ranges_by_angle(ScanData* data, double start_angle,
+unsigned int Sensor::get_ranges_by_angle(ScanData& data, double start_angle,
         double end_angle, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDataError();
-
     // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
@@ -1032,12 +1026,9 @@
 }
 
 
-unsigned int Sensor::get_ranges_intensities(ScanData* data, int start_step,
+unsigned int Sensor::get_ranges_intensities(ScanData& data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
-
     char buffer[11];
     memset(buffer, 0, sizeof(char) * 11);
 
@@ -1074,23 +1065,20 @@
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer) +
+    data.laser_time_ = decode_4_byte_value(buffer) +
         step_to_time_offset_(start_step);
-    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
+    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the GE command
     read_3_byte_range_data_(data, num_steps);
 
-    return data->ranges_length_;
+    return data.ranges_length_;
 }
 
 
-unsigned int Sensor::get_ranges_intensities_by_angle(ScanData* data,
+unsigned int Sensor::get_ranges_intensities_by_angle(ScanData& data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDataError();
-
     // Calculate the given angles in steps, rounding towards front_step_
     int start_step, end_step;
     start_step = angle_to_step(start_angle);
@@ -1114,12 +1102,9 @@
 }
 
 
-unsigned int Sensor::get_new_ranges(ScanData* data, int start_step,
+unsigned int Sensor::get_new_ranges(ScanData& data, int start_step,
         int end_step, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
-
     if(scip_version_ == 1)
         throw UnsupportedError(16);
     else if(scip_version_ != 2)
@@ -1192,22 +1177,20 @@
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer) +
+    data.laser_time_ = decode_4_byte_value(buffer) +
         step_to_time_offset_(start_step);
-    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
+    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the MD command
     read_3_byte_range_data_(data, num_steps);
 
-    return data->ranges_length_;
+    return data.ranges_length_;
 }
 
 
-unsigned int Sensor::get_new_ranges_by_angle(ScanData* data,
+unsigned int Sensor::get_new_ranges_by_angle(ScanData& data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
     if(scip_version_ == 1)
         throw UnsupportedError(16);
 
@@ -1234,12 +1217,9 @@
 }
 
 
-unsigned int Sensor::get_new_ranges_intensities(ScanData* data,
+unsigned int Sensor::get_new_ranges_intensities(ScanData& data,
         int start_step, int end_step, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
-
     if(scip_version_ == 1)
         throw UnsupportedError(17);
     else if(scip_version_ != 2)
@@ -1313,22 +1293,20 @@
     // timestamp back if there was no data.
     if(read_line_with_check_(buffer) == 0)
         throw NoDataError();
-    data->laser_time_ = decode_4_byte_value(buffer) +
+    data.laser_time_ = decode_4_byte_value(buffer) +
         step_to_time_offset_(start_step);
-    data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_));
+    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the ME command
     read_3_byte_range_and_intensity_data_(data, num_steps);
 
-    return data->ranges_length_;
+    return data.ranges_length_;
 }
 
 
-unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData* data,
+unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData& data,
         double start_angle, double end_angle, unsigned int cluster_count)
 {
-    if(data == 0)
-        throw NoDestinationError();
     if(scip_version_ == 1)
         throw UnsupportedError(17);
 
@@ -2147,7 +2125,7 @@
 }
 
 
-void Sensor::read_2_byte_range_data_(ScanData* data, unsigned int num_steps)
+void Sensor::read_2_byte_range_data_(ScanData& data, unsigned int num_steps)
 {
     if(verbose_)
     {
@@ -2157,9 +2135,9 @@
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
-    data->allocate_data(num_steps);
-    data->model_ = model_;
-    data->error_ = false;
+    data.allocate_data(num_steps);
+    data.model_ = model_;
+    data.error_ = false;
 
     // 2 byte data is easy since it fits neatly in a 64-byte block
     char buffer[SCIP2_LINE_LENGTH];
@@ -2180,7 +2158,7 @@
                 // Line feed in the middle of a data block? Why?
                 throw MisplacedLineFeedError();
             }
-            data->write_range_(current_step, decode_2_byte_value(&buffer[ii]));
+            data.write_range_(current_step, decode_2_byte_value(&buffer[ii]));
         }
         // End of this line. Go around again.
     }
@@ -2193,7 +2171,7 @@
 }
 
 
-void Sensor::read_3_byte_range_data_(ScanData* data, unsigned int num_steps)
+void Sensor::read_3_byte_range_data_(ScanData& data, unsigned int num_steps)
 {
     if(verbose_)
     {
@@ -2209,9 +2187,9 @@
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
-    data->allocate_data(num_steps);
-    data->model_ = model_;
-    data->error_ = false;
+    data.allocate_data(num_steps);
+    data.model_ = model_;
+    data.error_ = false;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
@@ -2231,16 +2209,16 @@
             if(echo_buffer[0] != -1)
             {
                 // Not the first value, so deal with the previous
-                data->write_range_(current_step,
+                data.write_range_(current_step,
                         process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
-                if(data->ranges_)
+                if(data.ranges_)
                 {
-                    if(data->ranges_[current_step] > max_range_)
+                    if(data.ranges_[current_step] > max_range_)
                     {
                         err_output_ << "WARNING: Sensor::" << __func__ <<
                             "() Value at step " << current_step <<
                             " beyond maximum range: " <<
-                            data->ranges_[current_step];
+                            data.ranges_[current_step];
                     }
                 }
                 current_step++;
@@ -2269,17 +2247,17 @@
                 else if(echo_buffer[0] != -1)
                 {
                     // Not the first value, so deal with the previous
-                    data->write_range_(current_step,
+                    data.write_range_(current_step,
                             process_echo_buffer_(echo_buffer,
                                 echo_buf_ind + 1));
-                    if(data->ranges_)
+                    if(data.ranges_)
                     {
-                        if(data->ranges_[current_step] > max_range_)
+                        if(data.ranges_[current_step] > max_range_)
                         {
                             err_output_ << "WARNING: Sensor::" << __func__ <<
                                 "() Value at step " << current_step <<
                                 " beyond maximum range: " <<
-                                data->ranges_[current_step];
+                                data.ranges_[current_step];
                         }
                     }
                     current_step++;
@@ -2337,7 +2315,7 @@
 }
 
 
-void Sensor::read_3_byte_range_and_intensity_data_(ScanData* data,
+void Sensor::read_3_byte_range_and_intensity_data_(ScanData& data,
         unsigned int num_steps)
 {
     if(verbose_)
@@ -2354,8 +2332,8 @@
 
     // This will automatically take care of whether it actually needs to
     // (re)allocate or not.
-    data->allocate_data(num_steps, true);
-    data->model_ = model_;
+    data.allocate_data(num_steps, true);
+    data.model_ = model_;
 
     // 3 byte data is a pain because it crosses the line boundary, it may
     // overlap by 0, 1 or 2 bytes
@@ -2379,7 +2357,7 @@
             if(echo_buffer[0] != -1)
             {
                 assert(nextIsIntensity == true);
-                data->write_intensity_(current_intensity,
+                data.write_intensity_(current_intensity,
                     process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
                 current_intensity++;
             }
@@ -2409,27 +2387,27 @@
                     // Not the first value, so deal with the previous
                     if(nextIsIntensity)
                     {
-                        data->write_intensity_(current_intensity,
+                        data.write_intensity_(current_intensity,
                                 process_echo_buffer_(echo_buffer,
                                     echo_buf_ind + 1));
                     }
                     else
                     {
-                        data->write_range_(current_range,
+                        data.write_range_(current_range,
                                 process_echo_buffer_(echo_buffer,
                                     echo_buf_ind + 1));
                     }
                     echo_buf_ind = 0;
                     echo_buffer[0] = -1;
-                    if(data->ranges_)
+                    if(data.ranges_)
                     {
-                        if(data->ranges_[current_range] > max_range_ &&
+                        if(data.ranges_[current_range] > max_range_ &&
                                 !nextIsIntensity)
                         {
                             err_output_ << "WARNING: Sensor::" << __func__ <<
                                 "() Value at step " << current_range <<
                                 " beyond maximum range: " <<
-                                data->ranges_[current_range] << " (raw bytes: ";
+                                data.ranges_[current_range] << " (raw bytes: ";
                             if(split_count != 0)
                                 err_output_ << split_value[0] << split_value[1] <<
                                     split_value[2] << ")\n";

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-11 00:49:53 UTC (rev 511)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-16 01:36:43 UTC (rev 512)
@@ -296,7 +296,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges(ScanData* data, int start_step = -1,
+        unsigned int get_ranges(ScanData& data, int start_step = -1,
                 int end_step = -1, unsigned int cluster_count = 1);
 
         /** @brief Get the latest scan data from the scanner.
@@ -313,7 +313,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_by_angle(ScanData* data, double start_angle,
+        unsigned int get_ranges_by_angle(ScanData& data, double start_angle,
                 double end_angle, unsigned int cluster_count = 1);
 
         /** @brief Get the latest scan data from the scanner with intensities.
@@ -338,7 +338,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_intensities(ScanData* data,
+        unsigned int get_ranges_intensities(ScanData& data,
                 int start_step = -1, int end_step = -1,
                 unsigned int cluster_count = 1);
 
@@ -356,7 +356,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_ranges_intensities_by_angle(ScanData* data,
+        unsigned int get_ranges_intensities_by_angle(ScanData& data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -388,7 +388,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges(ScanData* data, int start_step = -1,
+        unsigned int get_new_ranges(ScanData& data, int start_step = -1,
                 int end_step = -1, unsigned int cluster_count = 1);
 
         /** @brief Get a new scan from the scanner.
@@ -407,7 +407,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_by_angle(ScanData* data,
+        unsigned int get_new_ranges_by_angle(ScanData& data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -438,7 +438,7 @@
         @param end_step The last step to get ranges from. Set to -1 for the last
         scannable step.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_intensities(ScanData* data,
+        unsigned int get_new_ranges_intensities(ScanData& data,
                 int start_step = -1, int end_step = -1,
                 unsigned int cluster_count = 1);
 
@@ -458,7 +458,7 @@
         single reading. The minimum value from a cluster is returned as the
         range for that cluster.
         @return The number of range readings read into @ref data. */
-        unsigned int get_new_ranges_intensities_by_angle(ScanData* data,
+        unsigned int get_new_ranges_intensities_by_angle(ScanData& data,
                 double start_angle, double end_angle,
                 unsigned int cluster_count = 1);
 
@@ -542,9 +542,9 @@
         void process_ii_line_(char const* buffer, SensorInfo* info);
 
         uint32_t process_echo_buffer_(int const* buffer, int num_echos);
-        void read_2_byte_range_data_(ScanData* data, unsigned int num_steps);
-        void read_3_byte_range_data_(ScanData* data, unsigned int num_steps);
-        void read_3_byte_range_and_intensity_data_(ScanData* data,
+        void read_2_byte_range_data_(ScanData& data, unsigned int num_steps);
+        void read_3_byte_range_data_(ScanData& data, unsigned int num_steps);
+        void read_3_byte_range_and_intensity_data_(ScanData& data,
                 unsigned int num_steps);
 
         int confirm_checksum_(char const* buffer, int length,

Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-11 00:49:53 UTC (rev 511)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-16 01:36:43 UTC (rev 512)
@@ -181,35 +181,35 @@
         {
             // Get all ranges
             if(get_new)
-                laser.get_new_ranges(&data, -1, -1, cluster_count);
+                laser.get_new_ranges(data, -1, -1, cluster_count);
             else if(get_intensities)
-                laser.get_new_ranges_intensities(&data, -1, -1, cluster_count);
+                laser.get_new_ranges_intensities(data, -1, -1, cluster_count);
             else
-                laser.get_ranges(&data, -1, -1, cluster_count);
+                laser.get_ranges(data, -1, -1, cluster_count);
         }
         else if(first_step != -1 || last_step != -1)
         {
             // Get by step
             if(get_new)
-                laser.get_new_ranges(&data, first_step, last_step,
+                laser.get_new_ranges(data, first_step, last_step,
                         cluster_count);
             else if(get_intensities)
-                laser.get_new_ranges_intensities(&data, first_step, last_step,
+                laser.get_new_ranges_intensities(data, first_step, last_step,
                         cluster_count);
             else
-                laser.get_ranges(&data, first_step, last_step, cluster_count);
+                laser.get_ranges(data, first_step, last_step, cluster_count);
         }
         else
         {
             // Get by angle
             if(get_new)
-                laser.get_new_ranges_by_angle(&data, start_angle, end_angle,
+                laser.get_new_ranges_by_angle(data, start_angle, end_angle,
                         cluster_count);
             else if(get_intensities)
-                laser.get_new_ranges_intensities_by_angle(&data, start_angle,
+                laser.get_new_ranges_intensities_by_angle(data, start_angle,
                         end_angle, cluster_count);
             else
-                laser.get_ranges_by_angle(&data, start_angle, end_angle,
+                laser.get_ranges_by_angle(data, start_angle, end_angle,
                         cluster_count);
         }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 513
          http://gearbox.svn.sourceforge.net/gearbox/?rev=513&view=rev
Author:   gbiggs
Date:     2010-08-16 02:09:25 +0000 (Mon, 16 Aug 2010)

Log Message:
-----------
Removed public and private tags

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/scan_data.cpp
    gearbox/trunk/src/hokuyo_aist/scan_data.h
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.h
    gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
    gearbox/trunk/src/hokuyo_aist/sensor_info.h

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp	2010-08-16 02:09:25 UTC (rev 513)
@@ -419,7 +419,7 @@
 }
 
 
-void ScanData::write_range_(unsigned int index, uint32_t value)
+void ScanData::write_range(unsigned int index, uint32_t value)
 {
     if(ranges_ != 0)
     {
@@ -432,7 +432,7 @@
 }
 
 
-void ScanData::write_intensity_(unsigned int index, uint32_t value)
+void ScanData::write_intensity(unsigned int index, uint32_t value)
 {
     if(intensities_ != 0)
     {

Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h	2010-08-16 02:09:25 UTC (rev 513)
@@ -110,7 +110,7 @@
         /** @brief Get the system time stamp of the data in milliseconds.
 
         This value is only available using SCIP version 2). */
-        unsigned int system_time_stamp() const { return system_time_; }
+        unsigned long long system_time_stamp() const { return system_time_; }
         /// Get the model of the laser that produced this scan.
         LaserModel model() const { return model_; }
         /// Check if the buffers are being provided instead of automatic.
@@ -143,14 +143,14 @@
         unsigned int intensities_length_;
         bool error_;
         unsigned int laser_time_;
-        unsigned int system_time_;
+        unsigned long long system_time_;
         LaserModel model_;
         bool buffers_provided_;
 
         void allocate_data(unsigned int length,
                 bool include_intensities = false);
-        void write_range_(unsigned int index, uint32_t value);
-        void write_intensity_(unsigned int index, uint32_t value);
+        void write_range(unsigned int index, uint32_t value);
+        void write_intensity(unsigned int index, uint32_t value);
 }; // class ScanData
 
 } // namespace hokuyo_aist

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-16 02:09:25 UTC (rev 513)
@@ -323,9 +323,9 @@
 
     // Figure out the SCIP version currently in use and switch to a higher one
     // if possible
-    get_and_set_scip_version_();
+    get_and_set_scip_version();
     // Get some values we need for providing default ranges
-    get_defaults_();
+    get_defaults();
 }
 
 
@@ -352,9 +352,9 @@
     {
         // Figure out the SCIP version currently in use and switch to a higher
         // one if possible
-        get_and_set_scip_version_();
+        get_and_set_scip_version();
         // Get some values we need for providing default ranges
-        get_defaults_();
+        get_defaults();
     }
     catch(BaseError)
     {
@@ -375,8 +375,8 @@
                 reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(bauds[ii]);
                 try
                 {
-                    get_and_set_scip_version_();
-                    get_defaults_();
+                    get_and_set_scip_version();
+                    get_defaults();
                     // If the above two functions succeed, break out of the
                     // loop and be happy
                     if(verbose_)
@@ -448,16 +448,16 @@
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            send_command_("L", "1", 1, 0);
+            send_command("L", "1", 1, 0);
         }
         else
         {
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            send_command_("L", "0", 1, 0);
+            send_command("L", "0", 1, 0);
         }
-        skip_lines_(1);
+        skip_lines(1);
     }
     else if(scip_version_ == 2)
     {
@@ -466,16 +466,16 @@
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser on.\n";
-            send_command_("BM", 0, 0, "02");
+            send_command("BM", 0, 0, "02");
         }
         else
         {
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Turning laser off.\n";
-            send_command_("QT", 0, 0, "02");
+            send_command("QT", 0, 0, "02");
         }
-        skip_lines_(1);
+        skip_lines(1);
     }
     else
         throw UnknownScipVersionError();
@@ -502,16 +502,16 @@
     if(scip_version_ == 1)
     {
         // Send the command to change baud rate
-        send_command_("S", newBaud, 13, 0);
-        skip_lines_(1);
+        send_command("S", newBaud, 13, 0);
+        skip_lines(1);
         // Change the port's baud rate
         reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud);
     }
     else if(scip_version_ == 2)
     {
         // Send the command to change baud rate
-        send_command_("SS", newBaud, 6, "03");
-        skip_lines_(1);
+        send_command("SS", newBaud, 6, "03");
+        skip_lines(1);
         // Change the port's baud rate
         reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud);
     }
@@ -542,9 +542,9 @@
         err_output_ << "Sensor::" << __func__ <<
             "() Setting IP information to $I" << params.str() << '\n';
     }
-    int status = send_command_(&command[0], params.str().c_str(), 48, 0);
+    int status = send_command(&command[0], params.str().c_str(), 48, 0);
     // Skip the extra line feed
-    skip_lines_(1);
+    skip_lines(1);
     if(status != 0)
         throw SetIPError();
 }
@@ -561,8 +561,8 @@
             err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
         }
-        send_command_("RS", 0, 0, 0);
-        skip_lines_(1);
+        send_command("RS", 0, 0, 0);
+        skip_lines(1);
     }
     else
         throw UnknownScipVersionError();
@@ -578,8 +578,8 @@
         if(verbose_)
             err_output_ << "Sensor::" << __func__ <<
                 "() Resetting laser.\n";
-        send_command_("RS", 0, 0, 0);
-        skip_lines_(1);
+        send_command("RS", 0, 0, 0);
+        skip_lines(1);
     }
     else
         throw UnknownScipVersionError();
@@ -627,8 +627,8 @@
             }
             number_to_string(speed, buffer, 2);
         }
-        send_command_("CR", buffer, 2, "03");
-        skip_lines_(1);
+        send_command("CR", buffer, 2, "03");
+        skip_lines(1);
     }
     else
         throw UnknownScipVersionError();
@@ -646,7 +646,7 @@
             if(verbose_)
                 err_output_ << "Sensor::" << __func__ <<
                     "() Switching to high sensitivity.\n";
-            send_command_("HS", "1", 1, "02");
+            send_command("HS", "1", 1, "02");
         }
         else
         {
@@ -655,9 +655,9 @@
                 err_output_ << "Sensor::" << __func__ <<
                     "() Switching to normal sensitivity.\n";
             }
-            send_command_("HS", "0", 1, "02");
+            send_command("HS", "0", 1, "02");
         }
-        skip_lines_(1);
+        skip_lines(1);
     }
     else
         throw UnknownScipVersionError();
@@ -677,36 +677,36 @@
                 "() Getting sensor information using SCIP version 1.\n";
         }
 
-        info->set_defaults_();
+        info->set_defaults();
 
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
 
-        send_command_("V", 0, 0, 0);
+        send_command("V", 0, 0, 0);
         // Get the vendor info line
-        read_line_(buffer);
+        read_line(buffer);
         info->vendor = &buffer[5]; // Chop off the "VEND:" tag
         // Get the product info line
-        read_line_(buffer);
+        read_line(buffer);
         info->product = &buffer[5];
         // Only the URG-04LX supports SCIP1
         model_ = MODEL_URG04LX;
         // Get the firmware line
-        read_line_(buffer);
+        read_line(buffer);
         info->firmware = &buffer[5];
         // Get the protocol version line
-        read_line_(buffer);
+        read_line(buffer);
         info->protocol = &buffer[5];
         // Get the serial number
-        read_line_(buffer);
+        read_line(buffer);
         info->serial = &buffer[5];
         // Get either the status line or the end of message
-        read_line_(buffer);
+        read_line(buffer);
         if(buffer[0] != '\0')
         {
             // Got a status line
             info->sensor_diagnostic = &buffer[5];
-            skip_lines_(1);
+            skip_lines(1);
         }
 
         // Check the firmware version major number. If it's >=3 there is
@@ -728,7 +728,7 @@
             {
                 // No bracket? Crud. Fail and use the hard-coded values from
                 // the manual.
-                info->calculate_values_();
+                info->calculate_values();
             }
             // Now put it through sscanf and hope...
             int aperture;
@@ -740,8 +740,8 @@
             {
                 // Didn't get enough values out, assume unknown format and fall
                 // back on the defaults
-                info->set_defaults_();
-                info->calculate_values_();
+                info->set_defaults();
+                info->calculate_values();
                 if(verbose_)
                 {
                     err_output_ << "Retrieved sensor info (hard-coded, not "
@@ -775,7 +775,7 @@
         {
             // We're stuck with hard-coded defaults from the manual (already
             // set earlier).
-            info->calculate_values_();
+            info->calculate_values();
             if(verbose_)
             {
                 err_output_ << "Retrieved sensor info (hard-coded):\n";
@@ -791,30 +791,30 @@
                 "() Getting sensor information using SCIP version 2.\n";
         }
 
-        info->set_defaults_();
+        info->set_defaults();
 
         char buffer[SCIP2_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
 
         // We need to send three commands to get all the info we want: VV, PP
         // and II
-        send_command_("VV", 0, 0, 0);
-        while(read_line_with_check_(buffer, -1, true) != 0)
-            process_vv_line_(buffer, info);
+        send_command("VV", 0, 0, 0);
+        while(read_line_with_check(buffer, -1, true) != 0)
+            process_vv_line(buffer, info);
 
         // Next up, PP
-        send_command_("PP", 0, 0, 0);
-        while(read_line_with_check_(buffer, -1, true) != 0)
-            process_pp_line_(buffer, info);
+        send_command("PP", 0, 0, 0);
+        while(read_line_with_check(buffer, -1, true) != 0)
+            process_pp_line(buffer, info);
 
         // Command II: Revenge of the Commands.
-        send_command_("II", 0, 0, 0);
-        while(read_line_with_check_(buffer, -1, true) != 0)
-            process_ii_line_(buffer, info);
+        send_command("II", 0, 0, 0);
+        while(read_line_with_check(buffer, -1, true) != 0)
+            process_ii_line(buffer, info);
 
         enable_checksum_workaround_ = false;
 
-        info->calculate_values_();
+        info->calculate_values();
         if(verbose_)
         {
             err_output_ << "Retrieved sensor info:\n";
@@ -828,7 +828,7 @@
 
 unsigned int Sensor::get_time()
 {
-    return offset_timestamp_(wrap_timestamp_(get_raw_time()));
+    return offset_timestamp(wrap_timestamp(get_raw_time()));
 }
 
 
@@ -841,12 +841,12 @@
         if(verbose_)
             err_output_ << "Sensor::" << __func__ <<
                 "() Retrieving time from laser.\n";
-        send_command_("TM", "0", 1, 0);
-        send_command_("TM", "1", 1, 0);
+        send_command("TM", "0", 1, 0);
+        send_command("TM", "1", 1, 0);
         char buffer[7];
-        read_line_with_check_(buffer, 6);
-        send_command_("TM", "2", 1, 0);
-        skip_lines_(1);
+        read_line_with_check(buffer, 6);
+        send_command("TM", "2", 1, 0);
+        skip_lines(1);
         // We need to decode the time value that's in the buffer
         return decode_4_byte_value(buffer);
     }
@@ -862,7 +862,7 @@
 {
     if(verbose_)
         err_output_ << "Entering timing mode.\n";
-    enter_timing_mode_();
+    enter_timing_mode();
 
     // From A. Carballo, Y. Hara, H. Kawata, T.
     // Yoshida, A. Ohya, S. Yuta, “Time synchronisation
@@ -882,9 +882,9 @@
     for(unsigned int ii = 0; ii < samples; ii++)
     {
         unsigned long long end_time(0);
-        unsigned long long start_time = get_computer_time_();
+        unsigned long long start_time = get_computer_time();
         unsigned long long laser_time =
-            wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6;
+            wrap_timestamp(get_timing_mode_time(&end_time)) * 1e6;
         offsets.push_back(start_time -
                 (laser_time - (end_time - start_time) / 2));
     }
@@ -908,9 +908,9 @@
         for(unsigned int ii = 0; ii < samples; ii++)
         {
             unsigned long long end_time(0);
-            unsigned long long start_time = get_computer_time_();
+            unsigned long long start_time = get_computer_time();
             unsigned long long laser_time =
-                wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6;
+                wrap_timestamp(get_timing_mode_time(&end_time)) * 1e6;
             offsets.push_back(start_time -
                     (laser_time - (end_time - start_time) / 2));
         }
@@ -934,7 +934,7 @@
     // All done.
     if(verbose_)
         err_output_ << "Leaving timing mode.\n";
-    leave_timing_mode_();
+    leave_timing_mode();
     return time_offset_;
 }
 
@@ -966,9 +966,9 @@
         number_to_string(start_step, buffer, 3);
         number_to_string(end_step, &buffer[3], 3);
         number_to_string(cluster_count, &buffer[6], 2);
-        send_command_("G", buffer, 8, 0);
+        send_command("G", buffer, 8, 0);
         // In SCIP1 mode we're going to get back 2-byte data
-        read_2_byte_range_data_(data, num_steps);
+        read_2_byte_range_data(data, num_steps);
     }
     else if(scip_version_ == 2)
     {
@@ -978,20 +978,20 @@
         number_to_string(end_step, &buffer[4], 4);
         number_to_string(cluster_count, &buffer[8], 2);
         if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
-            send_command_("HD", buffer, 10, 0);
+            send_command("HD", buffer, 10, 0);
         else
-            send_command_("GD", buffer, 10, 0);
+            send_command("GD", buffer, 10, 0);
         // There will be a timestamp before the data (if there is data)
         // Normally we would send 6 for the expected length, but we may get no
         // timestamp back if there was no data.
-        if(read_line_with_check_(buffer) == 0)
+        if(read_line_with_check(buffer) == 0)
             throw NoDataError();
         data.laser_time_ = decode_4_byte_value(buffer) +
-            step_to_time_offset_(start_step);
-        data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
+            step_to_time_offset(start_step);
+        data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_));
         // In SCIP2 mode we're going to get back 3-byte data because we're
         // sending the GD command
-        read_3_byte_range_data_(data, num_steps);
+        read_3_byte_range_data(data, num_steps);
     }
     else
         throw UnknownScipVersionError();
@@ -1057,20 +1057,20 @@
     number_to_string(end_step, &buffer[4], 4);
     number_to_string(cluster_count, &buffer[8], 2);
     if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF)
-        send_command_("HE", buffer, 10, 0);
+        send_command("HE", buffer, 10, 0);
     else
-        send_command_("GE", buffer, 10, 0);
+        send_command("GE", buffer, 10, 0);
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(read_line_with_check_(buffer) == 0)
+    if(read_line_with_check(buffer) == 0)
         throw NoDataError();
     data.laser_time_ = decode_4_byte_value(buffer) +
-        step_to_time_offset_(start_step);
-    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
+        step_to_time_offset(start_step);
+    data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the GE command
-    read_3_byte_range_data_(data, num_steps);
+    read_3_byte_range_data(data, num_steps);
 
     return data.ranges_length_;
 }
@@ -1140,13 +1140,13 @@
         command[0] = 'M';
     command[1] = 'D';
     command[2] = '\0';
-    send_command_(command, buffer, 13, 0);
+    send_command(command, buffer, 13, 0);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
     char response[17];
-    skip_lines_(1); // End of the command echo message
-    read_line_(response, 16); // Size is command(2)+params(13)+new line(1)
+    skip_lines(1); // End of the command echo message
+    read_line(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
     if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
@@ -1155,7 +1155,7 @@
     if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
-    read_line_with_check_(response, 4);
+    read_line_with_check(response, 4);
     if(verbose_)
     {
         err_output_ << "Sensor::" << __func__ <<
@@ -1167,7 +1167,7 @@
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
-        skip_lines_(1);
+        skip_lines(1);
         throw ResponseError(response, command);
     }
 
@@ -1175,14 +1175,14 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(read_line_with_check_(buffer) == 0)
+    if(read_line_with_check(buffer) == 0)
         throw NoDataError();
     data.laser_time_ = decode_4_byte_value(buffer) +
-        step_to_time_offset_(start_step);
-    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
+        step_to_time_offset(start_step);
+    data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the MD command
-    read_3_byte_range_data_(data, num_steps);
+    read_3_byte_range_data(data, num_steps);
 
     return data.ranges_length_;
 }
@@ -1256,13 +1256,13 @@
         command[0] = 'M';
     command[1] = 'E';
     command[2] = '\0';
-    send_command_(command, buffer, 13, 0);
+    send_command(command, buffer, 13, 0);
     // Mx commands will perform a scan, then send the data prefixed with
     // another command echo.
     // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
     char response[17];
-    skip_lines_(1); // End of the command echo message
-    read_line_(response, 16); // Size is command(2)+params(13)+new line(1)
+    skip_lines(1); // End of the command echo message
+    read_line(response, 16); // Size is command(2)+params(13)+new line(1)
     // Check the echo is correct
     if(response[0] != command[0] || response[1] != command[1])
         throw CommandEchoError(command, response);
@@ -1271,7 +1271,7 @@
     if(memcmp(&response[2], buffer, 13) != 0)
         throw ParamEchoError(command);
     // The next line should be the status line
-    read_line_with_check_(response, 4);
+    read_line_with_check(response, 4);
     if(verbose_)
     {
         err_output_ << "Sensor::" << __func__ <<
@@ -1283,7 +1283,7 @@
     {
         // There is an extra line feed after an error status (signalling
         // end of message)
-        skip_lines_(1);
+        skip_lines(1);
         throw ResponseError(response, command);
     }
 
@@ -1291,14 +1291,14 @@
     // There will be a timestamp before the data (if there is data)
     // Normally we would send 6 for the expected length, but we may get no
     // timestamp back if there was no data.
-    if(read_line_with_check_(buffer) == 0)
+    if(read_line_with_check(buffer) == 0)
         throw NoDataError();
     data.laser_time_ = decode_4_byte_value(buffer) +
-        step_to_time_offset_(start_step);
-    data.system_time_ = offset_timestamp_(wrap_timestamp_(data.laser_time_));
+        step_to_time_offset(start_step);
+    data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_));
     // In SCIP2 mode we're going to get back 3-byte data because we're
     // sending the ME command
-    read_3_byte_range_and_intensity_data_(data, num_steps);
+    read_3_byte_range_and_intensity_data(data, num_steps);
 
     return data.ranges_length_;
 }
@@ -1368,7 +1368,7 @@
 // keep flushing until the port reports there is no data left after a timeout.
 // This shouldn't be called too much, as it introduces a delay as big as the
 // timeout (which may be infinite).
-void Sensor::clear_read_buffer_()
+void Sensor::clear_read_buffer()
 {
     while(port_->BytesAvailableWait() > 0)
         port_->Flush();
@@ -1383,7 +1383,7 @@
 // The line feed that terminates a line will be replaced with a 0.
 // The return value is the number of bytes received, not including the 0
 // byte or the line feed.
-int Sensor::read_line_(char* buffer, int expected_length)
+int Sensor::read_line(char* buffer, int expected_length)
 {
     int linelength = 0;
 
@@ -1436,7 +1436,7 @@
 // This function will read a line and then calculate its checksum, comparing it
 // with the checksum at the end of the line. The checksum will be removed
 // (along with the semi-colon, if present).  buffer and expected_length args are
-// as for read_line_().
+// as for read_line().
 // If has_semicolon is true, the byte before the checksum is assumed to be the
 // semi-colon separator and so not a part of the checksum. If it's not a
 // semi-colon, an exception is thrown.
@@ -1450,10 +1450,10 @@
 // model_ is MODEL_UTM30LX. In this case, if the checksum fails normally, it
 // scans the line for "<-" and recalculates the checksum on the bytes up to
 // that point. This only happens in SCIP v2.
-int Sensor::read_line_with_check_(char* buffer, int expected_length,
+int Sensor::read_line_with_check(char* buffer, int expected_length,
         bool has_semicolon)
 {
-    int linelength = read_line_(buffer, expected_length);
+    int linelength = read_line(buffer, expected_length);
     if(scip_version_ == 1)
     {
         // No checksums in SCIP version 1
@@ -1481,7 +1481,7 @@
     int checkSum = 0;
     try
     {
-        checkSum = confirm_checksum_(buffer, bytesToConsider,
+        checkSum = confirm_checksum(buffer, bytesToConsider,
                 static_cast<int>(buffer[checksumIndex]));
     }
     catch(ProtocolError& e)
@@ -1506,7 +1506,7 @@
                             linelength);
                 }
 
-                checkSum = confirm_checksum_(buffer, newBytesToConsider,
+                checkSum = confirm_checksum(buffer, newBytesToConsider,
                         static_cast<int>(buffer[checksumIndex]));
             }
             else
@@ -1526,7 +1526,7 @@
 
 
 // Reads lines until the number specified has passed.
-void Sensor::skip_lines_(int count)
+void Sensor::skip_lines(int count)
 {
     if(verbose_)
         err_output_ << "Sensor::" << __func__ << "() Skipping " << count <<
@@ -1545,7 +1545,7 @@
 // If param_length is 0, no parameters will be sent or expected in the reply.
 // extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
 // Return value is the status code returned for the command.
-int Sensor::send_command_(char const* cmd, char const* param,
+int Sensor::send_command(char const* cmd, char const* param,
         int param_length, char const* extra_ok)
 {
     int statusCode = -1;
@@ -1577,8 +1577,8 @@
         // 16 including \n's depending on the parameters): cmd[0] params \n
         // status \n
         int statusIndex = 2 + param_length;
-        read_line_(response, 2 + param_length);
-        read_line_(&response[statusIndex], 2);
+        read_line(response, 2 + param_length);
+        read_line(&response[statusIndex], 2);
         // First make sure that the echoed command matches
         if(response[0] != cmd[0])
         {
@@ -1616,7 +1616,7 @@
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
-                    skip_lines_(1);
+                    skip_lines(1);
                     throw Scip1ResponseError(response[statusIndex], cmd[0]);
                 }
             }
@@ -1624,7 +1624,7 @@
             {
                 // There is an extra line feed after an error status
                 // (signalling end of message)
-                skip_lines_(1);
+                skip_lines(1);
                 throw Scip1ResponseError(response[statusIndex], cmd[0]);
             }
         }
@@ -1651,7 +1651,7 @@
             throw WriteError(21);
 
         // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
-        read_line_(response, 3 + param_length);
+        read_line(response, 3 + param_length);
         // Check the echo is correct
         if(response[0] != cmd[0] || response[1] != cmd[1])
             throw CommandEchoError(cmd, response);
@@ -1663,7 +1663,7 @@
         }
 
         // The next line should be the status line
-        read_line_with_check_(response, 4);
+        read_line_with_check(response, 4);
         if(verbose_)
         {
             err_output_ << "Sensor::" << __func__ <<
@@ -1681,7 +1681,7 @@
                 {
                     // There is an extra line feed after an error status
                     // (signalling end of message)
-                    skip_lines_(1);
+                    skip_lines(1);
                     throw ResponseError(response, cmd);
                 }
             }
@@ -1689,7 +1689,7 @@
             {
                 // There is an extra line feed after an error status
                 // (signalling end of message)
-                skip_lines_(1);
+                skip_lines(1);
                 throw ResponseError(response, cmd);
             }
         }
@@ -1704,25 +1704,25 @@
 
 
 /// Puts the laser into the timing mode.
-void Sensor::enter_timing_mode_()
+void Sensor::enter_timing_mode()
 {
-    send_command_("TM", "0", 1, 0);
-    skip_lines_(1);
+    send_command("TM", "0", 1, 0);
+    skip_lines(1);
 }
 
 
 /// Take the laser out of timing mode.
-void Sensor::leave_timing_mode_()
+void Sensor::leave_timing_mode()
 {
-    send_command_("TM", "2", 1, 0);
-    skip_lines_(1);
+    send_command("TM", "2", 1, 0);
+    skip_lines(1);
 }
 
 
 /// Get the timestamp from the laser.
 /// If @ref reception_time is not 0, it will be filled with the time at
 /// which data reception was completed.
-unsigned int Sensor::get_timing_mode_time_(unsigned long long* reception_time)
+unsigned int Sensor::get_timing_mode_time(unsigned long long* reception_time)
 {
     // To get the most accurate result, we cannot do any processing while
     // receiving. We want to know the time that reception finished as exactly
@@ -1734,7 +1734,7 @@
         throw WriteError(19);
     unsigned int line_length = port_->Read(response, 16);
     if(reception_time)
-        *reception_time = get_computer_time_();
+        *reception_time = get_computer_time();
     // Process the response to confirm it is correct
     if(line_length < 0)
         throw ReadError(0);
@@ -1749,7 +1749,7 @@
     if(response[4] != '0' || response[5] != '0' || response[6] != 'P')
         throw ResponseError(response, "TM");
     // Check the checksum on the time stamp is accurate
-    confirm_checksum_(&response[8], 4, response[12]);
+    confirm_checksum(&response[8], 4, response[12]);
     // Decode the time stamp
     unsigned int timestamp =
         decode_4_byte_value(&response[8]);
@@ -1758,7 +1758,7 @@
 
 
 /// Get the computer's time as accurately as possible.
-unsigned long long Sensor::get_computer_time_()
+unsigned long long Sensor::get_computer_time()
 {
 #if defined(_POSIX_TIMERS)
     struct timespec ts;
@@ -1773,7 +1773,7 @@
 
 
 /// timestamp must be in milliseconds. The result is in milliseconds.
-unsigned int Sensor::wrap_timestamp_(unsigned int timestamp)
+unsigned int Sensor::wrap_timestamp(unsigned int timestamp)
 {
     if(timestamp < last_timestamp_)
     {
@@ -1786,14 +1786,14 @@
 
 
 /// timestamp must be in milliseconds. The result is in nanoseconds.
-unsigned long long Sensor::offset_timestamp_(unsigned int timestamp)
+unsigned long long Sensor::offset_timestamp(unsigned int timestamp)
 {
     return ((1 - time_drift_rate_) * timestamp * 1e6 + time_offset_) /
         (1 - time_skew_alpha_);
 }
 
 
-unsigned int Sensor::step_to_time_offset_(int start_step)
+unsigned int Sensor::step_to_time_offset(int start_step)
 {
     if(start_step < 0)
         return first_step_ * time_resolution_;
@@ -1803,7 +1803,7 @@
 
 
 /// Search a string for the laser's model.
-void Sensor::find_model_(char const* buffer)
+void Sensor::find_model(char const* buffer)
 {
     if(strstr(buffer, "URG-04LX") != 0)
         model_ = MODEL_URG04LX;
@@ -1825,7 +1825,7 @@
 }
 
 
-void Sensor::get_and_set_scip_version_()
+void Sensor::get_and_set_scip_version()
 {
     bool scip2Failed = false;
 
@@ -1836,7 +1836,7 @@
     // Try SCIP version 2 first by sending an info command
     try
     {
-        send_command_("VV", 0, 0, 0);
+        send_command("VV", 0, 0, 0);
     }
     catch(BaseError)
     {
@@ -1856,18 +1856,18 @@
         port_->Flush();
         try
         {
-            send_command_("V", 0, 0, 0);
+            send_command("V", 0, 0, 0);
         }
         catch(BaseError)
         {
             throw ScipVersionError();
         }
         // Skip the vendor and product info
-        skip_lines_(2);
+        skip_lines(2);
         // Get the firmware line
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
-        read_line_(buffer);
+        read_line(buffer);
 
         if(strncmp(buffer, "FIRM:", 5) != 0)
             throw MissingFirmSpecError();
@@ -1887,7 +1887,7 @@
         }
         // Dump the rest of the V command result (one of these will be the
         // empty last line)
-        skip_lines_(3);
+        skip_lines(3);
 
         // If the firmware version is less than 3, we're stuck with SCIP
         // version 1.
@@ -1910,7 +1910,7 @@
             // first byte as the command and the other 6 as parameters.
             try
             {
-                send_command_("S", "CIP2.0", 6, 0);
+                send_command("S", "CIP2.0", 6, 0);
             }
             catch(BaseError)
             {
@@ -1921,7 +1921,7 @@
                 return;
             }
             // There'll be a trailing line on the end
-            skip_lines_(1);
+            skip_lines(1);
 
             // Changed to SCIP version 2
             if(verbose_)
@@ -1937,7 +1937,7 @@
         scip_version_ = 2;
 
         // Dump the rest of the result
-        skip_lines_(6);
+        skip_lines(6);
         if(verbose_)
             err_output_ << "Sensor::" << __func__ <<
                 "() Using SCIP version 2.\n";
@@ -1949,7 +1949,7 @@
 }
 
 
-void Sensor::get_defaults_()
+void Sensor::get_defaults()
 {
     if(verbose_)
         err_output_ << "Sensor::" << __func__ <<
@@ -1976,7 +1976,7 @@
 }
 
 
-void Sensor::process_vv_line_(char const* buffer, SensorInfo* info)
+void Sensor::process_vv_line(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "VEND", 4) == 0)
         info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
@@ -1984,7 +1984,7 @@
     {
         info->product = &buffer[5]; // Product info
         // Find the product model
-        find_model_(&buffer[5]);
+        find_model(&buffer[5]);
         info->detected_model = model_;
     }
     else if(strncmp(buffer, "FIRM", 4) == 0)
@@ -1998,7 +1998,7 @@
 }
 
 
-void Sensor::process_pp_line_(char const* buffer, SensorInfo* info)
+void Sensor::process_pp_line(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
         info->model = &buffer[5];   // Model
@@ -2031,7 +2031,7 @@
 }
 
 
-void Sensor::process_ii_line_(char const* buffer, SensorInfo* info)
+void Sensor::process_ii_line(char const* buffer, SensorInfo* info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
         // Do nothing here - we already know this value from PP
@@ -2094,7 +2094,7 @@
 
 /// Combines up to three values from an echo buffer into a single value based
 /// on the setting of multiecho_mode_.
-uint32_t Sensor::process_echo_buffer_(int const* buffer, int num_echos)
+uint32_t Sensor::process_echo_buffer(int const* buffer, int num_echos)
 {
     uint32_t sum = 0;
     switch(multiecho_mode_)
@@ -2125,7 +2125,7 @@
 }
 
 
-void Sensor::read_2_byte_range_data_(ScanData& data, unsigned int num_steps)
+void Sensor::read_2_byte_range_data(ScanData& data, unsigned int num_steps)
 {
     if(verbose_)
     {
@@ -2146,7 +2146,7 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = read_line_with_check_(buffer);
+        numBytesInLine = read_line_with_check(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
             break;
@@ -2158,7 +2158,7 @@
                 // Line feed in the middle of a data block? Why?
                 throw MisplacedLineFeedError();
             }
-            data.write_range_(current_step, decode_2_byte_value(&buffer[ii]));
+            data.write_range(current_step, decode_2_byte_value(&buffer[ii]));
         }
         // End of this line. Go around again.
     }
@@ -2171,7 +2171,7 @@
 }
 
 
-void Sensor::read_3_byte_range_data_(ScanData& data, unsigned int num_steps)
+void Sensor::read_3_byte_range_data(ScanData& data, unsigned int num_steps)
 {
     if(verbose_)
     {
@@ -2202,15 +2202,15 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = read_line_with_check_(buffer);
+        numBytesInLine = read_line_with_check(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
         {
             if(echo_buffer[0] != -1)
             {
                 // Not the first value, so deal with the previous
-                data.write_range_(current_step,
-                        process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
+                data.write_range(current_step,
+                        process_echo_buffer(echo_buffer, echo_buf_ind + 1));
                 if(data.ranges_)
                 {
                     if(data.ranges_[current_step] > max_range_)
@@ -2247,8 +2247,8 @@
                 else if(echo_buffer[0] != -1)
                 {
                     // Not the first value, so deal with the previous
-                    data.write_range_(current_step,
-                            process_echo_buffer_(echo_buffer,
+                    data.write_range(current_step,
+                            process_echo_buffer(echo_buffer,
                                 echo_buf_ind + 1));
                     if(data.ranges_)
                     {
@@ -2315,7 +2315,7 @@
 }
 
 
-void Sensor::read_3_byte_range_and_intensity_data_(ScanData& data,
+void Sensor::read_3_byte_range_and_intensity_data(ScanData& data,
         unsigned int num_steps)
 {
     if(verbose_)
@@ -2347,7 +2347,7 @@
     while(true)
     {
         // Read a line of data
-        numBytesInLine = read_line_with_check_(buffer);
+        numBytesInLine = read_line_with_check(buffer);
         // Check if we've reached the end of the data
         if(numBytesInLine == 0)
         {
@@ -2357,8 +2357,8 @@
             if(echo_buffer[0] != -1)
             {
                 assert(nextIsIntensity == true);
-                data.write_intensity_(current_intensity,
-                    process_echo_buffer_(echo_buffer, echo_buf_ind + 1));
+                data.write_intensity(current_intensity,
+                    process_echo_buffer(echo_buffer, echo_buf_ind + 1));
                 current_intensity++;
             }
             break;
@@ -2387,14 +2387,14 @@
                     // Not the first value, so deal with the previous
                     if(nextIsIntensity)
                     {
-                        data.write_intensity_(current_intensity,
-                                process_echo_buffer_(echo_buffer,
+                        data.write_intensity(current_intensity,
+                                process_echo_buffer(echo_buffer,
                                     echo_buf_ind + 1));
                     }
                     else
                     {
-                        data.write_range_(current_range,
-                                process_echo_buffer_(echo_buffer,
+                        data.write_range(current_range,
+                                process_echo_buffer(echo_buffer,
                                     echo_buf_ind + 1));
                     }
                     echo_buf_ind = 0;
@@ -2478,7 +2478,7 @@
 }
 
 
-int Sensor::confirm_checksum_(char const* buffer, int length,
+int Sensor::confirm_checksum(char const* buffer, int length,
         int expected_sum)
 {
     int checksum = 0;

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-16 02:09:25 UTC (rev 513)
@@ -509,45 +509,43 @@
         /// The clock skew alpha value.
         float time_skew_alpha_;
 
-        void clear_read_buffer_();
-        int read_line_(char* buffer, int expected_length = -1);
-        int read_line_with_check_(char* buffer, int expected_length = -1,
+        void clear_read_buffer();
+        int read_line(char* buffer, int expected_length = -1);
+        int read_line_with_check(char* buffer, int expected_length = -1,
                 bool has_semicolon = false);
-        void skip_lines_(int count);
-        int send_command_(char const* cmd, char const* param, int param_length,
+        void skip_lines(int count);
+        int send_command(char const* cmd, char const* param, int param_length,
                 char const* extra_ok);
 
-    public:
-        void enter_timing_mode_();
-        void leave_timing_mode_();
+        void enter_timing_mode();
+        void leave_timing_mode();
         /// Get the laser time in milliseconds.
-        unsigned int get_timing_mode_time_(unsigned long long* reception_time=0);
+        unsigned int get_timing_mode_time(unsigned long long* reception_time=0);
         /// Get the computer time in nanoseconds.
-        unsigned long long get_computer_time_();
+        unsigned long long get_computer_time();
         /// Adjust a wrapped laser timestamp, in milliseconds.
-        unsigned int wrap_timestamp_(unsigned int timestamp);
+        unsigned int wrap_timestamp(unsigned int timestamp);
         /// Offset a laser timestamp, in milliseconds, into computer time, in
         /// nanoseconds.
-        unsigned long long offset_timestamp_(unsigned int timestamp);
-    private:
+        unsigned long long offset_timestamp(unsigned int timestamp);
         /// Convert a step value into its time offset from the start of a scan,
         /// in milliseconds.
-        unsigned int step_to_time_offset_(int start_step);
+        unsigned int step_to_time_offset(int start_step);
 
-        void find_model_(char const* buffer);
-        void get_and_set_scip_version_();
-        void get_defaults_();
-        void process_vv_line_(char const* buffer, SensorInfo* info);
-        void process_pp_line_(char const* buffer, SensorInfo* info);
-        void process_ii_line_(char const* buffer, SensorInfo* info);
+        void find_model(char const* buffer);
+        void get_and_set_scip_version();
+        void get_defaults();
+        void process_vv_line(char const* buffer, SensorInfo* info);
+        void process_pp_line(char const* buffer, SensorInfo* info);
+        void process_ii_line(char const* buffer, SensorInfo* info);
 
-        uint32_t process_echo_buffer_(int const* buffer, int num_echos);
-        void read_2_byte_range_data_(ScanData& data, unsigned int num_steps);
-        void read_3_byte_range_data_(ScanData& data, unsigned int num_steps);
-        void read_3_byte_range_and_intensity_data_(ScanData& data,
+        uint32_t process_echo_buffer(int const* buffer, int num_echos);
+        void read_2_byte_range_data(ScanData& data, unsigned int num_steps);
+        void read_3_byte_range_data(ScanData& data, unsigned int num_steps);
+        void read_3_byte_range_and_intensity_data(ScanData& data,
                 unsigned int num_steps);
 
-        int confirm_checksum_(char const* buffer, int length,
+        int confirm_checksum(char const* buffer, int length,
                 int expected_sum);
 }; // class Sensor
 

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp	2010-08-16 02:09:25 UTC (rev 513)
@@ -39,7 +39,7 @@
 
 
 // Set various known values based on what the manual says
-void SensorInfo::set_defaults_()
+void SensorInfo::set_defaults()
 {
     switch(detected_model)
     {
@@ -103,7 +103,7 @@
 }
 
 
-void SensorInfo::calculate_values_()
+void SensorInfo::calculate_values()
 {
     resolution = DTOR(360.0) / steps;
     // If any of the steps are beyond INT_MAX, we have problems.

Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-16 01:36:43 UTC (rev 512)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h	2010-08-16 02:09:25 UTC (rev 513)
@@ -206,8 +206,8 @@
         LaserModel detected_model;
 
     private:
-        void set_defaults_();
-        void calculate_values_();
+        void set_defaults();
+        void calculate_values();
 }; // class SensorInfo
 
 }; // namespace hokuyo_aist


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 514
          http://gearbox.svn.sourceforge.net/gearbox/?rev=514&view=rev
Author:   gbiggs
Date:     2010-08-16 02:22:54 +0000 (Mon, 16 Aug 2010)

Log Message:
-----------
Changed pointer to reference

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/getid/getid.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.cpp
    gearbox/trunk/src/hokuyo_aist/sensor.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-08-16 02:09:25 UTC (rev 513)
+++ gearbox/trunk/src/hokuyo_aist/getid/getid.cpp	2010-08-16 02:22:54 UTC (rev 514)
@@ -63,24 +63,24 @@
         // Set the laser to verbose mode (so we see more information in the
         // console)
         if(verbose)
-            laser.set_verbose (true);
+            laser.set_verbose(true);
 
         // Open the laser
-        laser.open (port_options);
+        laser.open(port_options);
         // Turn the laser on
-        laser.set_power (true);
+        laser.set_power(true);
 
         // Get some laser info
         hokuyo_aist::SensorInfo info;
-        laser.get_sensor_info (&info);
+        laser.get_sensor_info(info);
         std::cout << info.serial << '\n';
 
         // Close the laser
-        laser.close ();
+        laser.close();
     }
     catch(hokuyo_aist::BaseError &e)
     {
-        std::cerr << "Caught exception: " << e.what () << '\n';
+        std::cerr << "Caught exception: " << e.what() << '\n';
         return 1;
     }
 

Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-16 02:09:25 UTC (rev 513)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp	2010-08-16 02:22:54 UTC (rev 514)
@@ -664,11 +664,8 @@
 }
 
 
-void Sensor::get_sensor_info(SensorInfo* info)
+void Sensor::get_sensor_info(SensorInfo& info)
 {
-    if(info == 0)
-        throw NoDestinationError();
-
     if(scip_version_ == 1)
     {
         if(verbose_)
@@ -677,7 +674,7 @@
                 "() Getting sensor information using SCIP version 1.\n";
         }
 
-        info->set_defaults();
+        info.set_defaults();
 
         char buffer[SCIP1_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH);
@@ -685,27 +682,27 @@
         send_command("V", 0, 0, 0);
         // Get the vendor info line
         read_line(buffer);
-        info->vendor = &buffer[5]; // Chop off the "VEND:" tag
+        info.vendor = &buffer[5]; // Chop off the "VEND:" tag
         // Get the product info line
         read_line(buffer);
-        info->product = &buffer[5];
+        info.product = &buffer[5];
         // Only the URG-04LX supports SCIP1
         model_ = MODEL_URG04LX;
         // Get the firmware line
         read_line(buffer);
-        info->firmware = &buffer[5];
+        info.firmware = &buffer[5];
         // Get the protocol version line
         read_line(buffer);
-        info->protocol = &buffer[5];
+        info.protocol = &buffer[5];
         // Get the serial number
         read_line(buffer);
-        info->serial = &buffer[5];
+        info.serial = &buffer[5];
         // Get either the status line or the end of message
         read_line(buffer);
         if(buffer[0] != '\0')
         {
             // Got a status line
-            info->sensor_diagnostic = &buffer[5];
+            info.sensor_diagnostic = &buffer[5];
             skip_lines(1);
         }
 
@@ -714,60 +711,60 @@
         // eg: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
         // Note that this example is right up against the maximum SCIP v1 line
         // length of 64 bytes.
-        if(atoi(info->firmware.c_str()) >= 3)
+        if(atoi(info.firmware.c_str()) >= 3)
         {
             if(verbose_)
                 err_output_ << "SCIP1 Firmware line for parsing: " <<
-                    info->firmware << '\n';
+                    info.firmware << '\n';
             // Now the fun part: parsing the line. It would be nice if we could
             // use the POSIX regex functions, but since MS doesn't believe in
             // POSIX we get to do it the hard way.
             // Start by finding the first (
             char const* valueStart;
-            if((valueStart = strchr(info->firmware.c_str(), '(')) == 0)
+            if((valueStart = strchr(info.firmware.c_str(), '(')) == 0)
             {
                 // No bracket? Crud. Fail and use the hard-coded values from
                 // the manual.
-                info->calculate_values();
+                info.calculate_values();
             }
             // Now put it through sscanf and hope...
             int aperture;
             int numFound = sscanf(valueStart,
-                    "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info->min_range,
-                    &info->max_range, &aperture, &info->first_step,
-                    &info->last_step, &info->speed);
+                    "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info.min_range,
+                    &info.max_range, &aperture, &info.first_step,
+                    &info.last_step, &info.speed);
             if(numFound != 6)
             {
                 // Didn't get enough values out, assume unknown format and fall
                 // back on the defaults
-                info->set_defaults();
-                info->calculate_values();
+                info.set_defaults();
+                info.calculate_values();
                 if(verbose_)
                 {
                     err_output_ << "Retrieved sensor info (hard-coded, not "
                         "enough values):\n";
-                    err_output_ << info->as_string();
+                    err_output_ << info.as_string();
                 }
             }
             else
             {
                 // Need to calculate stuff differently since it gave us an
                 // aperture value
-                info->resolution = DTOR(static_cast<double>(aperture)) /
-                    static_cast<double>(info->last_step - info->first_step);
+                info.resolution = DTOR(static_cast<double>(aperture)) /
+                    static_cast<double>(info.last_step - info.first_step);
                 // Assume that the range is evenly spread
-                info->scanable_steps = info->last_step - info->first_step + 1;
-                info->front_step = info->scanable_steps / 2 +
-                    info->first_step - 1;
-                info->min_angle = (static_cast<int>(info->first_step) -
-                        static_cast<int>(info->front_step)) * info->resolution;
-                info->max_angle = (info->last_step - info->front_step) *
-                    info->resolution;
+                info.scanable_steps = info.last_step - info.first_step + 1;
+                info.front_step = info.scanable_steps / 2 +
+                    info.first_step - 1;
+                info.min_angle = (static_cast<int>(info.first_step) -
+                        static_cast<int>(info.front_step)) * info.resolution;
+                info.max_angle = (info.last_step - info.front_step) *
+                    info.resolution;
 
                 if(verbose_)
                 {
                     err_output_ << "Retrieved sensor info (from FIRM line):\n";
-                    err_output_ << info->as_string();
+                    err_output_ << info.as_string();
                 }
             }
         }
@@ -775,11 +772,11 @@
         {
             // We're stuck with hard-coded defaults from the manual (already
             // set earlier).
-            info->calculate_values();
+            info.calculate_values();
             if(verbose_)
             {
                 err_output_ << "Retrieved sensor info (hard-coded):\n";
-                err_output_ << info->as_string();
+                err_output_ << info.as_string();
             }
         }
     }
@@ -791,7 +788,7 @@
                 "() Getting sensor information using SCIP version 2.\n";
         }
 
-        info->set_defaults();
+        info.set_defaults();
 
         char buffer[SCIP2_LINE_LENGTH];
         memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH);
@@ -814,11 +811,11 @@
 
         enable_checksum_workaround_ = false;
 
-        info->calculate_values();
+        info.calculate_values();
         if(verbose_)
         {
             err_output_ << "Retrieved sensor info:\n";
-            err_output_ << info->as_string();
+            err_output_ << info.as_string();
         }
     }
     else
@@ -1957,7 +1954,7 @@
 
     // Get the laser's info
     SensorInfo info;
-    get_sensor_info(&info);
+    get_sensor_info(info);
 
     min_angle_ = info.min_angle;
     max_angle_ = info.max_angle;
@@ -1976,62 +1973,62 @@
 }
 
 
-void Sensor::process_vv_line(char const* buffer, SensorInfo* info)
+void Sensor::process_vv_line(char const* buffer, SensorInfo& info)
 {
     if(strncmp(buffer, "VEND", 4) == 0)
-        info->vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
+        info.vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag
     else if(strncmp(buffer, "PROD", 4) == 0)
     {
-        info->product = &buffer[5]; // Product info
+        info.product = &buffer[5]; // Product info
         // Find the product model
         find_model(&buffer[5]);
-        info->detected_model = model_;
+        info.detected_model = model_;
     }
     else if(strncmp(buffer, "FIRM", 4) == 0)
-        info->firmware = &buffer[5]; // Firmware version
+        info.firmware = &buffer[5]; // Firmware version
     else if(strncmp(buffer, "PROT", 4) == 0)
-        info->protocol = &buffer[5]; // Protocol version
+        info.protocol = &buffer[5]; // Protocol version
     else if(strncmp(buffer, "SERI", 4) == 0)
-        info->serial = &buffer[5]; // Serial number
+        info.serial = &buffer[5]; // Serial number
     else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::process_pp_line(char const* buffer, SensorInfo* info)
+void Sensor::process_pp_line(char const* buffer, SensorInfo& info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
-        info->model = &buffer[5];   // Model
+        info.model = &buffer[5];   // Model
     // On to the fun ones that require parsing
     else if(strncmp(buffer, "DMIN", 4) == 0)
-        info->min_range = atoi(&buffer[5]);
+        info.min_range = atoi(&buffer[5]);
     else if(strncmp(buffer, "DMAX", 4) == 0)
-        info->max_range = atoi(&buffer[5]);
+        info.max_range = atoi(&buffer[5]);
     else if(strncmp(buffer, "ARES", 4) == 0)
-        info->steps = atoi(&buffer[5]);
+        info.steps = atoi(&buffer[5]);
     else if(strncmp(buffer, "AMIN", 4) == 0)
-        info->first_step = atoi(&buffer[5]);
+        info.first_step = atoi(&buffer[5]);
     else if(strncmp(buffer, "AMAX", 4) == 0)
-        info->last_step = atoi(&buffer[5]);
+        info.last_step = atoi(&buffer[5]);
     else if(strncmp(buffer, "AFRT", 4) == 0)
-        info->front_step = atoi(&buffer[5]);
+        info.front_step = atoi(&buffer[5]);
     else if(strncmp(buffer, "SCAN", 4) == 0)
-        info->standard_speed = atoi(&buffer[5]);
+        info.standard_speed = atoi(&buffer[5]);
     /* No example in the manual and sensor with support for this has not
      * arrived yet, so don't know what to look for.
     else if(strncmp(buffer, "", 4) == 0)
     {
         if(strstr(buffer, "CCW") != 0)
-            info->rot_dir = COUNTERCLOCKWISE;
+            info.rot_dir = COUNTERCLOCKWISE;
         else
-            info->rot_dir = CLOCKWISE;
+            info.rot_dir = CLOCKWISE;
     }*/
     else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }
 
 
-void Sensor::process_ii_line(char const* buffer, SensorInfo* info)
+void Sensor::process_ii_line(char const* buffer, SensorInfo& info)
 {
     if(strncmp(buffer, "MODL", 4) == 0)
         // Do nothing here - we already know this value from PP
@@ -2039,54 +2036,54 @@
     else if(strncmp(buffer, "LASR", 4) == 0)
     {
         if(strncmp(&buffer[5], "OFF", 3) == 0)
-            info->power = false;
+            info.power = false;
         else
-            info->power = true;
+            info.power = true;
     }
     else if(strncmp(buffer, "SCSP", 4) == 0)
     {
         if(strncmp(&buffer[5], "Initial", 7) == 0)
         {
             // Unchanged motor speed
-            if(sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
+            if(sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info.speed) != 1)
             {
                 throw ParseError(buffer, "Motor speed");
             }
-            info->speed_level = 0;
-            time_resolution_ = info->speed;
+            info.speed_level = 0;
+            time_resolution_ = info.speed;
         }
         else
         {
             // Changed motor speed, format is:
             // <level>%<ignored string>(<speed>[rpm])
-            if(sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info->speed_level,
-                        &info->speed) != 2)
+            if(sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info.speed_level,
+                        &info.speed) != 2)
             {
                 throw ParseError(buffer, "Motor speed");
             }
-            time_resolution_ = info->speed;
+            time_resolution_ = info.speed;
         }
     }
     else if(strncmp(buffer, "MESM", 4) == 0)
-        info->measure_state = &buffer[5];
+        info.measure_state = &buffer[5];
     else if(strncmp(buffer, "SBPS", 4) == 0)
     {
         if(strncmp(&buffer[5], "USB only", 8) == 0 ||
             strncmp(&buffer[5], "USB Full Speed", 14) == 0)
         {
             // No baud rate for USB-only devices such as the UHG-08LX
-            info->baud = 0;
+            info.baud = 0;
         }
-        else if(sscanf(buffer, "SBPS:%d[bps]", &info->baud) != 1)
+        else if(sscanf(buffer, "SBPS:%d[bps]", &info.baud) != 1)
             throw ParseError(buffer, "Baud rate");
     }
     else if(strncmp(buffer, "TIME", 4) == 0)
     {
-        if(sscanf(buffer, "TIME:%x", &info->time) != 1)
+        if(sscanf(buffer, "TIME:%x", &info.time) != 1)
             throw ParseError(buffer, "Timestamp");
     }
     else if(strncmp(buffer, "STAT", 4) == 0)
-        info->sensor_diagnostic = &buffer[5];
+        info.sensor_diagnostic = &buffer[5];
     else if(!ignore_unknowns_)
         throw UnknownLineError(buffer);
 }

Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-16 02:09:25 UTC (rev 513)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h	2010-08-16 02:22:54 UTC (rev 514)
@@ -20,7 +20,6 @@
 #ifndef SENSOR_H__
 #define SENSOR_H__
 
-#include <flexiport/port.h>
 #include <string>
 
 #if defined(WIN32)
@@ -38,6 +37,11 @@
     #define HOKUYO_AIST_EXPORT
 #endif
 
+namespace flexiport
+{
+    class Port;
+}
+
 /** @ingroup gbx_library_hokuyo_aist
 @{
 */
@@ -189,7 +193,7 @@
         /** @brief Get various information about the scanner.
 
         Much of the information is not available with the SCIP v1 protocol. */
-        void get_sensor_info(SensorInfo* info);
+        void get_sensor_info(SensorInfo& info);
 
         /** @brief Get the value of the scanner's clock in milliseconds.
 
@@ -535,9 +539,9 @@
         void find_model(char const* buffer);
         void get_and_set_scip_version();
         void get_defaults();
-        void process_vv_line(char const* buffer, SensorInfo* info);
-        void process_pp_line(char const* buffer, SensorInfo* info);
-        void process_ii_line(char const* buffer, SensorInfo* info);
+        void process_vv_line(char const* buffer, SensorInfo& info);
+        void process_pp_line(char const* buffer, SensorInfo& info);
+        void process_ii_line(char const* buffer, SensorInfo& info);
 
         uint32_t process_echo_buffer(int const* buffer, int num_echos);
         void read_2_byte_range_data(ScanData& data, unsigned int num_steps);

Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-16 02:09:25 UTC (rev 513)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp	2010-08-16 02:22:54 UTC (rev 514)
@@ -171,7 +171,7 @@
         // Get some laser info
         std::cout << "Laser sensor information:\n";
         hokuyo_aist::SensorInfo info;
-        laser.get_sensor_info(&info);
+        laser.get_sensor_info(info);
         std::cout << info.as_string();
 
         // Get range data


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.