Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[510] gearbox/trunk/src/hokuyo_aist
|
From: <gb...@us...> - 2010-08-08 04:31:25
|
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...
[truncated message content] |