From: <gb...@us...> - 2008-06-19 20:52:18
|
Revision: 6650 http://playerstage.svn.sourceforge.net/playerstage/?rev=6650&view=rev Author: gbiggs Date: 2008-06-19 20:52:11 -0700 (Thu, 19 Jun 2008) Log Message: ----------- Replaced urg_nz driver with a new version that uses the new Gearbox urg_nz library Modified Paths: -------------- code/player/branches/release-2-1-patches/acinclude.m4 code/player/branches/release-2-1-patches/config/Makefile.am code/player/branches/release-2-1-patches/server/drivers/ranger/urg_nz.cc code/player/branches/release-2-1-patches/server/libplayerdrivers/driverregistry.cc Added Paths: ----------- code/player/branches/release-2-1-patches/config/urg_nz.cfg Modified: code/player/branches/release-2-1-patches/acinclude.m4 =================================================================== --- code/player/branches/release-2-1-patches/acinclude.m4 2008-06-20 03:24:49 UTC (rev 6649) +++ code/player/branches/release-2-1-patches/acinclude.m4 2008-06-20 03:52:11 UTC (rev 6650) @@ -442,7 +442,7 @@ AC_CHECK_HEADERS(linux/serial.h, [], [], []) fi -PLAYER_ADD_DRIVER([urg_nz],[no],[],[],[],[URG_NZ],[urg_nz >= 0.0.1]) +PLAYER_ADD_DRIVER([urg_nz],[yes],[],[],[],[URG_NZ],[urg_nz >= 0.0.1]) PLAYER_DRIVER_EXTRA_LIBS="$PLAYER_DRIVER_EXTRA_LIBS $URG_NZ_LIBS" PLAYER_ADD_DRIVER([vec2map],[yes],["geos_c.h"],[],["-lgeos"]) Modified: code/player/branches/release-2-1-patches/config/Makefile.am =================================================================== --- code/player/branches/release-2-1-patches/config/Makefile.am 2008-06-20 03:24:49 UTC (rev 6649) +++ code/player/branches/release-2-1-patches/config/Makefile.am 2008-06-20 03:52:11 UTC (rev 6650) @@ -32,6 +32,7 @@ umass_ATRVJr.cfg \ umass_ATRVMini.cfg \ umass_reb.cfg \ +urg_nz.cfg \ urglaser.cfg \ vfh.cfg \ wavefront.cfg \ Added: code/player/branches/release-2-1-patches/config/urg_nz.cfg =================================================================== --- code/player/branches/release-2-1-patches/config/urg_nz.cfg (rev 0) +++ code/player/branches/release-2-1-patches/config/urg_nz.cfg 2008-06-20 03:52:11 UTC (rev 6650) @@ -0,0 +1,6 @@ +driver +( + name "urg_nz" + provides ["ranger:0"] + portopts "type=serial,device=/dev/ttyACM0,timeout=1" +) Modified: code/player/branches/release-2-1-patches/server/drivers/ranger/urg_nz.cc =================================================================== --- code/player/branches/release-2-1-patches/server/drivers/ranger/urg_nz.cc 2008-06-20 03:24:49 UTC (rev 6649) +++ code/player/branches/release-2-1-patches/server/drivers/ranger/urg_nz.cc 2008-06-20 03:52:11 UTC (rev 6650) @@ -1,72 +1,60 @@ /* * Player - One Hell of a Robot Server - * Copyright (C) 2003 - * Brian Gerkey + * Copyright (C) 2008 + * Geoffrey Biggs * * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * This program is free software: you can redistribute it and/or modify it under the terms of the + * GNU Lesser General Public License as published by the Free Software Foundation, either version + * 3 of the License, or (at your option) any later version. * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * + * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without + * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License along with this program. + * If not, see <http://www.gnu.org/licenses/>. */ -/////////////////////////////////////////////////////////////////////////// -// -// Desc: Driver wrapper around the Gearbox urg_nz library. -// Author: Geoffrey Biggs -// Date: 25/02/2008 -// -// Provides - Ranger device. -// -/////////////////////////////////////////////////////////////////////////// +/* + Desc: Wrapper driver around the Gearbox urg_nz library (see http://gearbox.sourceforge.net) + Author: Geoffrey Biggs + Date: 20 June 2008 + CVS: $Id$ +*/ /** @ingroup drivers */ /** @{ */ /** @defgroup driver_urg_nz urg_nz * @brief Gearbox urg_nz Hokuyo URG laser scanner driver library -This driver provides a @ref interface_ranger interface to the urg_nz Hokuyo URG laser scanner driver -provided by Gearbox. Communication with the laser can be either via USB or RS232. The driver -supports SCIP procol versions 1 and 2. + This driver provides a @ref interface_ranger interface to the urg_nz Hokuyo URG laser scanner + driver provided by Gearbox. Communication with the laser is via the Gearbox Flexiport library. The + driver supports the SCIP protocol versions 1 and 2. -@par Compile-time dependencies + @par Compile-time dependencies -- Gearbox library urg_nz + - Gearbox library urg_nz + - Gearbox library flexiport -@par Provides + @par Provides -- @ref interface_ranger : Output ranger interface + - @ref interface_ranger : Output ranger interface -@par Configuration requests + @par Configuration requests -- PLAYER_RANGER_REQ_GET_GEOM -- PLAYER_RANGER_REQ_GET_CONFIG -- PLAYER_RANGER_REQ_SET_CONFIG - - Note: Only the min_angle and max_angle values can be configured using this request. + - PLAYER_RANGER_REQ_GET_GEOM + - PLAYER_RANGER_REQ_GET_CONFIG + - PLAYER_RANGER_REQ_SET_CONFIG + - Note: Only the min_angle, max_angle and frequency values can be configured using this request. + In addition, the frequency value must be equivalent to a suitable RPM value (see the urg_nz + library documentation for suitable values). -@par Configuration file options + @par Configuration file options - - port (string) - - Default: "/dev/ttyACM0" - - Port to which the laser is connected. Can be a serial port or the port associated with a USB ACM - device. - - baudrate (integer) - - Default: 115200 - - Initial baud rate to connect at. Can be changed with the "baudrate" property. Valid rates are - 19200, 57600 and 115200. Only applies when use_serial is true. - - use_serial (boolean) - - Default: false - - Connect over an RS232 serial connection instead of the default USB connection. + - portopts (string) + - Default: "type=serial,device=/dev/ttyACM0,timeout=1" + - Options to create the Flexiport port with. - pose (float 6-tuple: (m, m, m, rad, rad, rad)) - Default: [0.0 0.0 0.0 0.0 0.0 0.0] - Pose (x, y, z, roll, pitch, yaw) of the laser relative to its parent object (e.g. the robot). @@ -74,348 +62,383 @@ - Default: [0.0 0.0 0.0] - Size of the laser in metres. - min_angle (float, radians) - - Default: -2.094 rad (-120.0 degrees) - - Minimum scan angle to return. + - Default: -2.08 rad (-119.0 degrees) + - Minimum scan angle to return. Will be adjusted if outside the laser's scannable range. - max_angle (float, radians) - - Default: 2.094 rad (120.0 degrees) - - Maximum scan angle to return. + - Default: 2.08 rad (119.0 degrees) + - Maximum scan angle to return. Will be adjusted if outside the laser's scannable range. + - frequency (float, Hz) + - Default: 10Hz + - The frequency at which the laser operates. This must be equivalent to a suitable RPM value. See + - the urg_nz library documentation for suitable values. + - power (boolean) + - Default: true + - If true, the sensor power will be switched on upon driver activation (i.e. when the first + client connects). Otherwise a power request must be made to turn it on before data will be + received. - verbose (boolean) - Default: false - Enable verbose debugging information in the underlying library. -@par Properties + @par Properties - baudrate (integer) - - Change the baud rate of the connection to the laser. Valid rates are 19200, 57600 and 115200. - Only applies when use_serial is true. Not currently supported if SCIP v2 is in use. + - Default: 19200bps + - Change the baud rate of the connection to the laser. See urg_nz documentation for valid values. -@par Example + @par Example -@verbatim -driver -( - name "urg_nz" - provides ["ranger:0"] - port "/dev/ttyACM0" -) -@endverbatim + @verbatim + driver + ( + name "urg_nz" + provides ["ranger:0"] + portopts "type=serial,device=/dev/ttyS0,timeout=1" + ) + @endverbatim -@author Geoffrey Biggs + @author Geoffrey Biggs -*/ + */ /** @} */ -#include <math.h> -#include <stdio.h> -#include <stdlib.h> -#include <unistd.h> - #include <string> -#include <iostream> -using namespace std; #include <urg_nz/urg_nz.h> - #include <libplayercore/playercore.h> -const int DEFAULT_BAUDRATE = 115200; +const int DEFAULT_BAUDRATE = 19200; +const int DEFAULT_SPEED = 600; +//////////////////////////////////////////////////////////////////////////////////////////////////// +// Driver object +//////////////////////////////////////////////////////////////////////////////////////////////////// + class UrgDriver : public Driver { - public: - UrgDriver (ConfigFile* cf, int section); - ~UrgDriver (void); + public: + UrgDriver (ConfigFile* cf, int section); + ~UrgDriver (void); - virtual int Setup (void); - virtual int Shutdown (void); - virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data); + virtual int Setup (void); + virtual int Shutdown (void); + virtual int ProcessMessage (QueuePointer &resp_queue, player_msghdr *hdr, void *data); - private: - virtual void Main (void); - bool ReadLaser (void); - bool CalculateMinMaxIndices (void); + private: + virtual void Main (void); + bool ReadLaser (void); + bool AllocateDataSpace (void); - // Configuration parameters - bool useSerial, verbose; - double minAngle, maxAngle; - IntProperty baudRate; - string port; - int numSamples; - // Config received from the laser - urg_nz::urg_nz_laser_config_t config; - // Geometry - player_ranger_geom_t geom; - player_pose3d_t sensorPose; - player_bbox3d_t sensorSize; - // Data storage, etc - double *ranges; - urg_nz::urg_nz_laser_readings_t *readings; - unsigned int minIndex, maxIndex; - // The hardware device itself - urg_nz::urg_laser device; + // Configuration parameters + bool _verbose, _powerOnStartup; + int _frequency; + double _minAngle, _maxAngle; + IntProperty _baudRate; + std::string _portOpts; + // Geometry + player_ranger_geom_t geom; + player_pose3d_t sensorPose; + player_bbox3d_t sensorSize; + // The hardware device itself + urg_nz::URGLaser _device; + // Data storage + urg_nz::URGData _data; + double *_ranges; }; -Driver* -UrgDriver_Init (ConfigFile* cf, int section) -{ - return reinterpret_cast <Driver*> (new UrgDriver (cf, section)); -} +//////////////////////////////////////////////////////////////////////////////////////////////////// +// Constructor/destructor +//////////////////////////////////////////////////////////////////////////////////////////////////// -void UrgDriver_Register(DriverTable* table) +UrgDriver::UrgDriver (ConfigFile* cf, int section) : + Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE), + _baudRate ("baudrate", DEFAULT_BAUDRATE, false), _ranges (NULL) { - table->AddDriver ("urg_nz", UrgDriver_Init); -} + // Get the baudrate and motor speed + RegisterProperty ("baudrate", &_baudRate, cf, section); -UrgDriver::UrgDriver (ConfigFile* cf, int section) - : Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE), - baudRate ("baudrate", DEFAULT_BAUDRATE, false), ranges (NULL), readings (NULL), - minIndex (0), maxIndex (urg_nz::MAX_READINGS) -{ - // Get and sanity-check the baudrate - RegisterProperty ("baudrate", &baudRate, cf, section); - if (baudRate.GetValue () != 19200 && baudRate.GetValue () != 57600 && baudRate.GetValue () != 115200) - { - PLAYER_WARN2 ("urg_nz: Ignored bad baud rate: %d, using default of %d", baudRate.GetValue (), DEFAULT_BAUDRATE); - baudRate.SetValue (DEFAULT_BAUDRATE); - } + // Get config + _minAngle = cf->ReadFloat (section, "min_angle", -2.08); + _maxAngle = cf->ReadFloat (section, "max_angle", 2.08); + _frequency = cf->ReadInt (section, "frequency", 10); + _portOpts = cf->ReadString (section, "portopts", "type=serial,device=/dev/ttyACM0,timeout=1"); + _verbose = cf->ReadBool (section, "verbose", false); + _powerOnStartup = cf->ReadBool (section, "power", true); - // Get config - minAngle = cf->ReadFloat (section, "min_angle", DTOR (-120.0f)); - maxAngle = cf->ReadFloat (section, "max_angle", DTOR (120.0f)); - useSerial = cf->ReadBool (section, "use_serial", false); - port = cf->ReadString (section, "port", "/dev/ttyACM0"); - verbose = cf->ReadBool (section, "verbose", false); + // 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.sensor_poses_count = 1; + geom.sensor_poses = &sensorPose; + memcpy(geom.sensor_poses, &geom.pose, sizeof (geom.pose)); + geom.sensor_sizes_count = 1; + geom.sensor_sizes = &sensorSize; + memcpy(geom.sensor_sizes, &geom.size, sizeof (geom.size)); - // Set up geometry information - geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0f); - geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0f); - geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0f); - geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0f); - geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0f); - geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0f); - geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f); - geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f); - geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f); - geom.sensor_poses_count = 1; - geom.sensor_poses = &sensorPose; - memcpy (geom.sensor_poses, &geom.pose, sizeof (geom.pose)); - geom.sensor_sizes_count = 1; - geom.sensor_sizes = &sensorSize; - memcpy (geom.sensor_sizes, &geom.size, sizeof (geom.size)); - - // Turn on/off verbose mode - device.SetVerbose (verbose); + // Turn on/off verbose mode + _device.SetVerbose (_verbose); } UrgDriver::~UrgDriver (void) { - if (ranges != NULL) - delete[] ranges; - if (readings != NULL) - delete[] readings; + if (_ranges != NULL) + delete[] _ranges; } -int UrgDriver::Setup (void) +//////////////////////////////////////////////////////////////////////////////////////////////////// +// Driver implementation +//////////////////////////////////////////////////////////////////////////////////////////////////// + +bool UrgDriver::AllocateDataSpace (void) { - try - { - // Open the laser - device.Open (port.c_str (), useSerial, baudRate.GetValue ()); - // Get the current config - device.GetSensorConfig (&config); - if (!CalculateMinMaxIndices ()) - return -1; - } - catch (urg_nz::urg_nz_exception &e) - { - PLAYER_ERROR2 ("urg_nz: Failed to setup laser driver: (%d) %s", e.error_code, e.error_desc.c_str ()); - SetError (e.error_code); - return -1; - } + if (_ranges != NULL) + delete _ranges; - // Create space to store data - if ((ranges = new double[maxIndex - minIndex + 1]) == NULL) - { - PLAYER_ERROR ("urg_nz: Failed to allocate data store."); - return -1; - } - if ((readings = new urg_nz::urg_nz_laser_readings_t) == NULL) - { - PLAYER_ERROR ("urg_nz: Failed to allocate intermediate data store."); - return -1; - } + int numRanges = _device.AngleToStep (_maxAngle) - _device.AngleToStep (_minAngle) + 1; + if ((_ranges = new double[numRanges]) == NULL) + { + PLAYER_ERROR1 ("urg_nz: Failed to allocate space for %d range readings.", numRanges); + return false; + } - StartThread(); - return 0; + return true; } -int UrgDriver::Shutdown (void) +void UrgDriver::Main (void) { - StopThread(); + while (true) + { + pthread_testcancel (); + ProcessMessages (); - device.Close (); - - if (ranges != NULL) - { - delete[] ranges; - ranges = NULL; - } - if (readings != NULL) - { - delete readings; - readings = NULL; - } - - return 0; + if (!ReadLaser ()) + break; + } } int UrgDriver::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); + // 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); - // Check for a change in the baud rate property; we need to handle this manually rather than letting the driver - // class handle it because we need to change the baud rate in the 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); - if (strcmp(req->key, "baudrate") == 0) - { - try - { - // Change the baud rate - if (device.ChangeBaud (baudRate, req->value) == 0) - { - baudRate.SetValueFromMessage (data); - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL); - } - else - { - PLAYER_WARN ("urg_nz: Unable to change baud rate."); - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, NULL, 0, NULL); - } - } - catch (urg_nz::urg_nz_exception &e) - { - PLAYER_ERROR2 ("urg_nz: Fatal error while changing baud rate: (%d) %s", e.error_code, e.error_desc.c_str ()); - SetError (e.error_code); - return -1; - } - return 0; - } - } - // Standard ranger messages - 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; - rangerConfig.min_angle = minAngle; - rangerConfig.max_angle = maxAngle; - rangerConfig.resolution = config.resolution; - rangerConfig.max_range = config.max_range / 1000.0f; - rangerConfig.range_res = 0.0f; - rangerConfig.frequency = 0.0f; - 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 (!CalculateMinMaxIndices ()) - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL); - else - { - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, newParams, sizeof (*newParams), NULL); - // Reallocate ranges - delete[] ranges; - if ((ranges = new double[maxIndex - minIndex + 1]) == NULL) - { - PLAYER_ERROR ("urg_nz: Failed to allocate data store."); - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL); - } - else - Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, newParams, sizeof (*newParams), NULL); - } - return 0; - } + // Property handlers that need to be done manually due to calling into the urg_nz 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, "baudrate", 8) == 0) + { + try + { + // Change the baud rate + _device.SetBaud (req->value); + } + catch (urg_nz::URGError &e) + { + if (e.Code () != urg_nz::URG_ERR_NOTSERIAL) + { + PLAYER_ERROR2 ("urg_nz: Error while changing baud rate: (%d) %s", e.Code (), + e.what ()); + SetError (e.Code ()); + } + else + PLAYER_WARN ("urg_nz: Cannot change the baud rate of a non-serial connection."); - return -1; + Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, + NULL, 0, NULL); + return -1; + } + _baudRate.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_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; + urg_nz::URGSensorInfo info; + _device.GetSensorInfo (&info); + + rangerConfig.min_angle = _minAngle; // These two are user-configurable + rangerConfig.max_angle = _maxAngle; + rangerConfig.resolution = info.resolution; + 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 ()) + { + Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG, + NULL, 0, NULL); + return -1; + } + + _frequency = static_cast<int> (newParams->frequency); + try + { + urg_nz::URGSensorInfo info; + _device.GetSensorInfo (&info); + if (_minAngle < info.minAngle) + { + _minAngle = info.minAngle; + PLAYER_WARN1 ("urg_nz: Adjusted min_angle to %lf", _minAngle); + } + if (_maxAngle> info.maxAngle) + { + _maxAngle = info.maxAngle; + PLAYER_WARN1 ("urg_nz: Adjusted max_angle to %lf", _maxAngle); + } + _device.SetMotorSpeed (_frequency * 60); + } + catch (urg_nz::URGError &e) + { + PLAYER_ERROR2 ("urg_nz: 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 -1; + } + + Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, + newParams, sizeof (*newParams), NULL); + return 0; + } + + return -1; } -void UrgDriver::Main (void) +bool UrgDriver::ReadLaser (void) { - while (true) - { - pthread_testcancel (); - ProcessMessages (); + player_ranger_data_range_t rangeData; - if (!ReadLaser ()) - break; - } + try + { + unsigned int numRead = _device.GetRanges (&_data, _minAngle, _maxAngle); + } + catch (urg_nz::URGError &e) + { + PLAYER_ERROR2 ("urg_nz: Failed to read scan: (%d) %s", e.Code (), e.what ()); + SetError (e.Code ()); + return false; + } + + for (unsigned int ii = 0; ii < _data.Length (); ii++) + _ranges[ii] = _data[ii] / 1000.0f; + rangeData.ranges = _ranges; + rangeData.ranges_count = _data.Length (); + Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, + reinterpret_cast<void*> (&rangeData), sizeof (rangeData), NULL); + + return true; } -bool UrgDriver::ReadLaser (void) +int UrgDriver::Setup (void) { - player_ranger_data_range_t rangeData; + try + { + // Open the laser + _device.Open (_portOpts); + // Get the sensor information and check _minAngle and _maxAngle are OK + urg_nz::URGSensorInfo info; + _device.GetSensorInfo (&info); + if (_minAngle < info.minAngle) + { + _minAngle = info.minAngle; + PLAYER_WARN1 ("urg_nz: Adjusted min_angle to %lf", _minAngle); + } + if (_maxAngle> info.maxAngle) + { + _maxAngle = info.maxAngle; + PLAYER_WARN1 ("urg_nz: Adjusted max_angle to %lf", _maxAngle); + } + if (!AllocateDataSpace ()) + return -1; - try - { - unsigned int numRead = device.GetReadings (readings, minIndex, maxIndex); - if (numRead != (maxIndex - minIndex + 1)) - { - PLAYER_WARN2 ("urg_nz: Warning: Got an unexpected number of range readings (%d != %d)", numRead, maxIndex - minIndex + 1); - return true; // Maybe we'll get more next time - } + if (_powerOnStartup) + _device.SetPower (true); - for (unsigned int ii; ii < numRead; ii++) - ranges[ii] = readings->Readings[ii] / 1000.0f; - rangeData.ranges = ranges; - rangeData.ranges_count = numRead; - Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_RANGER_DATA_RANGE, reinterpret_cast<void*> (&rangeData), sizeof (rangeData), NULL); - } - catch (urg_nz::urg_nz_exception &e) - { - PLAYER_ERROR2 ("urg_nz: Failed to read scan: (%d) %s", e.error_code, e.error_desc.c_str ()); - SetError (e.error_code); - return false; - } + try + { + _device.SetBaud (_baudRate.GetValue ()); + } + catch (urg_nz::URGError &e) + { + if (e.Code () != urg_nz::URG_ERR_NOTSERIAL) + throw; + PLAYER_WARN ("urg_nz: Cannot change the baud rate of a non-serial connection."); + } + } + catch (urg_nz::URGError &e) + { + PLAYER_ERROR2 ("urg_nz: Failed to setup laser driver: (%d) %s", e.Code (), e.what ()); + SetError (e.Code ()); + return -1; + } - return true; + StartThread (); + return 0; } -bool UrgDriver::CalculateMinMaxIndices (void) +int UrgDriver::Shutdown (void) { - unsigned int minPossibleIndex, maxPossibleIndex; + StopThread (); - // Calculate min and max scan indices - minIndex = static_cast<unsigned int> (round ((urg_nz::MAX_READINGS / 2) + minAngle / config.resolution)); - maxIndex = static_cast<unsigned int> (round ((urg_nz::MAX_READINGS / 2) + maxAngle / config.resolution)); - // Sanity check - if (minIndex > maxIndex) - minIndex = maxIndex; - // Clip the min and max scan indices - minPossibleIndex = static_cast<unsigned int> (round ((urg_nz::MAX_READINGS / 2) + config.min_angle / config.resolution)); - maxPossibleIndex = static_cast<unsigned int> (round ((urg_nz::MAX_READINGS / 2) + config.max_angle / config.resolution)); - if (minIndex < minPossibleIndex) - { - minIndex = minPossibleIndex; - minAngle = config.min_angle; - PLAYER_WARN1 ("urg_nz: Warning: min_angle clipped to %f", config.min_angle); - } - if (maxIndex > maxPossibleIndex) - { - maxIndex = maxPossibleIndex; - maxAngle = config.max_angle; - PLAYER_WARN1 ("urg_nz: Warning: max_angle clipped to %f", config.max_angle); - } + _device.Close (); + _data.CleanUp (); + if (_ranges != NULL) + delete[] _ranges; - return true; + return 0; } + +//////////////////////////////////////////////////////////////////////////////////////////////////// +// Driver management functions +//////////////////////////////////////////////////////////////////////////////////////////////////// + +Driver* UrgDriver_Init (ConfigFile* cf, int section) +{ + return reinterpret_cast <Driver*> (new UrgDriver (cf, section)); +} + +void urg_nz_Register (DriverTable* table) +{ + table->AddDriver ("urg_nz", UrgDriver_Init); +} Modified: code/player/branches/release-2-1-patches/server/libplayerdrivers/driverregistry.cc =================================================================== --- code/player/branches/release-2-1-patches/server/libplayerdrivers/driverregistry.cc 2008-06-20 03:24:49 UTC (rev 6649) +++ code/player/branches/release-2-1-patches/server/libplayerdrivers/driverregistry.cc 2008-06-20 03:52:11 UTC (rev 6650) @@ -507,7 +507,7 @@ #endif #ifdef INCLUDE_URG_NZ -void UrgDriver_Register(DriverTable* table); +void urg_nz_Register(DriverTable* table); #endif /* @@ -977,6 +977,6 @@ #endif #ifdef INCLUDE_URG_NZ - UrgDriver_Register(driverTable); + urg_nz_Register(driverTable); #endif } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |