Revision: 6648
http://playerstage.svn.sourceforge.net/playerstage/?rev=6648&view=rev
Author: gbiggs
Date: 2008-06-19 20:14:13 -0700 (Thu, 19 Jun 2008)
Log Message:
-----------
Replaced urg_nz driver with a new one that uses the new Gearbox urg_nz library
Modified Paths:
--------------
code/player/trunk/config/CMakeLists.txt
code/player/trunk/server/drivers/ranger/urg_nz.cc
Added Paths:
-----------
code/player/trunk/config/urg_nz.cfg
Modified: code/player/trunk/config/CMakeLists.txt
===================================================================
--- code/player/trunk/config/CMakeLists.txt 2008-06-19 21:49:14 UTC (rev
6647)
+++ code/player/trunk/config/CMakeLists.txt 2008-06-20 03:14:13 UTC (rev
6648)
@@ -29,6 +29,7 @@
umass_ATRVJr.cfg
umass_ATRVMini.cfg
umass_reb.cfg
+ urg_nz.cfg
urglaser.cfg
vfh.cfg
wavefront.cfg
Added: code/player/trunk/config/urg_nz.cfg
===================================================================
--- code/player/trunk/config/urg_nz.cfg (rev 0)
+++ code/player/trunk/config/urg_nz.cfg 2008-06-20 03:14:13 UTC (rev 6648)
@@ -0,0 +1,6 @@
+driver
+(
+ name "urg_nz"
+ provides ["ranger:0"]
+ portopts "type=serial,device=/dev/ttyACM0,timeout=1"
+)
Modified: code/player/trunk/server/drivers/ranger/urg_nz.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/urg_nz.cc 2008-06-19 21:49:14 UTC
(rev 6647)
+++ code/player/trunk/server/drivers/ranger/urg_nz.cc 2008-06-20 03:14:13 UTC
(rev 6648)
@@ -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.
[EMAIL PROTECTED] Compile-time dependencies
+ @par Compile-time dependencies
-- Gearbox library urg_nz
+ - Gearbox library urg_nz
+ - Gearbox library flexiport
[EMAIL PROTECTED] Provides
+ @par Provides
-- @ref interface_ranger : Output ranger interface
+ - @ref interface_ranger : Output ranger interface
[EMAIL PROTECTED] 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).
[EMAIL PROTECTED] 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.
[EMAIL PROTECTED] 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.
[EMAIL PROTECTED] Example
+ @par Example
[EMAIL PROTECTED]
-driver
-(
- name "urg_nz"
- provides ["ranger:0"]
- port "/dev/ttyACM0"
-)
[EMAIL PROTECTED]
+ @verbatim
+ driver
+ (
+ name "urg_nz"
+ provides ["ranger:0"]
+ portopts "type=serial,device=/dev/ttyS0,timeout=1"
+ )
+ @endverbatim
[EMAIL PROTECTED] 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 urg_nz_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);
+}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://sourceforge.net/services/buy/index.php
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
