Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[505] gearbox/trunk/src/hokuyo_aist
|
From: <gb...@us...> - 2010-08-04 05:30:30
|
Revision: 505
http://gearbox.svn.sourceforge.net/gearbox/?rev=505&view=rev
Author: gbiggs
Date: 2010-08-04 05:30:21 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
Identify laser models
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/scan_data.cpp
gearbox/trunk/src/hokuyo_aist/scan_data.h
gearbox/trunk/src/hokuyo_aist/sensor.cpp
gearbox/trunk/src/hokuyo_aist/sensor.h
gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
gearbox/trunk/src/hokuyo_aist/sensor_info.h
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -32,13 +32,13 @@
ScanData::ScanData()
: _ranges(NULL), _intensities(NULL), _length(0),
- _error(false), _time(0)
+ _error(false), _time(0), _model(MODEL_UNKNOWN)
{
}
ScanData::ScanData(uint32_t *ranges, unsigned int length, bool error,
- unsigned int time)
- : _error(error), _time(time)
+ unsigned int time, LaserModel model)
+ : _error(error), _time(time), _model(model)
{
_length = length;
if (_length == 0)
@@ -64,8 +64,9 @@
}
ScanData::ScanData(uint32_t *ranges, uint32_t *intensities,
- unsigned int length, bool error, unsigned int time)
- : _error(error), _time(time)
+ unsigned int length, bool error, unsigned int time,
+ LaserModel model)
+ : _error(error), _time(time), _model(model)
{
_length = length;
if (_length == 0)
@@ -118,6 +119,7 @@
}
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
ScanData::~ScanData()
@@ -128,7 +130,7 @@
std::string ScanData::error_code_to_string(uint32_t error_code)
{
- if (_sensorIsUTM30LX)
+ if (_model == MODEL_UTM30LX)
{
switch (error_code)
{
@@ -215,6 +217,7 @@
_intensities = NULL;
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
else
{
@@ -265,6 +268,7 @@
_error = rhs.get_error_status();
_time = rhs.time_stamp();
+ _model = rhs.model();
}
return *this;
@@ -281,7 +285,8 @@
{
std::stringstream ss;
- ss << _length << " readings:\n";
+ ss << _length << " readings from model ";
+ ss << model_to_string(_model) << ":\n";
for (unsigned int ii = 0; ii < _length; ii++)
ss << _ranges[ii] << "\t";
if (_intensities != NULL)
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -35,6 +35,8 @@
#define HOKUYO_AIST_EXPORT
#endif
+#include "sensor_info.h"
+
#include <string>
/** @ingroup gbx_library_hokuyo_aist
@@ -57,11 +59,12 @@
ScanData();
/// This constructor performs a deep copy of existing range data.
ScanData(uint32_t* ranges, unsigned int length, bool error,
- unsigned int time);
+ unsigned int time, LaserModel model);
/// This constructor performs a deep copy of existing range and
/// intensity data.
ScanData(uint32_t* ranges, uint32_t* intensities,
- unsigned int length, bool error, unsigned int time);
+ unsigned int length, bool error, unsigned int time,
+ LaserModel model);
/// This copy constructor performs a deep copy of present data.
ScanData(const ScanData &rhs);
~ScanData();
@@ -90,6 +93,8 @@
This value is only available using SCIP version 2). */
unsigned int time_stamp() const { return _time; }
+ /// Get the model of the laser that produced this scan.
+ LaserModel model() const { return _model; }
/// @brief Assignment operator.
ScanData& operator=(const ScanData &rhs);
@@ -110,7 +115,7 @@
unsigned int _length;
bool _error;
unsigned int _time;
- bool _sensorIsUTM30LX;
+ LaserModel _model;
void allocate_data(unsigned int length,
bool include_intensities = false);
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -225,16 +225,16 @@
///////////////////////////////////////////////////////////////////////////////
Sensor::Sensor()
- : _port(NULL), _err_output(std::cerr), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ : _port(NULL), _err_output(std::cerr), _scip_version(2),
+ _verbose(false), _enable_checksum_workaround(false),
_ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
_resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
}
Sensor::Sensor(std::ostream& err_output)
- : _port(NULL), _err_output(err_output), _scip_version(2), _verbose(false),
- _sensorIsUTM30LX(false), _enable_checksum_workaround(false),
+ : _port(NULL), _err_output(err_output), _scip_version(2),
+ _verbose(false), _enable_checksum_workaround(false),
_ignore_unknowns(false), _min_angle(0.0), _max_angle(0.0),
_resolution(0.0), _first_step(0), _last_step(0), _front_step(0)
{
@@ -577,6 +577,8 @@
// Get the product info line
_read_line(buffer);
info->product = &buffer[5];
+ // Only the URG-04LX supports SCIP1
+ _model = MODEL_URG04LX;
// Get the firmware line
_read_line(buffer);
info->firmware = &buffer[5];
@@ -1153,7 +1155,7 @@
// BIG NOTE: The UTM-30LX has a probable bug in its response to the II code
// whereby it does not include anything after and including "<-" in the
// checksum calculation. A workaround is enabled in this function when
-// _sensorIsUTM30LX is true. In this case, if the checksum fails normally, it
+// _model is MODEL_UTM30LX. In this case, if the checksum fails normally, it
// scans the line for "<-" and recalculates the checksum on the bytes up to
// that point. This only happens in SCIP v2.
int Sensor::_read_line_with_check(char *buffer, int expected_length,
@@ -1192,7 +1194,7 @@
}
catch (ProtocolError &e)
{
- if (_sensorIsUTM30LX && _enable_checksum_workaround)
+ if (_model == MODEL_UTM30LX && _enable_checksum_workaround)
{
// Here comes the UTM-30LX workaround
char* hasComment = strstr(buffer, "<-");
@@ -1406,6 +1408,23 @@
return statusCode;
}
+
+void Sensor::_find_model(char const* buffer)
+{
+ if (strstr(buffer, "URG-04LX") != NULL)
+ _model = MODEL_URG04LX;
+ else if (strstr(buffer, "UTM-30LX") != NULL)
+ {
+ _model = MODEL_UTM30LX;
+ // Also enable the work around for a checksum problem in this
+ // model.
+ _enable_checksum_workaround = true;
+ }
+ else
+ _model = MODEL_UNKNOWN;
+}
+
+
void Sensor::_get_and_set_scip_version()
{
bool scip2Failed = false;
@@ -1562,16 +1581,8 @@
else if (strncmp(buffer, "PROD", 4) == 0)
{
info->product = &buffer[5]; // Product info
- // Also do a check to see if this is a UTM-30LX, because we need to
- // work around a checksum problem and the error messages for data
- // change if this is the case
- if (strstr(buffer, "UTM-30LX") != NULL)
- {
- _sensorIsUTM30LX = true;
- _enable_checksum_workaround = true;
- }
- else
- _sensorIsUTM30LX = false;
+ // Find the product model
+ _find_model(&buffer[5]);
}
else if (strncmp(buffer, "FIRM", 4) == 0)
info->firmware = &buffer[5]; // Firmware version
@@ -1673,7 +1684,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
data->_error = false;
// 2 byte data is easy since it fits neatly in a 64-byte block
@@ -1718,7 +1729,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
data->_error = false;
// 3 byte data is a pain because it crosses the line boundary, it may
@@ -1820,7 +1831,7 @@
// This will automatically take care of whether it actually needs to
// (re)allocate or not.
data->allocate_data(num_steps, true);
- data->_sensorIsUTM30LX = _sensorIsUTM30LX;
+ data->_model = _model;
// 3 byte data is a pain because it crosses the line boundary, it may
// overlap by 0, 1 or 2 bytes
Modified: gearbox/trunk/src/hokuyo_aist/sensor.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -292,7 +292,8 @@
std::ostream& _err_output;
uint8_t _scip_version;
- bool _verbose, _sensorIsUTM30LX, _enable_checksum_workaround,
+ LaserModel _model;
+ bool _verbose, _enable_checksum_workaround,
_ignore_unknowns;
double _min_angle, _max_angle, _resolution;
int _first_step, _last_step, _front_step;
@@ -306,6 +307,7 @@
int _send_command(const char *cmd, const char *param, int param_length,
const char *extra_ok);
+ void _find_model(char const* buffer);
void _get_and_set_scip_version();
void _get_defaults();
void _process_vv_line(const char *buffer, SensorInfo *info);
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-04 05:30:21 UTC (rev 505)
@@ -32,7 +32,7 @@
: min_range(0), max_range(0), steps(0), first_step(0), last_step(0),
front_step(0), standard_speed(0), power(false), speed(0), speed_level(0),
baud(0), time(0), min_angle(0.0), max_angle(0.0), resolution(0.0),
- scanable_steps(0)
+ scanable_steps(0), detected_model(MODEL_UNKNOWN)
{
}
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 04:48:00 UTC (rev 504)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-04 05:30:21 UTC (rev 505)
@@ -44,6 +44,26 @@
namespace hokuyo_aist
{
+/// Laser models
+enum LaserModel {
+ MODEL_URG04LX,
+ MODEL_UTM30LX,
+ MODEL_UNKNOWN
+};
+
+HOKUYO_AIST_EXPORT inline char const* model_to_string(LaserModel model)
+{
+ switch(model)
+ {
+ case MODEL_URG04LX:
+ return "URG-04LX";
+ case MODEL_UTM30LX:
+ return "UTM-30LX";
+ default:
+ return "Unknown model";
+ }
+}
+
class Sensor;
/** @brief Sensor information.
@@ -122,13 +142,15 @@
double resolution;
/// Total number of steps in a full scan (lastStep - firstStep).
unsigned int scanable_steps;
+ /// Detected model of the laser.
+ LaserModel detected_model;
private:
void set_defaults();
void calculate_values();
}; // class SensorInfo
-} // namespace hokuyo_aist
+}; // namespace hokuyo_aist
/** @} */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|