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 #include #include - +#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 -#include +#include #include 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 (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 (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 (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(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 (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 (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 (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 (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 (&rangeData), sizeof (rangeData), &ts); - } - else - { - Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, - reinterpret_cast (&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 (&intensityData), sizeof (intensityData), &ts); - } - else - { - Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS, - reinterpret_cast (&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 (&rangeData), sizeof (rangeData), &ts); - } - else - { - Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, - reinterpret_cast (&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(&rangeData), sizeof(rangeData), + &ts); + } + else + { + Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, + reinterpret_cast(&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(&intensityData), + sizeof(intensityData), &ts); + } + else + { + Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_INTNS, + reinterpret_cast(&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 (&rangeData), sizeof(rangeData), + &ts); + } + else + { + Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, + reinterpret_cast (&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 (new HokuyoDriver (cf, section)); + return reinterpret_cast (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); }