From: <gb...@us...> - 2009-04-14 09:12:05
|
Revision: 390 http://gearbox.svn.sourceforge.net/gearbox/?rev=390&view=rev Author: gbiggs Date: 2009-04-14 09:11:54 +0000 (Tue, 14 Apr 2009) Log Message: ----------- Changed ordering of SCIP protocol version checks. Removed a misplaced (by an entire project) error message from some CMakeLists.txt files. Modified Paths: -------------- gearbox/trunk/src/flexiport/python/CMakeLists.txt gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt Modified: gearbox/trunk/src/flexiport/python/CMakeLists.txt =================================================================== --- gearbox/trunk/src/flexiport/python/CMakeLists.txt 2009-04-08 03:30:00 UTC (rev 389) +++ gearbox/trunk/src/flexiport/python/CMakeLists.txt 2009-04-14 09:11:54 UTC (rev 390) @@ -20,8 +20,6 @@ GET_FILENAME_COMPONENT (boostPythonLib ${Boost_PYTHON_LIBRARY} NAME_WE CACHE) # Chop off the lib at the front, too, if present STRING (REGEX REPLACE "^lib" "" boostPythonLib ${boostPythonLib}) - MESSAGE (STATUS - "PlayerC++ client library will be built with Boost::Thread support.") else(Boost_PYTHON_FOUND) MESSAGE (STATUS "Boost::Python library was not found. Cannot build Python bindings for Flexiport.") @@ -67,4 +65,4 @@ ADD_SUBDIRECTORY (test) ELSEIF(NOT GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) MESSAGE (STATUS "Flexiport Python bindings will not be built - must build flexiport as a shared library.") -endif(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) \ No newline at end of file +endif(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp =================================================================== --- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2009-04-08 03:30:00 UTC (rev 389) +++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp 2009-04-14 09:11:54 UTC (rev 390) @@ -114,6 +114,8 @@ - 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 @@ -447,7 +449,7 @@ // HokuyoError class //////////////////////////////////////////////////////////////////////////////////////////////////// -string HokuyoError::AsString (void) const throw () +string HokuyoError::AsString () const throw () { switch (_errorCode) { @@ -487,7 +489,7 @@ // HokuyoSensorInfo class //////////////////////////////////////////////////////////////////////////////////////////////////// -HokuyoSensorInfo::HokuyoSensorInfo (void) +HokuyoSensorInfo::HokuyoSensorInfo () : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0), standardSpeed (0), power (false), speed (0), speedLevel (0), baud (0), time (0), minAngle (0.0), maxAngle (0.0), resolution (0.0), scanableSteps (0) @@ -495,7 +497,7 @@ } // Set various known values based on what the manual says -void HokuyoSensorInfo::SetDefaults (void) +void HokuyoSensorInfo::SetDefaults () { minRange = 20; maxRange = 4095; @@ -505,7 +507,7 @@ frontStep = 384; } -void HokuyoSensorInfo::CalculateValues (void) +void HokuyoSensorInfo::CalculateValues () { resolution = DTOR (360.0) / steps; // If any of the steps are beyond INT_MAX, we have problems. @@ -515,7 +517,7 @@ scanableSteps = lastStep - firstStep + 1; } -string HokuyoSensorInfo::AsString (void) +string HokuyoSensorInfo::AsString () { stringstream ss; @@ -549,7 +551,7 @@ // HokuyoData class //////////////////////////////////////////////////////////////////////////////////////////////////// -HokuyoData::HokuyoData (void) +HokuyoData::HokuyoData () : _ranges (NULL), _intensities (NULL), _length (0), _error (false), _time (0), _sensorIsUTM30LX (false) { @@ -637,7 +639,7 @@ _time = rhs.TimeStamp (); } -HokuyoData::~HokuyoData (void) +HokuyoData::~HokuyoData () { if (_ranges != NULL) delete[] _ranges; @@ -788,7 +790,7 @@ return _ranges[index]; } -string HokuyoData::AsString (void) +string HokuyoData::AsString () { stringstream ss; @@ -818,7 +820,7 @@ return ss.str (); } -void HokuyoData::CleanUp (void) +void HokuyoData::CleanUp () { if (_ranges != NULL) delete[] _ranges; @@ -894,13 +896,14 @@ // Public API //////////////////////////////////////////////////////////////////////////////////////////////////// -HokuyoLaser::HokuyoLaser (void) - : _port (NULL), _scipVersion (1), _verbose (false), _sensorIsUTM30LX (false),_minAngle (0.0), - _maxAngle (0.0), _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0) +HokuyoLaser::HokuyoLaser () + : _port (NULL), _scipVersion (2), _verbose (false), _sensorIsUTM30LX (false), + _enableCheckSumWorkaround (false), _minAngle (0.0), _maxAngle (0.0), _resolution (0.0), + _firstStep (0), _lastStep (0), _frontStep (0) { } -HokuyoLaser::~HokuyoLaser (void) +HokuyoLaser::~HokuyoLaser () { if (_port != NULL) delete _port; @@ -1015,7 +1018,7 @@ return 0; } -void HokuyoLaser::Close (void) +void HokuyoLaser::Close () { if (!_port) throw HokuyoError (HOKUYO_ERR_CLOSE_FAILED, "Port is not open."); @@ -1025,7 +1028,7 @@ _port = NULL; } -bool HokuyoLaser::IsOpen (void) const +bool HokuyoLaser::IsOpen () const { if (_port != NULL) return _port->IsOpen (); @@ -1111,7 +1114,7 @@ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version."); } -void HokuyoLaser::Reset (void) +void HokuyoLaser::Reset () { if (_scipVersion == 1) { @@ -1235,6 +1238,12 @@ // Get the product info line ReadLine (buffer); info->product = &buffer[5]; + // 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; + else + _sensorIsUTM30LX = false; // Get the firmware line ReadLine (buffer); info->firmware = &buffer[5]; @@ -1324,6 +1333,9 @@ "() Getting sensor information using SCIP version 2." << endl; } + if (_sensorIsUTM30LX) + _enableCheckSumWorkaround = true; + char buffer[SCIP2_LINE_LENGTH]; memset (buffer, 0, sizeof (char) * SCIP2_LINE_LENGTH); @@ -1434,6 +1446,9 @@ // Skip the end-of-message SkipLines (1); + if (_sensorIsUTM30LX) + _enableCheckSumWorkaround = false; + info->CalculateValues (); if (_verbose) { @@ -1445,7 +1460,7 @@ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version."); } -unsigned int HokuyoLaser::GetTime (void) +unsigned int HokuyoLaser::GetTime () { if (_scipVersion == 1) { @@ -1821,7 +1836,7 @@ // 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 HokuyoLaser::ClearReadBuffer (void) +void HokuyoLaser::ClearReadBuffer () { while (_port->BytesAvailableWait () > 0) _port->Flush (); @@ -1891,6 +1906,11 @@ // 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 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. 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 HokuyoLaser::ReadLineWithCheck (char *buffer, int expectedLength, bool hasSemicolon) { int lineLength = ReadLine (buffer, expectedLength); @@ -1923,27 +1943,44 @@ } int checkSum = 0; - // Start by adding the byte values - for (int ii = 0; ii < bytesToConsider; ii++) - checkSum += buffer[ii]; - // Take the lowest 6 bits - checkSum &= 0x3F; - // Add 0x30 - checkSum += 0x30; - - if (_verbose) + try { - cerr << "HokuyoLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" << - static_cast<char> (checkSum) << "), given checksum = " << - static_cast<int> (buffer[checksumIndex]) << " (" << buffer[checksumIndex] << - ")" << endl; + checkSum = ConfirmCheckSum (buffer, bytesToConsider, + static_cast<int> (buffer[checksumIndex])); } - if (checkSum != static_cast<int> (buffer[checksumIndex])) + catch (HokuyoError &e) { - stringstream ss; - ss << "Invalid checksum - given: " << static_cast<int> (buffer[checksumIndex]) << - ", calculated: " << checkSum; - throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ()); + if (e.Code () == HOKUYO_ERR_PROTOCOL && _sensorIsUTM30LX && _enableCheckSumWorkaround) + { + // Here comes the UTM-30LX workaround + char* hasComment = strstr (buffer, "<-"); + if (hasComment != NULL) + { + int newBytesToConsider = hasComment - buffer; + if (_verbose) + { + cerr << "HokuyoLaser::" << __func__ << "() Performing UTM-30LX II response " + "checksum workaround: trying with " << newBytesToConsider << + " bytes." << endl; + } + if (newBytesToConsider < 1) + { + stringstream ss; + ss << "Not enough bytes to calculate checksum with: " << newBytesToConsider << + " bytes (line length is " << lineLength << " bytes)."; + throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ()); + } + + checkSum = ConfirmCheckSum (buffer, bytesToConsider, + 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 @@ -2128,94 +2165,47 @@ return statusCode; } -void HokuyoLaser::GetAndSetSCIPVersion (void) +void HokuyoLaser::GetAndSetSCIPVersion () { - bool scip1Failed = false; + bool scip2Failed = false; if (_verbose) cerr << "HokuyoLaser::" << __func__ << "() Testing SCIP protocol version." << endl; - // Try SCIP version 1 first by sending an info command + // Try SCIP version 2 first by sending an info command try { - SendCommand ("V", NULL, 0, NULL); + SendCommand ("VV", NULL, 0, NULL); } catch (HokuyoError) { // That didn't work too well... if (_verbose) - cerr << "HokuyoLaser::" << __func__ << "() Initial SCIP version 1 test failed." << endl; - scip1Failed = true; + cerr << "HokuyoLaser::" << __func__ << "() Initial SCIP version 2 test failed." << endl; + scip2Failed = true; } - if (scip1Failed) + if (scip2Failed) { - // Currently using SCIP version 2 - _scipVersion = 2; + // Currently using SCIP version 1 + // Get the firmware version and check if we can move to SCIP version 2 + _scipVersion = 1; _port->Flush (); - // Make sure by sending a VV command try { - SendCommand ("VV", NULL, 0, NULL); + SendCommand ("V", NULL, 0, NULL); } catch (HokuyoError) { throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed."); } - - // Otherwise all OK, dump the rest of the result - SkipLines (6); - if (_verbose) - cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl; - return; - } - else - { - // Currently using SCIP version 1 - // Get the firmware version and check if we can move to SCIP version 2 - _scipVersion = 1; - // Skip the vendor and product info SkipLines (2); // Get the firmware line char buffer[SCIP1_LINE_LENGTH]; memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH); - try - { - // If the laser is already in SCIP2 mode this has a tendency to time out (rather than - // the laser doing what the manual says and giving us an error code to the command - // that we sent to get the info). - ReadLine (buffer); - } - catch (HokuyoError e) - { - if (e.Code () != HOKUYO_ERR_READ) // We're only interested in timeouts - throw; - if (_verbose) - { - cerr << "HokuyoLaser::" << __func__ << - "() Timed out trying SCIP version 1, trying SCIP version 2." << endl; - } - // Already in SCIP version 2 mode. - _scipVersion = 2; - _port->Flush (); - // Make sure by sending a VV command - try - { - SendCommand ("VV", NULL, 0, NULL); - } - catch (HokuyoError &e) - { - cout << "error: " << e.Code() << " " << e.what() << endl; - throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed."); - } - // Otherwise all OK, dump the rest of the result - SkipLines (6); - if (_verbose) - cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl; - return; - } + ReadLine (buffer); if (strncmp (buffer, "FIRM:", 5) != 0) { @@ -2223,6 +2213,9 @@ "'FIRM:' was not found when checking firmware version."); } // 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 HokuyoError (HOKUYO_ERR_BADFIRMWARE, "Out-of-range firmware version."); @@ -2271,12 +2264,23 @@ return; } } + else + { + // Currently using SCIP version 2 + _scipVersion = 2; + // Dump the rest of the result + SkipLines (6); + if (_verbose) + cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl; + return; + } + // Fallback case if didn't find a good SCIP version and return above throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version."); } -void HokuyoLaser::GetDefaults (void) +void HokuyoLaser::GetDefaults () { if (_verbose) cerr << "HokuyoLaser::" << __func__ << "() Getting default values." << endl; @@ -2536,4 +2540,31 @@ } } +int HokuyoLaser::ConfirmCheckSum (char *buffer, int length, int expectedSum) +{ + 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) + { + cerr << "HokuyoLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" << + static_cast<char> (checkSum) << "), given checksum = " << + static_cast<int> (expectedSum) << " (" << expectedSum << ")" << endl; + } + if (checkSum != expectedSum) + { + stringstream ss; + ss << "Invalid checksum - given: " << expectedSum << ", calculated: " << checkSum; + throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ()); + } + + return checkSum; +} + } // namespace hokuyo_aist Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h =================================================================== --- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2009-04-08 03:30:00 UTC (rev 389) +++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h 2009-04-14 09:11:54 UTC (rev 390) @@ -64,19 +64,19 @@ HokuyoError (unsigned int code, std::string desc) : _errorCode (code), _errorDesc (desc) {} - virtual ~HokuyoError (void) throw () {}; + virtual ~HokuyoError () throw () {}; - virtual unsigned int Code (void) const throw () + virtual unsigned int Code () const throw () { return _errorCode; } - virtual const char* what (void) const throw () + virtual const char* what () const throw () { return _errorDesc.c_str (); } - virtual std::string AsString (void) const throw(); + virtual std::string AsString () const throw(); private: /** Error code. */ @@ -155,14 +155,14 @@ public: friend class HokuyoLaser; - HokuyoSensorInfo (void); + HokuyoSensorInfo (); HokuyoSensorInfo (const HokuyoSensorInfo &rhs); /// @brief Assignment operator. HokuyoSensorInfo& operator= (const HokuyoSensorInfo &rhs); /// @brief Format the entire object into a string. - std::string AsString (void); + std::string AsString (); // Version details. /// Vendor name. @@ -223,8 +223,8 @@ unsigned int scanableSteps; private: - void SetDefaults (void); - void CalculateValues (void); + void SetDefaults (); + void CalculateValues (); }; /** @brief Structure to store data returned from the laser scanner. */ @@ -234,7 +234,7 @@ friend class HokuyoLaser; /// This constructor creates an empty HokuyoData with no data currently allocated. - HokuyoData (void); + HokuyoData (); /// This constructor performs a deep copy of range data. HokuyoData (uint32_t *ranges, unsigned int length, bool error, unsigned int time); /// This constructor performs a deep copy of range and intensity data. @@ -242,27 +242,27 @@ bool error, unsigned int time); /// This copy constructor performs a deep copy of present data. HokuyoData (const HokuyoData &rhs); - ~HokuyoData (void); + ~HokuyoData (); /** @brief Return a pointer to an 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 (void) const { return _ranges; } + const uint32_t* Ranges () const { return _ranges; } /// @brief Return a pointer to an array of intensity readings. - const uint32_t* Intensities (void) const { return _intensities; } + const uint32_t* Intensities () const { return _intensities; } /// @brief Get the number of samples in the data. - unsigned int Length (void) const { return _length; } + 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 ErrorCodeToString to get a textual representation of the error. */ - bool GetErrorStatus (void) const { return _error; } + bool GetErrorStatus () const { return _error; } /// @brief Return a string representing the error for the given error code. std::string ErrorCodeToString (uint32_t errorCode); /** @brief Get the time stamp of the data in milliseconds (only available using SCIP version 2). */ - unsigned int TimeStamp (void) const { return _time; } + unsigned int TimeStamp () const { return _time; } /// @brief Assignment operator. HokuyoData& operator= (const HokuyoData &rhs); @@ -270,10 +270,10 @@ uint32_t operator[] (unsigned int index); /// @brief Format the entire object into a string. - std::string AsString (void); + std::string AsString (); /// @brief Force the data to clean up. - void CleanUp (void); + void CleanUp (); protected: uint32_t *_ranges; @@ -300,8 +300,8 @@ class HOKUYO_AIST_EXPORT HokuyoLaser { public: - HokuyoLaser (void); - ~HokuyoLaser (void); + HokuyoLaser (); + ~HokuyoLaser (); /// @brief Open the laser scanner and begin scanning. void Open (std::string portOptions); @@ -317,10 +317,10 @@ unsigned int OpenWithProbing (std::string portOptions); /// @brief Close the connection to the laser scanner. - void Close (void); + void Close (); /// @brief Checks if the connection to the laser scanner is open. - bool IsOpen (void) const; + bool IsOpen () const; /// @brief Switch the laser scanner on or off. void SetPower (bool on); @@ -334,7 +334,7 @@ /** @brief Reset the laser scanner to its default settings. Not available with the SCIP v1 protocol. */ - void Reset (void); + void Reset (); /** @brief Set the speed at which the scanner's sensor spins. @@ -358,7 +358,7 @@ /** @brief Get the current value of the scanner's clock in milliseconds. Not available with the SCIP v1 protocol. */ - unsigned int GetTime (void); + unsigned int GetTime (); /** @brief Get the latest scan data from the scanner. @@ -470,11 +470,11 @@ double endAngle, unsigned int clusterCount = 1); /// @brief Return the major version of the SCIP protocol in use. - uint8_t SCIPVersion (void) const { return _scipVersion; } + uint8_t SCIPVersion () const { return _scipVersion; } /** @brief Turns on and off printing of verbose operating information to stderr. Default is off. */ - void SetVerbose (bool verbose) { _verbose = verbose; } + void SetVerbose (bool verbose) { _verbose = verbose; } /// @brief A convenience function to convert a step index to an angle. double StepToAngle (unsigned int step); @@ -485,22 +485,24 @@ flexiport::Port *_port; uint8_t _scipVersion; - bool _verbose, _sensorIsUTM30LX; + bool _verbose, _sensorIsUTM30LX, _enableCheckSumWorkaround; double _minAngle, _maxAngle, _resolution; int _firstStep, _lastStep, _frontStep; unsigned int _maxRange; - void ClearReadBuffer (void); + void ClearReadBuffer (); int ReadLine (char *buffer, int expectedLength = -1); int ReadLineWithCheck (char *buffer, int expectedLength = -1, bool hasSemicolon = false); void SkipLines (int count); int SendCommand (const char *cmd, const char *param, int paramLength, const char *extraOK); - void GetAndSetSCIPVersion (void); - void GetDefaults (void); + void GetAndSetSCIPVersion (); + void GetDefaults (); void Read2ByteRangeData (HokuyoData *data, unsigned int numSteps); void Read3ByteRangeData (HokuyoData *data, unsigned int numSteps); void Read3ByteRangeAndIntensityData (HokuyoData *data, unsigned int numSteps); + + int ConfirmCheckSum (char *buffer, int length, int expectedSum); }; } // namespace hokuyo_aist Modified: gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt =================================================================== --- gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt 2009-04-08 03:30:00 UTC (rev 389) +++ gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt 2009-04-14 09:11:54 UTC (rev 390) @@ -20,8 +20,6 @@ GET_FILENAME_COMPONENT (boostPythonLib ${Boost_PYTHON_LIBRARY} NAME_WE CACHE) # Chop off the lib at the front, too, if present STRING (REGEX REPLACE "^lib" "" boostPythonLib ${boostPythonLib}) - MESSAGE (STATUS - "PlayerC++ client library will be built with Boost::Thread support.") else(Boost_PYTHON_FOUND) MESSAGE (STATUS "Boost::Python library was not found. Cannot build Python bindings for Hokuyo_aist.") @@ -66,4 +64,4 @@ ADD_SUBDIRECTORY (test) ELSEIF(NOT GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) MESSAGE (STATUS "Hokuyo_aist Python bindings will not be built - must build hokuyo_aist as a shared library.") -endif(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) \ No newline at end of file +endif(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |