diff -up ./player-3.0.2/server/drivers/gps/gbxgarminacfr.cc.gbx10 ./player-3.0.2/server/drivers/gps/gbxgarminacfr.cc
--- ./player-3.0.2/server/drivers/gps/gbxgarminacfr.cc.gbx10 2010-06-28 00:12:42.000000000 -0400
+++ ./player-3.0.2/server/drivers/gps/gbxgarminacfr.cc 2012-08-12 18:52:11.860249699 -0400
@@ -44,7 +44,8 @@ should include most GPS devices that use
@par Provides
-- @ref interface_gps: Output GPS interface
+- @ref interface_gps
+ - Output GPS interface
@par Supported configuration requests
@@ -152,10 +153,11 @@ GbxGarminAcfr::~GbxGarminAcfr (void)
int GbxGarminAcfr::MainSetup (void)
{
// Validate the configuration
- //std::string reason;
- if (!_config.isValid ())
+ std::string reason;
+ if (!_config.isValid (reason))
{
- PLAYER_ERROR("GbxGarminAcfr: Invalid sensor configuration.\n");//, reason.c_str());
+ //PLAYER_ERROR1("GbxGarminAcfr: Invalid sensor configuration: %s.\n", reason.c_str());
+ PLAYER_ERROR("GbxGarminAcfr: Invalid sensor configuration.\n");
return -1;
}
@@ -186,8 +188,6 @@ void GbxGarminAcfr::MainQuit (void)
int GbxGarminAcfr::ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data)
{
- // Check for capability requests
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
return -1;
}
diff -up ./player-3.0.2/server/drivers/opaque/CMakeLists.txt.gbx10 ./player-3.0.2/server/drivers/opaque/CMakeLists.txt
--- ./player-3.0.2/server/drivers/opaque/CMakeLists.txt.gbx10 2010-06-28 00:12:42.000000000 -0400
+++ ./player-3.0.2/server/drivers/opaque/CMakeLists.txt 2012-08-12 18:52:11.860249699 -0400
@@ -1,6 +1,6 @@
PLAYERDRIVER_OPTION (flexiport build_flexiport ON)
PLAYERDRIVER_REQUIRE_PKG (flexiport build_flexiport flexiport flexiport_includeDirs
- flexiport_libDirs flexiport_linkLibs flexiport_linkFlags flexiport_cFlags =1.0.0)
+ flexiport_libDirs flexiport_linkLibs flexiport_linkFlags flexiport_cFlags >=1.0.0)
PLAYERDRIVER_ADD_DRIVER (flexiport build_flexiport
INCLUDEDIRS ${flexiport_includeDirs} LIBDIRS ${flexiport_libDirs}
LINKLIBS ${flexiport_linkLibs} LINKFLAGS ${flexiport_linkFlags} CFLAGS ${flexiport_cFlags}
diff -up ./player-3.0.2/server/drivers/opaque/flexiport.cc.gbx10 ./player-3.0.2/server/drivers/opaque/flexiport.cc
--- ./player-3.0.2/server/drivers/opaque/flexiport.cc.gbx10 2009-01-24 18:14:21.000000000 -0500
+++ ./player-3.0.2/server/drivers/opaque/flexiport.cc 2012-08-12 18:52:11.860249699 -0400
@@ -88,7 +88,7 @@ driver
#include <flexiport/flexiport.h>
#include <flexiport/port.h>
#include <string>
-
+#include "unistd.h"
const int DEFAULT_OPAQUE_BUFFER_SIZE = 4096;
////////////////////////////////////////////////////////////////////////////////////////////////////
diff -up ./player-3.0.2/server/drivers/ranger/CMakeLists.txt.gbx10 ./player-3.0.2/server/drivers/ranger/CMakeLists.txt
--- ./player-3.0.2/server/drivers/ranger/CMakeLists.txt.gbx10 2009-12-15 19:51:26.000000000 -0500
+++ ./player-3.0.2/server/drivers/ranger/CMakeLists.txt 2012-08-12 18:52:11.860249699 -0400
@@ -7,14 +7,14 @@ PLAYERDRIVER_ADD_DRIVER (gbxsickacfr bui
LINKLIBS ${gbxsickacfr_linkLibs} LINKFLAGS ${gbxsickacfr_linkFlags}
CFLAGS ${gbxsickacfr_cFlags} SOURCES gbxsickacfr.cc)
-PLAYERDRIVER_OPTION (hokuyo_aist build_hokuyo_aist ON)
-PLAYERDRIVER_REQUIRE_PKG (hokuyo_aist build_hokuyo_aist hokuyo_aist
- hokuyo_aist_includeDirs hokuyo_aist_libDirs hokuyo_aist_linkLibs
- hokuyo_aist_linkFlags hokuyo_aist_cFlags =1.0.0)
-PLAYERDRIVER_ADD_DRIVER (hokuyo_aist build_hokuyo_aist
- INCLUDEDIRS ${hokuyo_aist_includeDirs} LIBDIRS ${hokuyo_aist_libDirs}
- LINKLIBS ${hokuyo_aist_linkLibs} LINKFLAGS ${hokuyo_aist_linkFlags}
- CFLAGS ${hokuyo_aist_cFlags} SOURCES hokuyo_aist.cc)
+PLAYERDRIVER_OPTION (hokuyoaist build_hokuyoaist ON)
+PLAYERDRIVER_REQUIRE_PKG (hokuyoaist build_hokuyoaist hokuyoaist
+ hokuyoaist_includeDirs hokuyoaist_libDirs hokuyoaist_linkLibs
+ hokuyoaist_linkFlags hokuyoaist_cFlags >=3.0.0)
+PLAYERDRIVER_ADD_DRIVER (hokuyoaist build_hokuyoaist
+ INCLUDEDIRS ${hokuyoaist_includeDirs} LIBDIRS ${hokuyoaist_libDirs}
+ LINKLIBS ${hokuyoaist_linkLibs} LINKFLAGS ${hokuyoaist_linkFlags}
+ CFLAGS ${hokuyoaist_cFlags} SOURCES hokuyo_aist.cc)
PLAYERDRIVER_OPTION (lasertoranger build_lasertoranger ON)
PLAYERDRIVER_ADD_DRIVER (lasertoranger build_lasertoranger SOURCES toranger.cc lasertoranger.cc)
diff -up ./player-3.0.2/server/drivers/ranger/hokuyo_aist.cc.gbx10 ./player-3.0.2/server/drivers/ranger/hokuyo_aist.cc
--- ./player-3.0.2/server/drivers/ranger/hokuyo_aist.cc.gbx10 2009-09-06 22:04:21.000000000 -0400
+++ ./player-3.0.2/server/drivers/ranger/hokuyo_aist.cc 2012-08-12 18:59:12.308477434 -0400
@@ -17,25 +17,25 @@
*/
/*
- Desc: Wrapper driver around the Gearbox hokuyo_aist library (see http://gearbox.sourceforge.net)
+ Desc: Wrapper driver around the HokuyoAIST library (see https://github.com/gbiggs/hokuyoaist)
Author: Geoffrey Biggs
Date: 20 June 2008
- CVS: $Id: hokuyo_aist.cc 8243 2009-09-07 02:04:21Z gbiggs $
+ CVS: $Id: hokuyo_aist.cc 9071 2011-08-11 01:31:19Z gbiggs $
*/
/** @ingroup drivers */
/** @{ */
-/** @defgroup driver_hokuyo_aist hokuyo_aist
- * @brief Gearbox hokuyo_aist Hokuyo laser scanner driver library
+/** @defgroup driver_hokuyoaist hokuyoaist
+ * @brief HokuyoAIST Hokuyo laser scanner driver library
- This driver provides a @ref interface_ranger interface to the hokuyo_aist Hokuyo laser scanner
- driver provided by Gearbox. Communication with the laser is via the Gearbox Flexiport library. The
+ This driver provides a @ref interface_ranger interface to the HokuyoAIST Hokuyo laser scanner
+ driver. Communication with the laser is via the Flexiport library. The
driver supports the SCIP protocol versions 1 and 2.
@par Compile-time dependencies
- - Gearbox library hokuyo_aist
- - Gearbox library flexiport
+ - HokuyoAIST 3.0
+ - Flexiport 2.0
@par Provides
@@ -75,6 +75,10 @@
- max_angle (float, radians)
- Default: 4.0 rad (Will use laser default)
- Maximum scan angle to return. Will be adjusted if outside the laser's scannable range.
+ - invert (boolean)
+ - Default: false
+ - If true, the reading will be inverted (i.e. read backwards). Useful if the laser scanner is
+ mounted upside down.
- power (boolean)
- Default: true
- If true, the sensor power will be switched on upon driver activation (i.e. when the first
@@ -91,12 +95,12 @@
- baud_rate (integer)
- Default: 19200bps
- - Change the baud rate of the connection to the laser. See hokuyo_aist documentation for valid
+ - Change the baud rate of the connection to the laser. See HokuyoAIST documentation for valid
values. This is separate from the scanner's power-on default baud rate, which should be
specified in portopts.
- speed_level (integer, 0 to 10 or 99)
- Default: 0
- - The speed at which the laser operates, as a level down from maximum speed. See the hokuyo_aist
+ - The speed at which the laser operates, as a level down from maximum speed. See the HokuyoAIST
library documentation for suitable values.
- high_sensitivity (integer)
- Default: 0
@@ -108,14 +112,14 @@
- hw_timestamps (boolean)
- Default: false
- When false, the server will use server time stamps in data messages. When true, the time stamp
- in the laser data will be used.
+ in the laser data will be used, offset to the current system time.
@par Example
@verbatim
driver
(
- name "hokuyo_aist"
+ name "hokuyoaist"
provides ["ranger:0"]
portopts "type=serial,device=/dev/ttyS0,timeout=1"
)
@@ -128,7 +132,7 @@
#include <string>
-#include <hokuyo_aist/hokuyo_aist.h>
+#include <hokuyoaist/hokuyoaist.h>
#include <libplayercore/playercore.h>
const int DEFAULT_BAUDRATE = 19200;
@@ -144,559 +148,646 @@ const bool DEFAULT_TIMESTAMPS = false;
class HokuyoDriver : public ThreadedDriver
{
- public:
- HokuyoDriver (ConfigFile* cf, int section);
- ~HokuyoDriver (void);
-
- virtual int MainSetup (void);
- virtual void MainQuit (void);
- virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data);
-
- private:
- virtual void Main (void);
- bool ReadLaser (void);
- bool AllocateDataSpace (void);
-
- // Configuration parameters
- bool _verbose, _powerOnStartup, _getIntensities, _ignoreUnknowns;
- double _minAngle, _maxAngle;
- IntProperty _baudRate, _speedLevel, _highSensitivity;
- DoubleProperty _minDist;
- BoolProperty _hwTimeStamps;
- std::string _portOpts;
- // Geometry
- player_ranger_geom_t _geom;
- player_pose3d_t _sensorPose;
- player_bbox3d_t _sensorSize;
- // The hardware device itself
- hokuyo_aist::HokuyoLaser _device;
- // Data storage
- hokuyo_aist::HokuyoData _data;
- double *_ranges;
- double *_intensities;
- int _numRanges;
+ public:
+ HokuyoDriver(ConfigFile* cf, int section);
+ ~HokuyoDriver(void);
+
+ virtual int MainSetup(void);
+ virtual void MainQuit(void);
+ virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr
+ *hdr, void *data);
+
+ private:
+ virtual void Main(void);
+ bool ReadLaser(void);
+ bool AllocateDataSpace(void);
+
+ // Configuration parameters
+ bool verbose_, invert_, powerOnStartup_, getIntensities_,
+ ignoreUnknowns_;
+ double minAngle_, maxAngle_;
+ IntProperty baudRate_, speedLevel_, highSensitivity_;
+ DoubleProperty minDist_;
+ BoolProperty hwTimeStamps_;
+ std::string portOpts_;
+ // Geometry
+ player_ranger_geom_t geom_;
+ player_pose3d_t sensorPose_;
+ player_bbox3d_t sensorSize_;
+ // The hardware device itself
+ hokuyoaist::Sensor device_;
+ // Data storage
+ hokuyoaist::ScanData data_;
+ double *_ranges;
+ double *_intensities;
+ int _numRanges;
};
////////////////////////////////////////////////////////////////////////////////////////////////////
// Constructor/destructor
////////////////////////////////////////////////////////////////////////////////////////////////////
-HokuyoDriver::HokuyoDriver (ConfigFile* cf, int section) :
- ThreadedDriver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE),
- _baudRate ("baud_rate", DEFAULT_BAUDRATE, false),
- _speedLevel ("speed_level", DEFAULT_SPEED_LEVEL, false),
- _highSensitivity ("high_sensitivity", DEFAULT_SENSITIVITY, false),
- _minDist ("min_dist", DEFAULT_MIN_DIST, false),
- _hwTimeStamps ("hw_timestamps", DEFAULT_TIMESTAMPS, false),
- _ranges (NULL), _intensities (NULL)
-{
- // Get the baudrate, speed and sensitivity
- RegisterProperty ("baud_rate", &_baudRate, cf, section);
- RegisterProperty ("speed_level", &_speedLevel, cf, section);
- RegisterProperty ("high_sensitivity", &_highSensitivity, cf, section);
- RegisterProperty ("min_dist", &_minDist, cf, section);
- RegisterProperty ("hw_timestamps", &_hwTimeStamps, cf, section);
-
- // Get config
- _getIntensities = cf->ReadBool (section, "get_intensities", false);
- _minAngle = cf->ReadFloat (section, "min_angle", -4.0);
- _maxAngle = cf->ReadFloat (section, "max_angle", 4.0);
- _portOpts = cf->ReadString (section, "portopts", "type=serial,device=/dev/ttyACM0,timeout=1");
- _verbose = cf->ReadBool (section, "verbose", false);
- _ignoreUnknowns = cf->ReadBool (section, "ignoreunknowns", false);
- _powerOnStartup = cf->ReadBool (section, "power", true);
-
- // Set up geometry information
- _geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0);
- _geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0);
- _geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0);
- _geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0);
- _geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0);
- _geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0);
- _geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0);
- _geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0);
- _geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0);
- _geom.element_poses_count = 1;
- _geom.element_poses = &_sensorPose;
- memcpy(_geom.element_poses, &_geom.pose, sizeof (_geom.pose));
- _geom.element_sizes_count = 1;
- _geom.element_sizes = &_sensorSize;
- memcpy(_geom.element_sizes, &_geom.size, sizeof (_geom.size));
+HokuyoDriver::HokuyoDriver(ConfigFile* cf, int section) :
+ ThreadedDriver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
+ PLAYER_RANGER_CODE),
+ baudRate_("baud_rate", DEFAULT_BAUDRATE, false),
+ speedLevel_("speed_level", DEFAULT_SPEED_LEVEL, false),
+ highSensitivity_("high_sensitivity", DEFAULT_SENSITIVITY, false),
+ minDist_("min_dist", DEFAULT_MIN_DIST, false),
+ hwTimeStamps_("hw_timestamps", DEFAULT_TIMESTAMPS, false),
+ _ranges(NULL), _intensities(NULL)
+{
+ // Get the baudrate, speed and sensitivity
+ RegisterProperty("baud_rate", &baudRate_, cf, section);
+ RegisterProperty("speed_level", &speedLevel_, cf, section);
+ RegisterProperty("high_sensitivity", &highSensitivity_, cf, section);
+ RegisterProperty("min_dist", &minDist_, cf, section);
+ RegisterProperty("hw_timestamps", &hwTimeStamps_, cf, section);
+
+ // Get config
+ getIntensities_ = cf->ReadBool(section, "get_intensities", false);
+ minAngle_ = cf->ReadFloat(section, "min_angle", -4.0);
+ maxAngle_ = cf->ReadFloat(section, "max_angle", 4.0);
+ invert_ = cf->ReadBool(section, "invert", false);
+ portOpts_ = cf->ReadString(section, "portopts",
+ "type=serial,device=/dev/ttyACM0,timeout=1");
+ verbose_ = cf->ReadBool(section, "verbose", false);
+ ignoreUnknowns_ = cf->ReadBool(section, "ignoreunknowns", false);
+ powerOnStartup_ = cf->ReadBool(section, "power", true);
+
+ // Set up geometry information
+ geom_.pose.px = cf->ReadTupleLength(section, "pose", 0, 0.0);
+ geom_.pose.py = cf->ReadTupleLength(section, "pose", 1, 0.0);
+ geom_.pose.pz = cf->ReadTupleLength(section, "pose", 2, 0.0);
+ geom_.pose.proll = cf->ReadTupleAngle(section, "pose", 3, 0.0);
+ geom_.pose.ppitch = cf->ReadTupleAngle(section, "pose", 4, 0.0);
+ geom_.pose.pyaw = cf->ReadTupleAngle(section, "pose", 5, 0.0);
+ geom_.size.sw = cf->ReadTupleLength(section, "size", 0, 0.0);
+ geom_.size.sl = cf->ReadTupleLength(section, "size", 1, 0.0);
+ geom_.size.sh = cf->ReadTupleLength(section, "size", 2, 0.0);
+ geom_.element_poses_count = 1;
+ geom_.element_poses = &sensorPose_;
+ memcpy(geom_.element_poses, &geom_.pose, sizeof(geom_.pose));
+ geom_.element_sizes_count = 1;
+ geom_.element_sizes = &sensorSize_;
+ memcpy(geom_.element_sizes, &geom_.size, sizeof(geom_.size));
- // Turn on/off verbose mode
- _device.SetVerbose (_verbose);
+ // Turn on/off verbose mode
+ device_.set_verbose(verbose_);
}
-HokuyoDriver::~HokuyoDriver (void)
+HokuyoDriver::~HokuyoDriver(void)
{
- if (_ranges != NULL)
- delete[] _ranges;
- if (_intensities != NULL)
- delete[] _intensities;
+ if (_ranges != NULL)
+ delete[] _ranges;
+ if (_intensities != NULL)
+ delete[] _intensities;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// Driver implementation
////////////////////////////////////////////////////////////////////////////////////////////////////
-bool HokuyoDriver::AllocateDataSpace (void)
+bool HokuyoDriver::AllocateDataSpace(void)
{
- if (_ranges != NULL)
- delete _ranges;
+ if (_ranges != NULL)
+ delete _ranges;
- _numRanges = _device.AngleToStep (_maxAngle) - _device.AngleToStep (_minAngle) + 1;
- if ((_ranges = new double[_numRanges]) == NULL)
- {
- PLAYER_ERROR1 ("hokuyo_aist: Failed to allocate space for %d range readings.", _numRanges);
- return false;
- }
+ _numRanges = device_.angle_to_step(maxAngle_) -
+ device_.angle_to_step(minAngle_) + 1;
+ if ((_ranges = new double[_numRanges]) == NULL)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Failed to allocate space for %d range readings.",
+ _numRanges);
+ return false;
+ }
+
+ if (getIntensities_)
+ {
+ if ((_intensities = new double[_numRanges]) == NULL)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Failed to allocate space for %d intensity readings.",
+ _numRanges);
+ return false;
+ }
+ }
- if (_getIntensities)
- {
- if ((_intensities = new double[_numRanges]) == NULL)
- {
- PLAYER_ERROR1 ("hokuyo_aist: Failed to allocate space for %d intensity readings.",
- _numRanges);
- return false;
- }
- }
-
- return true;
+ return true;
}
-void HokuyoDriver::Main (void)
+void HokuyoDriver::Main(void)
{
- while (true)
- {
- ProcessMessages ();
-
- if (!ReadLaser ())
- break;
- }
+ while (true)
+ {
+ ProcessMessages();
+
+ if (!ReadLaser())
+ break;
+ }
}
-int HokuyoDriver::ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data)
+int HokuyoDriver::ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr,
+ void *data)
{
- // Check for capability requests
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_GEOM);
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG);
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_SET_CONFIG);
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_POWER);
- HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
- PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS);
-
- // Property handlers that need to be done manually due to calling into the hokuyo_aist library.
- if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ, this->device_addr))
- {
- player_intprop_req_t *req = reinterpret_cast<player_intprop_req_t*> (data);
- // Change in the baud rate
- if (strncmp (req->key, "baud_rate", 9) == 0)
- {
- try
- {
- // Change the baud rate
- _device.SetBaud (req->value);
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
-#if defined (WIN32)
- if (e.Code () != HOKUYO_ERR_NOTSERIAL)
-#else
- if (e.Code () != hokuyo_aist::HOKUYO_ERR_NOTSERIAL)
-#endif
- {
- PLAYER_ERROR2 ("hokuyo_aist: Error while changing baud rate: (%d) %s",
- e.Code (), e.what ());
- SetError (e.Code ());
- }
- else
- {
- PLAYER_WARN (
- "hokuyo_aist: Cannot change the baud rate of a non-serial connection.");
- }
-
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ,
- NULL, 0, NULL);
- return 0;
- }
- _baudRate.SetValueFromMessage (data);
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL,
- 0, NULL);
- return 0;
- }
- else if (strncmp (req->key, "speed_level", 11) == 0)
- {
- try
- {
- _device.SetMotorSpeed (req->value);
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Error while changing motor speed: (%d) %s",
- e.Code (), e.what ());
- SetError (e.Code ());
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ,
- NULL, 0, NULL);
- return 0;
- }
- _speedLevel.SetValueFromMessage (data);
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL,
- 0, NULL);
- return 0;
- }
- else if (strncmp (req->key, "high_sensitivity", 16) == 0)
- {
- try
- {
- _device.SetHighSensitivity (req->value != 0);
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Error while changing sensitivity: (%d) %s",
- e.Code (), e.what ());
- SetError (e.Code ());
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ,
- NULL, 0, NULL);
- return 0;
- }
- _highSensitivity.SetValueFromMessage (data);
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL,
- 0, NULL);
- return 0;
- }
- }
+ // Check for capability requests
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_GEOM);
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG);
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_SET_CONFIG);
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_POWER);
+ HANDLE_CAPABILITY_REQUEST(device_addr, resp_queue, hdr, data,
+ PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS);
+
+ // Property handlers that need to be done manually due to calling into the
+ // HokuyoAIST library.
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ,
+ this->device_addr))
+ {
+ player_intprop_req_t *req =
+ reinterpret_cast<player_intprop_req_t*> (data);
+ // Change in the baud rate
+ if (strncmp(req->key, "baud_rate", 9) == 0)
+ {
+ //try
+ //{
+ // Change the baud rate
+ // device_.set_baud(req->value);
+ //}
+ //catch(hokuyoaist::NotSerialError)
+ //{
+ PLAYER_WARN(
+ "HokuyoAIST: Cannot change the baud rate of a non-serial connection.");
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ //}
+ /*
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Error while changing baud rate: %s",
+ e.what());
+ SetError(-1);
+
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ }
+ */
+ baudRate_.SetValueFromMessage(data);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ }
+ else if (strncmp(req->key, "speed_level", 11) == 0)
+ {
+ try
+ {
+ device_.set_motor_speed(req->value);
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Error while changing motor speed: %s",
+ e.what());
+ SetError(-1);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ }
+ speedLevel_.SetValueFromMessage(data);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL,
+ 0, NULL);
+ return 0;
+ }
+ else if (strncmp(req->key, "high_sensitivity", 16) == 0)
+ {
+ try
+ {
+ device_.set_high_sensitivity(req->value != 0);
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1(
+ "HokuyoAIST: Error while changing sensitivity: %s",
+ e.what());
+ SetError(-1);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ }
+ highSensitivity_.SetValueFromMessage(data);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_SET_INTPROP_REQ, NULL, 0, NULL);
+ return 0;
+ }
+ }
+
+ // Standard ranger messages
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_POWER, device_addr))
+ {
+ player_ranger_power_config_t *config =
+ reinterpret_cast<player_ranger_power_config_t*> (data);
+ try
+ {
+ if (config->state)
+ device_.set_power(true);
+ else
+ device_.set_power(false);
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Error while setting power state: %s",
+ e.what());
+ SetError(-1);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_RANGER_REQ_POWER, NULL, 0, NULL);
+ }
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_RANGER_REQ_POWER, NULL, 0, NULL);
+ return 0;
+ }
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_INTNS, device_addr))
+ {
+ bool newValue =
+ (reinterpret_cast<player_ranger_intns_config_t*>(data)->state != 0);
+ if (newValue && !getIntensities_)
+ {
+ // State change, allocate space for intensity data
+ if ((_intensities = new double[_numRanges]) == NULL)
+ {
+ PLAYER_ERROR1(
+ "HokuyoAIST: Failed to allocate space for %d intensity readings.",
+ _numRanges);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_RANGER_REQ_INTNS, NULL, 0, NULL);
+ return 0;
+ }
+ }
+ else if (!newValue && getIntensities_)
+ {
+ // State change, remove allocated space
+ delete[] _intensities;
+ _intensities = NULL;
+ }
+ getIntensities_ = newValue;
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_RANGER_REQ_INTNS, NULL, 0, NULL);
+ return 0;
+ }
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_GEOM, device_addr))
+ {
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_RANGER_REQ_GET_GEOM, &geom_, sizeof(geom_), NULL);
+ return 0;
+ }
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_CONFIG, device_addr))
+ {
+ player_ranger_config_t rangerConfig;
+ hokuyoaist::SensorInfo info;
+ device_.get_sensor_info(info);
+
+ if (!invert_)
+ {
+ rangerConfig.min_angle = minAngle_; // These two are user-configurable
+ rangerConfig.max_angle = maxAngle_;
+ }
+ else
+ {
+ rangerConfig.min_angle = -maxAngle_;
+ rangerConfig.max_angle = -minAngle_;
+ }
+
+ rangerConfig.angular_res = info.resolution;
+ rangerConfig.min_range = info.min_range / 1000.0;
+ rangerConfig.max_range = info.max_range / 1000.0;
+ rangerConfig.range_res = 0.001; // 1mm
+ rangerConfig.frequency = info.speed / 60.0;
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_RANGER_REQ_GET_CONFIG, &rangerConfig,
+ sizeof(rangerConfig), NULL);
+ return 0;
+ }
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_SET_CONFIG, device_addr))
+ {
+ player_ranger_config_t *newParams =
+ reinterpret_cast<player_ranger_config_t*> (data);
+
+ if (!invert_)
+ {
+ minAngle_ = newParams->min_angle;
+ maxAngle_ = newParams->max_angle;
+ }
+ else
+ {
+ minAngle_ = -newParams->max_angle;
+ maxAngle_ = -newParams->min_angle;
+ }
+
+ if (!AllocateDataSpace())
+ {
+ PLAYER_ERROR(
+ "HokuyoAIST: Failed to allocate space for storing range data.");
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL);
+ return 0;
+ }
+
+ try
+ {
+ hokuyoaist::SensorInfo info;
+ device_.get_sensor_info(info);
+ if (minAngle_ < info.min_angle)
+ {
+ minAngle_ = info.min_angle;
+ PLAYER_WARN1("HokuyoAIST: Adjusted min_angle to %lf", minAngle_);
+ }
+ if (maxAngle_> info.max_angle)
+ {
+ maxAngle_ = info.max_angle;
+ PLAYER_WARN1("HokuyoAIST: Adjusted max_angle to %lf", maxAngle_);
+ }
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1(
+ "HokuyoAIST: Library error while changing settings: %s",
+ e.what());
+ SetError(-1);
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL);
+ return 0;
+ }
+
+ Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_RANGER_REQ_GET_CONFIG, newParams, sizeof(*newParams),
+ NULL);
+ return 0;
+ }
- // Standard ranger messages
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_POWER, device_addr))
- {
- player_ranger_power_config_t *config =
- reinterpret_cast<player_ranger_power_config_t*> (data);
- try
- {
- if (config->state)
- _device.SetPower (true);
- else
- _device.SetPower (false);
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Error while setting power state: (%d) %s",
- e.Code (), e.what ());
- SetError (e.Code ());
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_POWER,
- NULL, 0, NULL);
- }
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_POWER, NULL,
- 0, NULL);
- return 0;
- }
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS, device_addr))
- {
- bool newValue = (reinterpret_cast<player_ranger_intns_config_t*> (data)->state != 0);
- if (newValue && !_getIntensities)
- {
- // State change, allocate space for intensity data
- if ((_intensities = new double[_numRanges]) == NULL)
- {
- PLAYER_ERROR1 ("hokuyo_aist: Failed to allocate space for %d intensity readings.",
- _numRanges);
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_INTNS,
- NULL, 0, NULL);
- return 0;
- }
- }
- else if (!newValue && _getIntensities)
- {
- // State change, remove allocated space
- delete[] _intensities;
- _intensities = NULL;
- }
- _getIntensities = newValue;
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_INTNS, NULL,
- 0, NULL);
- return 0;
- }
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_GEOM,
- device_addr))
- {
- Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_GEOM,
- &_geom, sizeof (_geom), NULL);
- return 0;
- }
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG,
- device_addr))
- {
- player_ranger_config_t rangerConfig;
- hokuyo_aist::HokuyoSensorInfo info;
- _device.GetSensorInfo (&info);
-
- rangerConfig.min_angle = _minAngle; // These two are user-configurable
- rangerConfig.max_angle = _maxAngle;
- rangerConfig.angular_res = info.resolution;
- rangerConfig.min_range = info.minRange / 1000.0;
- rangerConfig.max_range = info.maxRange / 1000.0;
- rangerConfig.range_res = 0.001; // 1mm
- rangerConfig.frequency = info.speed / 60.0;
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG,
- &rangerConfig, sizeof (rangerConfig), NULL);
- return 0;
- }
- else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_SET_CONFIG,
- device_addr))
- {
- player_ranger_config_t *newParams = reinterpret_cast<player_ranger_config_t*> (data);
-
- _minAngle = newParams->min_angle;
- _maxAngle = newParams->max_angle;
- if (!AllocateDataSpace ())
- {
- PLAYER_ERROR ("hokuyo_aist: Failed to allocate space for storing range data.");
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG,
- NULL, 0, NULL);
- return 0;
- }
-
- try
- {
- hokuyo_aist::HokuyoSensorInfo info;
- _device.GetSensorInfo (&info);
- if (_minAngle < info.minAngle)
- {
- _minAngle = info.minAngle;
- PLAYER_WARN1 ("hokuyo_aist: Adjusted min_angle to %lf", _minAngle);
- }
- if (_maxAngle> info.maxAngle)
- {
- _maxAngle = info.maxAngle;
- PLAYER_WARN1 ("hokuyo_aist: Adjusted max_angle to %lf", _maxAngle);
- }
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Library error while changing settings: (%d) %s", e.Code (),
- e.what ());
- SetError (e.Code ());
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG,
- NULL, 0, NULL);
- return 0;
- }
-
- Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG,
- newParams, sizeof (*newParams), NULL);
- return 0;
- }
-
- return -1;
+ return -1;
}
-bool HokuyoDriver::ReadLaser (void)
+bool HokuyoDriver::ReadLaser(void)
{
- double time1, time2;
- if (_getIntensities)
- {
- player_ranger_data_range_t rangeData;
- player_ranger_data_intns_t intensityData;
-
- try
- {
- GlobalTime->GetTimeDouble (&time1);
- _device.GetNewRangesAndIntensitiesByAngle (&_data, _minAngle, _maxAngle);
- GlobalTime->GetTimeDouble (&time2);
- time1 = (time1 + time2) / 2.0;
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Failed to read scan: (%d) %s", e.Code (), e.what ());
- SetError (e.Code ());
- return false;
- }
-
- double lastValidValue = _minDist;
- for (unsigned int ii = 0; ii < _data.Length (); ii++)
- {
- _ranges[ii] = _data[ii] / 1000.0f;
- _intensities[ii] = _data.Intensities ()[ii];
- if (_minDist > 0)
- {
- if (_ranges[ii] < _minDist)
- _ranges[ii] = lastValidValue;
- else
- lastValidValue = _ranges[ii];
- }
- }
-
- rangeData.ranges = _ranges;
- rangeData.ranges_count = _data.Length ();
- if (_hwTimeStamps.GetValue ())
- {
- double ts = _data.TimeStamp () / 1000.0;
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
- reinterpret_cast<void*> (&rangeData), sizeof (rangeData), &ts);
- }
- else
- {
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
- reinterpret_cast<void*> (&rangeData), sizeof (rangeData), &time1);
- }
-
- intensityData.intensities = _intensities;
- intensityData.intensities_count = _data.Length ();
- if (_hwTimeStamps.GetValue ())
- {
- double ts = _data.TimeStamp () / 1000.0;
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS,
- reinterpret_cast<void*> (&intensityData), sizeof (intensityData), &ts);
- }
- else
- {
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS,
- reinterpret_cast<void*> (&intensityData), sizeof (intensityData), &time1);
- }
- }
- else
- {
- player_ranger_data_range_t rangeData;
-
- try
- {
- GlobalTime->GetTimeDouble (&time1);
- _device.GetRangesByAngle (&_data, _minAngle, _maxAngle);
- GlobalTime->GetTimeDouble (&time2);
- time1 = (time1 + time2) / 2.0;
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Failed to read scan: (%d) %s", e.Code (), e.what ());
- SetError (e.Code ());
- return false;
- }
-
- double lastValidValue = _minDist;
- for (unsigned int ii = 0; ii < _data.Length (); ii++)
- {
- _ranges[ii] = _data[ii] / 1000.0f;
- if (_minDist > 0)
- {
- if (_ranges[ii] < _minDist)
- _ranges[ii] = lastValidValue;
- else
- lastValidValue = _ranges[ii];
- }
- }
- rangeData.ranges = _ranges;
- rangeData.ranges_count = _data.Length ();
- if (_hwTimeStamps.GetValue ())
- {
- double ts = _data.TimeStamp () / 1000.0;
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
- reinterpret_cast<void*> (&rangeData), sizeof (rangeData), &ts);
- }
- else
- {
- Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
- reinterpret_cast<void*> (&rangeData), sizeof (rangeData), &time1);
- }
- }
+ double time1, time2;
+ if (getIntensities_)
+ {
+ player_ranger_data_range_t rangeData;
+ player_ranger_data_intns_t intensityData;
+
+ try
+ {
+ GlobalTime->GetTimeDouble(&time1);
+ device_.get_new_ranges_intensities_by_angle(data_, minAngle_,
+ maxAngle_);
+ GlobalTime->GetTimeDouble(&time2);
+ time1 = (time1 + time2) / 2.0;
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Failed to read scan: %s", e.what());
+ SetError(-1);
+ return false;
+ }
+
+ double lastValidValue = minDist_;
+ if (!invert_)
+ {
+ for (unsigned int ii = 0; ii < data_.ranges_length(); ii++)
+ {
+ _ranges[ii] = data_[ii] / 1000.0f;
+ _intensities[ii] = data_.intensities()[ii];
+ if (minDist_ > 0)
+ {
+ if (_ranges[ii] < minDist_)
+ _ranges[ii] = lastValidValue;
+ else
+ lastValidValue = _ranges[ii];
+ }
+ }
+ }
+ else // Invert
+ {
+ for (unsigned int ii = 0; ii < data_.ranges_length(); ii++)
+ {
+ unsigned int jj = data_.ranges_length() - 1 - ii;
+ _ranges[ii] = data_[jj] / 1000.0f;
+ _intensities[ii] = data_.intensities()[jj];
+ if (minDist_ > 0)
+ {
+ if (_ranges[ii] < minDist_)
+ _ranges[ii] = lastValidValue;
+ else
+ lastValidValue = _ranges[ii];
+ }
+ }
+ }
+
+ rangeData.ranges = _ranges;
+ rangeData.ranges_count = data_.ranges_length();
+ if (hwTimeStamps_.GetValue())
+ {
+ double ts = data_.system_time_stamp() / 1000000000.0;
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
+ reinterpret_cast<void*>(&rangeData), sizeof(rangeData),
+ &ts);
+ }
+ else
+ {
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
+ reinterpret_cast<void*>(&rangeData), sizeof(rangeData),
+ &time1);
+ }
+
+ intensityData.intensities = _intensities;
+ intensityData.intensities_count = data_.intensities_length();
+ if (hwTimeStamps_.GetValue())
+ {
+ double ts = data_.system_time_stamp() / 1000000000.0;
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS,
+ reinterpret_cast<void*>(&intensityData),
+ sizeof(intensityData), &ts);
+ }
+ else
+ {
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS,
+ reinterpret_cast<void*>(&intensityData),
+ sizeof(intensityData), &time1);
+ }
+ }
+ else
+ {
+ player_ranger_data_range_t rangeData;
+
+ try
+ {
+ GlobalTime->GetTimeDouble(&time1);
+ device_.get_ranges_by_angle(data_, minAngle_, maxAngle_);
+ GlobalTime->GetTimeDouble(&time2);
+ time1 = (time1 + time2) / 2.0;
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Failed to read scan: %s", e.what());
+ SetError(-1);
+ return false;
+ }
+
+ double lastValidValue = minDist_;
+ if (!invert_) {
+ for (unsigned int ii = 0; ii < data_.ranges_length(); ii++)
+ {
+ _ranges[ii] = data_[ii] / 1000.0f;
+ if (minDist_ > 0)
+ {
+ if (_ranges[ii] < minDist_)
+ _ranges[ii] = lastValidValue;
+ else
+ lastValidValue = _ranges[ii];
+ }
+ }
+ }
+ else // Invert
+ {
+ for (unsigned int ii = 0; ii < data_.ranges_length(); ii++)
+ {
+ _ranges[ii] = data_[data_.ranges_length() - 1 - ii] / 1000.0f;
+ if (minDist_ > 0)
+ {
+ if (_ranges[ii] < minDist_)
+ _ranges[ii] = lastValidValue;
+ else
+ lastValidValue = _ranges[ii];
+ }
+ }
+ }
+ rangeData.ranges = _ranges;
+ rangeData.ranges_count = data_.ranges_length();
+ if (hwTimeStamps_.GetValue())
+ {
+ double ts = data_.system_time_stamp() / 1000000000.0;
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
+ reinterpret_cast<void*> (&rangeData), sizeof(rangeData),
+ &ts);
+ }
+ else
+ {
+ Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE,
+ reinterpret_cast<void*> (&rangeData), sizeof(rangeData),
+ &time1);
+ }
+ }
- return true;
+ return true;
}
-int HokuyoDriver::MainSetup (void)
+int HokuyoDriver::MainSetup(void)
{
+ try
+ {
+ device_.ignore_unknowns(ignoreUnknowns_);
+ // Open the laser
+ device_.open(portOpts_);
+ // Get the sensor information and check minAngle_ and maxAngle_ are OK
+ hokuyoaist::SensorInfo info;
+ device_.get_sensor_info(info);
+ if (minAngle_ < info.min_angle)
+ {
+ minAngle_ = info.min_angle;
+ PLAYER_WARN1("HokuyoAIST: Adjusted min_angle to %lf", minAngle_);
+ }
+ if (maxAngle_> info.max_angle)
+ {
+ maxAngle_ = info.max_angle;
+ PLAYER_WARN1("HokuyoAIST: Adjusted max_angle to %lf", maxAngle_);
+ }
+ if (!AllocateDataSpace())
+ return -1;
+
+ /*
try
- {
- _device.IgnoreUnknowns (_ignoreUnknowns);
- // Open the laser
- _device.OpenWithProbing (_portOpts);
- // Get the sensor information and check _minAngle and _maxAngle are OK
- hokuyo_aist::HokuyoSensorInfo info;
- _device.GetSensorInfo (&info);
- if (_minAngle < info.minAngle)
- {
- _minAngle = info.minAngle;
- PLAYER_WARN1 ("hokuyo_aist: Adjusted min_angle to %lf", _minAngle);
- }
- if (_maxAngle> info.maxAngle)
- {
- _maxAngle = info.maxAngle;
- PLAYER_WARN1 ("hokuyo_aist: Adjusted max_angle to %lf", _maxAngle);
- }
- if (!AllocateDataSpace ())
- return -1;
-
- if (_powerOnStartup)
- _device.SetPower (true);
-
- try
- {
- _device.SetBaud (_baudRate.GetValue ());
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
-#if defined (WIN32)
- if (e.Code () != HOKUYO_ERR_NOTSERIAL)
-#else
- if (e.Code () != hokuyo_aist::HOKUYO_ERR_NOTSERIAL)
-#endif
- PLAYER_WARN ("hokuyo_aist: Cannot change the baud rate of a non-serial connection.");
- else
- PLAYER_WARN2 ("hokuyo_aist: Error changing baud rate: (%d) %s", e.Code (), e.what ());
- }
- try
- {
- // Catch any errors here as this is an optional setting not supported by all models
- _device.SetMotorSpeed (_speedLevel.GetValue ());
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_WARN2 ("hokuyo_aist: Unable to set motor speed: (%d) %s", e.Code (), e.what ());
- }
- try
- {
- // Optional setting
- _device.SetHighSensitivity (_highSensitivity.GetValue () != 0);
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_WARN2 ("hokuyo_aist: Unable to set sensitivity: (%d) %s", e.Code (), e.what ());
- }
- }
- catch (hokuyo_aist::HokuyoError &e)
- {
- PLAYER_ERROR2 ("hokuyo_aist: Failed to setup laser driver: (%d) %s", e.Code (), e.what ());
- SetError (e.Code ());
- return -1;
- }
- return 0;
+ {
+ device_.set_baud(baudRate_.GetValue());
+ }
+ catch(hokuyoaist::NotSerialError)
+ {
+ PLAYER_WARN(
+ "HokuyoAIST: Cannot change the baud rate of a non-serial connection.");
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_WARN1("HokuyoAIST: Error while changing baud rate: %s",
+ e.what());
+ }
+ */
+ try
+ {
+ // Catch any errors here as this is an optional setting not supported by all models
+ device_.set_motor_speed(speedLevel_.GetValue());
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_WARN1("HokuyoAIST: Unable to set motor speed: %s",
+ e.what());
+ }
+ try
+ {
+ // Optional setting
+ device_.set_high_sensitivity(highSensitivity_.GetValue() != 0);
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_WARN1("HokuyoAIST: Unable to set sensitivity: %s",
+ e.what());
+ }
+ if (hwTimeStamps_.GetValue()) {
+ device_.calibrate_time();
+ }
+
+ if (powerOnStartup_)
+ device_.set_power(true);
+
+ }
+ catch(hokuyoaist::BaseError &e)
+ {
+ PLAYER_ERROR1("HokuyoAIST: Failed to setup laser driver: %s",
+ e.what());
+ SetError(-1);
+ return -1;
+ }
+ return 0;
}
-void HokuyoDriver::MainQuit (void)
+void HokuyoDriver::MainQuit(void)
{
- _device.Close ();
- _data.CleanUp ();
- if (_ranges != NULL)
- {
- delete[] _ranges;
- _ranges = NULL;
- }
+ device_.close();
+ data_.clean_up();
+ if (_ranges != NULL)
+ {
+ delete[] _ranges;
+ _ranges = NULL;
+ }
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// Driver management functions
////////////////////////////////////////////////////////////////////////////////////////////////////
-Driver* HokuyoDriver_Init (ConfigFile* cf, int section)
+Driver* HokuyoDriver_Init(ConfigFile* cf, int section)
{
- return reinterpret_cast <Driver*> (new HokuyoDriver (cf, section));
+ return reinterpret_cast <Driver*> (new HokuyoDriver(cf, section));
}
-void hokuyo_aist_Register (DriverTable* table)
+void hokuyoaist_Register(DriverTable* table)
{
- table->AddDriver ("hokuyo_aist", HokuyoDriver_Init);
+ table->AddDriver("hokuyoaist", HokuyoDriver_Init);
}