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