From 2fa7165be202c90387a8d2279f26c1077d721890 Mon Sep 17 00:00:00 2001 From: Rich Mattes Date: Aug 06 2012 14:46:24 +0000 Subject: Added dependency on hokuyoaist library - Added dependency on flexiport library - Fixed hokuyoaist driver to work with hokuyoaist library - Removed gearbox dependency --- diff --git a/player-3.0.2.gbx10.patch b/player-3.0.2.gbx10.patch new file mode 100644 index 0000000..f13482c --- /dev/null +++ b/player-3.0.2.gbx10.patch @@ -0,0 +1,1328 @@ +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-05 11:58:18.920031468 -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-05 11:58:18.921031504 -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-05 11:58:18.921031504 -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-05 11:58:18.921031504 -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-05 12:04:35.255599367 -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,644 @@ 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); + } diff --git a/player.spec b/player.spec index 55100b2..8e42fd3 100644 --- a/player.spec +++ b/player.spec @@ -1,6 +1,6 @@ Name: player Version: 3.0.2 -Release: 19%{?dist} +Release: 20%{?dist} Summary: Cross-platform robot device interface and server Group: Applications/System @@ -19,6 +19,7 @@ Patch4: %{name}-3.0.2.enablephidget.patch Patch5: %{name}-3.0.2.s390x.patch Patch6: %{name}-3.0.2.boostutc.patch Patch7: %{name}-3.0.2.gzfile.patch +Patch8: %{name}-3.0.2.gbx10.patch BuildRoot: %{_tmppath}/%{name}-%{version}-%{release}-root-%(%{__id_u} -n) # Sort order: build tools, feature libs, within alphabetically, group related @@ -35,7 +36,7 @@ BuildRequires: avahi-compat-howl-devel BuildRequires: boost-devel boost-thread BuildRequires: festival-devel BuildRequires: flexiport-devel -BuildRequires: gearbox-devel +BuildRequires: hokuyoaist-devel BuildRequires: geos-devel BuildRequires: gsl-devel BuildRequires: gstreamer-devel @@ -137,13 +138,14 @@ are experimental. %patch5 -p1 -b .s390x %patch6 -p0 %patch7 -p0 +%patch8 -p2 -b .gbx10 find . -name '*.c' -exec chmod -x {} \; find . -name '*.cc' -exec chmod -x {} \; find . -name '*.cpp' -exec chmod -x {} \; find . -name '*.h' -exec chmod -x {} \; %build -%cmake %{?_cmake_skip_rpath} -DBUILD_DOCUMENTATION=ON -DBUILD_PLAYERCC=ON -DSWIG_EXECUTABLE=/usr/bin/swig -DBUILD_PLAYERCC_BOOST=ON -DBUILD_PYTHONC_BINDINGS=ON -DBUILD_PYTHONCPP_BINDINGS=ON -DBUILD_EXAMPLES=ON -DBUILD_RUBYCPP_BINDINGS=ON -DBoost_USE_MULTITHREAD=ON -DENABLE_DRIVER_OCEANSERVER=OFF -DUNICAP_DIR=/usr -DRUBY_BINDINGS_INSTALL_DIR=%{ruby_vendorarchdir} . +%cmake %{?_cmake_skip_rpath} -DBUILD_DOCUMENTATION=ON -DBUILD_PLAYERCC=ON -DSWIG_EXECUTABLE=/usr/bin/swig -DBUILD_PLAYERCC_BOOST=ON -DBUILD_PYTHONC_BINDINGS=ON -DBUILD_PYTHONCPP_BINDINGS=ON -DBUILD_EXAMPLES=ON -DBUILD_RUBYCPP_BINDINGS=ON -DBoost_USE_MULTITHREAD=ON -DUNICAP_DIR=/usr -DRUBY_BINDINGS_INSTALL_DIR=%{ruby_vendorarchdir} . make make doc #pushd doc @@ -219,6 +221,12 @@ rm -rf $RPM_BUILD_ROOT %{ruby_vendorarchdir}/*.so %changelog +* Mon Jul 30 2012 Rich Mattes - 3.0.2-20 +- Added dependency on hokuyoaist library +- Added dependency on flexiport library +- Fixed hokuyoaist driver to work with hokuyoaist library +- Removed gearbox dependency + * Sat Jul 21 2012 Fedora Release Engineering - 3.0.2-19 - Rebuilt for https://fedoraproject.org/wiki/Fedora_18_Mass_Rebuild