Revision: 6516
http://playerstage.svn.sourceforge.net/playerstage/?rev=6516&view=rev
Author: gbiggs
Date: 2008-06-10 03:22:13 -0700 (Tue, 10 Jun 2008)
Log Message:
-----------
Whoops, that wasn't supposed to go in yet...
Modified Paths:
--------------
code/player/trunk/server/drivers/ranger/CMakeLists.txt
code/player/trunk/server/drivers/ranger/urg_nz.cc
Modified: code/player/trunk/server/drivers/ranger/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/ranger/CMakeLists.txt 2008-06-10
10:14:33 UTC (rev 6515)
+++ code/player/trunk/server/drivers/ranger/CMakeLists.txt 2008-06-10
10:22:13 UTC (rev 6516)
@@ -9,13 +9,8 @@
PLAYERDRIVER_OPTION (urg_nz build_urg_nz ON)
PLAYERDRIVER_REQUIRE_PKG (urg_nz build_urg_nz urg_nz urg_nz_includeDir
urg_nz_libDir
urg_nz_linkFlags urg_nz_cFlags)
-PLAYERDRIVER_REQUIRE_PKG (urg_nz build_urg_nz flexiport flexiport_includeDir
flexiport_libDir
- flexiport_linkFlags flexiport_cFlags)
-PLAYERDRIVER_ADD_DRIVER (urg_nz build_urg_nz
- INCLUDEDIRS "${urg_nz_includeDir} ${flexiport_includeDir}"
- LIBDIRS "${urg_nz_libDir} ${flexiport_libDir}"
- LINKFLAGS "${urg_nz_linkFlags} ${flexiport_linkFlags}"
- CFLAGS "${urg_nz_cFlags} ${flexiport_cFlags}"
+PLAYERDRIVER_ADD_DRIVER (urg_nz build_urg_nz INCLUDEDIRS
"${urg_nz_includeDir}" LIBDIRS "${urg_nz_libDir}"
+ LINKFLAGS "${urg_nz_linkFlags}" CFLAGS "${urg_nz_cFlags}"
SOURCES urg_nz.cc)
PLAYERDRIVER_OPTION (lasertoranger build_lasertoranger ON)
Modified: code/player/trunk/server/drivers/ranger/urg_nz.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/urg_nz.cc 2008-06-10 10:14:33 UTC
(rev 6515)
+++ code/player/trunk/server/drivers/ranger/urg_nz.cc 2008-06-10 10:22:13 UTC
(rev 6516)
@@ -35,33 +35,38 @@
/** @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 is via the Gearbox
Flexiport library. The
- driver supports the SCIP protocol 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 can be either via USB or
RS232. The driver
+supports SCIP procol versions 1 and 2.
- @par Compile-time dependencies
[EMAIL PROTECTED] Compile-time dependencies
- - Gearbox library urg_nz
- - Gearbox library flexiport
+- Gearbox library urg_nz
- @par Provides
[EMAIL PROTECTED] Provides
- - @ref interface_ranger : Output ranger interface
+- @ref interface_ranger : Output ranger interface
- @par Configuration requests
[EMAIL PROTECTED] Configuration requests
- - 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).
+- 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.
- @par Configuration file options
[EMAIL PROTECTED] Configuration file options
- - portopts (string)
- - Default: "type=serial,device=/dev/ttyACM0,timeout=1"
- - Options to create the Flexiport port with.
+ - 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.
- 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).
@@ -69,366 +74,348 @@
- Default: [0.0 0.0 0.0]
- Size of the laser in metres.
- min_angle (float, radians)
- - Default: -2.08 rad (-119.0 degrees)
- - Minimum scan angle to return. Will be adjusted if outside the laser's
scannable range.
+ - Default: -2.094 rad (-120.0 degrees)
+ - Minimum scan angle to return.
- max_angle (float, radians)
- - 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.
+ - Default: 2.094 rad (120.0 degrees)
+ - Maximum scan angle to return.
- verbose (boolean)
- Default: false
- Enable verbose debugging information in the underlying library.
- @par Properties
[EMAIL PROTECTED] Properties
- baudrate (integer)
- - Default: 19200bps
- - Change the baud rate of the connection to the laser. See urg_nz
documentation for valid values.
+ - 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.
- @par Example
[EMAIL PROTECTED] Example
- @verbatim
- driver
- (
- name "urg_nz"
- provides ["ranger:0"]
- portopts "type=serial,device=/dev/ttyS0,timeout=1"
- )
- @endverbatim
[EMAIL PROTECTED]
+driver
+(
+ name "urg_nz"
+ provides ["ranger:0"]
+ port "/dev/ttyACM0"
+)
[EMAIL PROTECTED]
- @author Geoffrey Biggs
[EMAIL PROTECTED] 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 = 19200;
-const int DEFAULT_SPEED = 600;
+const int DEFAULT_BAUDRATE = 115200;
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 AllocateDataSpace (void);
+ private:
+ virtual void Main (void);
+ bool ReadLaser (void);
+ bool CalculateMinMaxIndices (void);
- // 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;
+ // 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;
};
-Driver* UrgDriver_Init (ConfigFile* cf, int section)
+Driver*
+UrgDriver_Init (ConfigFile* cf, int section)
{
- return reinterpret_cast <Driver*> (new UrgDriver (cf, section));
+ return reinterpret_cast <Driver*> (new UrgDriver (cf, section));
}
-void urg_nz_Register (DriverTable* table)
+void urg_nz_Register(DriverTable* table)
{
- table->AddDriver ("urg_nz", UrgDriver_Init);
+ table->AddDriver ("urg_nz", UrgDriver_Init);
}
-UrgDriver::UrgDriver (ConfigFile* cf, int section) :
- Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_RANGER_CODE),
- _baudRate ("baudrate", DEFAULT_BAUDRATE, false), _ranges (NULL)
+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 the baudrate and motor speed
- RegisterProperty ("baudrate", &_baudRate, cf, section);
+ // 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 (ranges != NULL)
+ delete[] ranges;
+ if (readings != NULL)
+ delete[] readings;
}
int UrgDriver::Setup (void)
{
- 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
+ {
+ // 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 (_powerOnStartup)
- _device.SetPower (true);
+ // 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;
+ }
- 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;
- }
-
- StartThread ();
- return 0;
+ StartThread();
+ return 0;
}
int UrgDriver::Shutdown (void)
{
- StopThread ();
+ StopThread();
- _device.Close ();
- _data.CleanUp ();
- if (_ranges != NULL)
- delete[] _ranges;
+ device.Close ();
- return 0;
+ if (ranges != NULL)
+ {
+ delete[] ranges;
+ ranges = NULL;
+ }
+ if (readings != NULL)
+ {
+ delete readings;
+ readings = NULL;
+ }
+
+ return 0;
}
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);
- 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 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);
- // 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.");
+ // 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;
+ }
- 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;
+ return -1;
}
void UrgDriver::Main (void)
{
- while (true)
- {
- pthread_testcancel ();
- ProcessMessages ();
+ while (true)
+ {
+ pthread_testcancel ();
+ ProcessMessages ();
- if (!ReadLaser ())
- break;
- }
+ if (!ReadLaser ())
+ break;
+ }
}
bool UrgDriver::ReadLaser (void)
{
- player_ranger_data_range_t rangeData;
+ player_ranger_data_range_t rangeData;
- 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;
- }
+ 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
+ }
- 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);
+ 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;
+ }
- return true;
+ return true;
}
-bool UrgDriver::AllocateDataSpace (void)
+bool UrgDriver::CalculateMinMaxIndices (void)
{
- if (_ranges != NULL)
- delete _ranges;
+ unsigned int minPossibleIndex, maxPossibleIndex;
- 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;
- }
+ // 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);
+ }
- return true;
+ return true;
}
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
