Revision: 6515 http://playerstage.svn.sourceforge.net/playerstage/?rev=6515&view=rev Author: gbiggs Date: 2008-06-10 03:14:33 -0700 (Tue, 10 Jun 2008)
Log Message: ----------- Fixed a mismatched if Modified Paths: -------------- code/player/trunk/client_libs/libplayerc++/CMakeLists.txt code/player/trunk/server/drivers/ranger/CMakeLists.txt code/player/trunk/server/drivers/ranger/urg_nz.cc code/player/trunk/utils/playerv/pv_dev_ranger.c Modified: code/player/trunk/client_libs/libplayerc++/CMakeLists.txt =================================================================== --- code/player/trunk/client_libs/libplayerc++/CMakeLists.txt 2008-06-10 09:46:54 UTC (rev 6514) +++ code/player/trunk/client_libs/libplayerc++/CMakeLists.txt 2008-06-10 10:14:33 UTC (rev 6515) @@ -90,10 +90,10 @@ PLAYERCC_ADD_LINK_LIB (boost_signals) MESSAGE (STATUS "PlayerC++ client library will be built with Boost::Signals support.") - ELSE (Boost_thread_FOUND) + ELSE (Boost_signals_FOUND) MESSAGE (STATUS "Boost::Signals library not found, support will not be included.") - ENDIF (Boost_thread_FOUND) + ENDIF (Boost_signals_FOUND) ELSE (Boost_FOUND) MESSAGE (STATUS "Boost libraries were not found. Boost::Signals and Boost::Thread support Modified: code/player/trunk/server/drivers/ranger/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/ranger/CMakeLists.txt 2008-06-10 09:46:54 UTC (rev 6514) +++ code/player/trunk/server/drivers/ranger/CMakeLists.txt 2008-06-10 10:14:33 UTC (rev 6515) @@ -9,8 +9,13 @@ 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_ADD_DRIVER (urg_nz build_urg_nz INCLUDEDIRS "${urg_nz_includeDir}" LIBDIRS "${urg_nz_libDir}" - LINKFLAGS "${urg_nz_linkFlags}" CFLAGS "${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}" 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 09:46:54 UTC (rev 6514) +++ code/player/trunk/server/drivers/ranger/urg_nz.cc 2008-06-10 10:14:33 UTC (rev 6515) @@ -35,38 +35,33 @@ /** @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 +69,366 @@ - 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; 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) +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), readings (NULL), - minIndex (0), maxIndex (urg_nz::MAX_READINGS) +UrgDriver::UrgDriver (ConfigFile* cf, int section) : + Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE), + _baudRate ("baudrate", DEFAULT_BAUDRATE, false), _ranges (NULL) { - // 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 the baudrate and motor speed + RegisterProperty ("baudrate", &_baudRate, cf, section); - // 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); + // 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); - // 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)); + // 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)); - // 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) { - 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; - } + 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; - // 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; - } + if (_powerOnStartup) + _device.SetPower (true); - StartThread(); - return 0; + 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; } int UrgDriver::Shutdown (void) { - StopThread(); + StopThread (); - device.Close (); + _device.Close (); + _data.CleanUp (); + if (_ranges != NULL) + delete[] _ranges; - if (ranges != NULL) - { - delete[] ranges; - ranges = NULL; - } - if (readings != NULL) - { - delete readings; - readings = NULL; - } - - return 0; + 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); + // 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) { - 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.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 - } + 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; 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; - } + 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; + return true; } -bool UrgDriver::CalculateMinMaxIndices (void) +bool UrgDriver::AllocateDataSpace (void) { - unsigned int minPossibleIndex, maxPossibleIndex; + if (_ranges != NULL) + delete _ranges; - // 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); - } + 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; + } - return true; + return true; } Modified: code/player/trunk/utils/playerv/pv_dev_ranger.c =================================================================== --- code/player/trunk/utils/playerv/pv_dev_ranger.c 2008-06-10 09:46:54 UTC (rev 6514) +++ code/player/trunk/utils/playerv/pv_dev_ranger.c 2008-06-10 10:14:33 UTC (rev 6515) @@ -48,7 +48,7 @@ snprintf(label, sizeof(label), "ranger:%d (%s)", index, ranger->drivername); ranger->menu = rtk_menu_create_sub(mainwnd->device_menu, label); ranger->subscribe_item = rtk_menuitem_create(ranger->menu, "Subscribe", 1); - ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 1); + ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 0); ranger->intns_item = rtk_menuitem_create(ranger->menu, "Draw intensity data", 1); ranger->device_item = rtk_menuitem_create(ranger->menu, "Singular", 1); 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