Update of /cvsroot/playerstage/code/player/server/drivers/laser
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27174/server/drivers/laser
Modified Files:
Tag: release-2-0-patches
urg_laser.cc urg_laser.h urglaserdriver.cc
Log Message:
merged driver changes from HEAD
Index: urglaserdriver.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/laser/urglaserdriver.cc,v
retrieving revision 1.7.2.1
retrieving revision 1.7.2.2
diff -C2 -d -r1.7.2.1 -r1.7.2.2
*** urglaserdriver.cc 7 Jun 2006 16:12:48 -0000 1.7.2.1
--- urglaserdriver.cc 24 Apr 2007 22:20:20 -0000 1.7.2.2
***************
*** 27,31 ****
The urglaser driver controls the Hokuyo URG scanning laser range-finder.
! Communication with the laser can be either via USB or RS232.
@par Compile-time dependencies
--- 27,32 ----
The urglaser driver controls the Hokuyo URG scanning laser range-finder.
! Communication with the laser can be either via USB or RS232. The driver
! supports both Hokuyo's SCIP1 and 2 protocols.
@par Compile-time dependencies
***************
*** 46,49 ****
--- 47,51 ----
- PLAYER_LASER_REQ_GET_CONFIG
- PLAYER_LASER_REQ_SET_CONFIG
+ - PLAYER_LASER_REQ_GET_ID
@par Configuration file options
***************
*** 60,64 ****
- min_angle, max_angle (angle float)
! - Default: [-2.0 2.0] (or [-115.0 115.0] in degrees)
- Minimum and maximum scan angles to return
--- 62,66 ----
- min_angle, max_angle (angle float)
! - Default: [-2.094 2.094] (or [-120.0 120.0] in degrees)
- Minimum and maximum scan angles to return
***************
*** 84,94 ****
@endverbatim
! @author Toby Collett
*/
/** @} */
-
-
#include <unistd.h>
#include <string.h>
--- 86,94 ----
@endverbatim
! @author Toby Collett, Nico Blodow
*/
/** @} */
#include <unistd.h>
#include <string.h>
***************
*** 108,115 ****
#include "urg_laser.h"
#include <libplayercore/playercore.h>
! class URGLaserDriver : public Driver {
! public:
// Constructor;
--- 108,119 ----
#include "urg_laser.h"
+ #define URG04_MIN_STEP 44
+ #define URG04_MAX_STEP 725
+
#include <libplayercore/playercore.h>
! class URGLaserDriver : public Driver
! {
! public:
// Constructor;
***************
*** 127,131 ****
void * data);
! private:
// Main function for device thread.
virtual void Main();
--- 131,135 ----
void * data);
! private:
// Main function for device thread.
virtual void Main();
***************
*** 139,143 ****
bool UseSerial;
! int BaudRate;
char * Port;
};
--- 143,148 ----
bool UseSerial;
! int BaudRate, min_i, max_i;
! float user_min_angle, user_max_angle;
char * Port;
};
***************
*** 147,223 ****
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
! URGLaserDriver::URGLaserDriver(ConfigFile* cf, int section)
: Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_LASER_CODE)
{
! //init vars
! memset(&Data, 0, sizeof(Data));
! memset(&Geom, 0, sizeof(Geom));
! memset(&Conf, 0, sizeof(Conf));
! Geom.size.sw = (0.050);
! Geom.size.sl = (0.050);
!
! Readings = new urg_laser_readings_t;
! assert(Readings);
!
! // read options from config file
! Geom.pose.px = (cf->ReadTupleLength(section,"pose",0,0));
! Geom.pose.py = (cf->ReadTupleLength(section,"pose",1,0));
! Geom.pose.pa = (cf->ReadTupleAngle(section,"pose",2,0));
!
! //set up config structure
! Conf.min_angle = cf->ReadAngle(section,"min_angle",DTOR(-115));
! Conf.max_angle = cf->ReadAngle(section,"max_angle",DTOR(115));
! Conf.resolution = DTOR(270.0/769.0);
! Conf.max_range = 4.0;
! Conf.range_res = 0.001;
! Conf.intensity = 0;
! int b = cf->ReadInt(section, "baud", 115200);
! switch(b)
! {
! case 115200:
! BaudRate = B115200;
! break;
! case 57600:
! BaudRate = B57600;
! break;
! case 19200:
! BaudRate = B19200;
! break;
! default:
! PLAYER_WARN1("ignoring invalid baud rate %d", b);
! BaudRate = B115200;
! break;
! }
! Port = strdup(cf->ReadString(section, "port", "/dev/ttyACM0"));
! UseSerial = (cf->ReadInt(section, "use_serial", 0)==1);
! return;
}
! URGLaserDriver::~URGLaserDriver()
{
! delete Readings;
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
! int URGLaserDriver::Setup() {
! //config data
! if(Laser.Open(Port,UseSerial,BaudRate) < 0)
! {
! this->SetError(1);
! return -1;
! }
!
!
! // Start the device thread; spawns a new thread and executes
! // ExampleDriver::Main(), which contains the main loop for the driver.
! StartThread();
! return(0);
}
--- 152,265 ----
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
! URGLaserDriver::URGLaserDriver (ConfigFile* cf, int section)
: Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_LASER_CODE)
{
+ //init vars
+ memset (&Data, 0, sizeof (Data));
+ memset (&Geom, 0, sizeof (Geom));
+ memset (&Conf, 0, sizeof (Conf));
+ Geom.size.sw = (0.050);
+ Geom.size.sl = (0.050);
! Readings = new urg_laser_readings_t;
! assert (Readings);
! // read options from config file
! Geom.pose.px = (cf->ReadTupleLength (section, "pose", 0, 0));
! Geom.pose.py = (cf->ReadTupleLength (section, "pose", 1, 0));
! Geom.pose.pa = (cf->ReadTupleAngle (section, "pose", 2, 0));
! // set up config structure
! Conf.min_angle = cf->ReadAngle (section, "min_angle", DTOR (-120));
! Conf.max_angle = cf->ReadAngle (section, "max_angle", DTOR (120));
! user_min_angle = Conf.min_angle;
! user_max_angle = Conf.max_angle;
!
! // Conf.resolution = DTOR (270.0/769.0);
! Conf.resolution = DTOR (360.0/1024.0);
! Conf.max_range = 4.0;
! Conf.range_res = 0.001;
! Conf.intensity = 0;
!
! int b = cf->ReadInt (section, "baud", 115200);
! switch (b)
! {
! case 115200:
! BaudRate = B115200;
! break;
! case 57600:
! BaudRate = B57600;
! break;
! case 19200:
! BaudRate = B19200;
! break;
! default:
! PLAYER_WARN1 ("ignoring invalid baud rate %d", b);
! BaudRate = B115200;
! break;
! }
! Port = strdup (cf->ReadString (section, "port", "/dev/ttyACM0"));
! UseSerial = (cf->ReadInt (section, "use_serial", 0) == 1);
}
!
////////////////////////////////////////////////////////////////////////////////
! URGLaserDriver::~URGLaserDriver ()
{
! delete Readings;
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
! int
! URGLaserDriver::Setup ()
! {
! //config data
! if (Laser.Open (Port, UseSerial, BaudRate) < 0)
! {
! this->SetError(1);
! return -1;
! }
! Laser.GetSensorConfig (&Conf);
!
! // Solve the min/max angle problem
! min_i = static_cast<int> (round (384 + Conf.min_angle/Conf.resolution));
! max_i = static_cast<int> (round (384 + Conf.max_angle/Conf.resolution));
!
! // For ancient firmware versions, set some hard limits on the min/max angle
capabilities
! if (min_i < URG04_MIN_STEP)
! min_i = URG04_MIN_STEP;
! if (max_i > URG04_MAX_STEP)
! max_i = URG04_MAX_STEP;
+ int user_min_i = static_cast<int> (round (384 +
user_min_angle/Conf.resolution));
+ int user_max_i = static_cast<int> (round (384 +
user_max_angle/Conf.resolution));
+
+ if (user_min_i > user_max_i)
+ user_min_i = user_max_i;
+
+ // We restrict the URG 04-LX to its capabilities
+ if (user_min_i < min_i)
+ {
+ PLAYER_WARN2 ("restricting the lower index bound to %d from %d", min_i,
user_min_i);
+ user_min_i = min_i;
+ }
+ min_i = user_min_i;
! if (user_max_i > max_i)
! {
! PLAYER_WARN2 ("restricting the upper index bound to %d from %d", max_i,
user_max_i);
! user_max_i = max_i;
! }
! max_i = user_max_i;
!
! Conf.min_angle = (min_i - 384) * Conf.resolution;
! Conf.max_angle = (max_i - 384) * Conf.resolution;
!
! // Start the device thread; spawns a new thread and executes
! // ExampleDriver::Main(), which contains the main loop for the driver.
! StartThread ();
!
! return (0);
}
***************
*** 225,239 ****
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
! int URGLaserDriver::Shutdown() {
// Stop and join the driver thread
! StopThread();
! Laser.Close();
! return(0);
}
! int URGLaserDriver::ProcessMessage(MessageQueue* resp_queue,
player_msghdr * hdr,
void * data)
--- 267,285 ----
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
! int
! URGLaserDriver::Shutdown ()
! {
// Stop and join the driver thread
! StopThread ();
! Laser.Close ();
! return (0);
}
!
////////////////////////////////////////////////////////////////////////////////
! int
! URGLaserDriver::ProcessMessage (MessageQueue* resp_queue,
player_msghdr * hdr,
void * data)
***************
*** 261,308 ****
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
! void URGLaserDriver::Main()
{
! // The main loop; interact with the device here
! for(;;) {
! // test if we are supposed to cancel
! pthread_testcancel();
!
! // Process any pending messages
! ProcessMessages();
!
! int min_i = static_cast<int> (384 +
Conf.min_angle/Conf.resolution);
! int max_i = static_cast<int> (384 +
Conf.max_angle/Conf.resolution);
!
! if (min_i > max_i)
! min_i = max_i;
! if (min_i < 0)
! min_i = 0;
!
! if (max_i > 769)
! max_i = 769;
!
!
! // update device data
! Laser.GetReadings(Readings);
! Data.min_angle = Conf.min_angle;
! Data.max_angle = Conf.max_angle;
! // TODO: check this value
! Data.max_range = 4.0;
! Data.resolution = Conf.resolution;
! Data.ranges_count = max_i - min_i;
! for (unsigned int i = 0; i < Data.ranges_count; ++i)
! {
! Data.ranges[i] = Readings->Readings[i+min_i] < 20 ?
(4095) : (Readings->Readings[i+min_i]);
! Data.ranges[i]/=1000;
! }
! Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCAN,
! &Data, sizeof(player_laser_data_t), NULL);
! //printf("Put Data to client\n");
! // Sleep (you might, for example, block on a read() instead)
! //usleep(10);
! }
}
--- 307,344 ----
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
! void
! URGLaserDriver::Main ()
{
! // The main loop; interact with the device here
! for (;;)
! {
! // test if we are supposed to cancel
! pthread_testcancel ();
! // Process any pending messages
! ProcessMessages ();
! // update device data
! Laser.GetReadings (Readings, min_i, max_i);
! Data.min_angle = Conf.min_angle;
! Data.max_angle = Conf.max_angle;
!
! // TODO: check this value
! Data.max_range = Conf.max_range;
! Data.resolution = Conf.resolution;
! Data.ranges_count = (max_i - min_i) + 1;
!
! for (unsigned int i = 0; i < Data.ranges_count; ++i)
! {
!
! Data.ranges[i] = Readings->Readings[i+min_i] < 20 ?
(Data.max_range*1000) : (Readings->Readings[i+min_i]);
! Data.ranges[i] /= 1000;
! }
! Publish (device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN,
! &Data, sizeof(player_laser_data_t), NULL);
! // Sleep (you might, for example, block on a read() instead)
! //usleep(10);
! }
}
***************
*** 312,329 ****
////////////////////////////////////////////////////////////////////////////////
! //Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
! Driver* URGLaserDriver_Init(ConfigFile* cf, int section) {
! // Create and return a new instance of this driver
! return((Driver*)(new URGLaserDriver(cf, section)));
}
! //Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
! int URGLaserDriver_Register(DriverTable* table) {
! table->AddDriver("urglaser", URGLaserDriver_Init);
! return 0;
}
-
-
-
--- 348,366 ----
////////////////////////////////////////////////////////////////////////////////
! // Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
! Driver*
! URGLaserDriver_Init(ConfigFile* cf, int section)
! {
! // Create and return a new instance of this driver
! return ((Driver*)(new URGLaserDriver (cf, section)));
}
! // Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
! int
! URGLaserDriver_Register (DriverTable* table)
! {
! table->AddDriver ("urglaser", URGLaserDriver_Init);
! return (0);
}
Index: urg_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/urg_laser.cc,v
retrieving revision 1.2
retrieving revision 1.2.2.1
diff -C2 -d -r1.2 -r1.2.2.1
*** urg_laser.cc 8 Feb 2006 22:17:21 -0000 1.2
--- urg_laser.cc 24 Apr 2007 22:20:20 -0000 1.2.2.1
***************
*** 12,17 ****
#include "urg_laser.h"
int
! urg_laser::ReadUntil(int fd, unsigned char *buf, int len, int timeout)
{
int ret;
--- 12,313 ----
#include "urg_laser.h"
+
///////////////////////////////////////////////////////////////////////////////
[...1058 lines suppressed...]
!
! // Read "SERI:H"
! ReadUntil (file, Buffer, 6, -1);
! // Read the serial number value
! for (i = 0; ; i++)
{
! ReadUntil (file, &Buffer[i], 1, -1);
! if (Buffer[i] == ';')
! {
! Buffer[i] = 0;
! break;
! }
}
!
! id = atol ((const char*)Buffer);
! ReadUntil (file, Buffer, 3, -1);
! }
! return id;
}
Index: urg_laser.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/urg_laser.h,v
retrieving revision 1.2
retrieving revision 1.2.2.1
diff -C2 -d -r1.2 -r1.2.2.1
*** urg_laser.h 8 Feb 2006 22:17:21 -0000 1.2
--- urg_laser.h 24 Apr 2007 22:20:20 -0000 1.2.2.1
***************
*** 1,3 ****
--- 1,4 ----
#include <stdio.h>
+ #include <libplayercore/playercore.h>
#define MAX_READINGS 769
***************
*** 5,31 ****
typedef struct urg_laser_readings
{
! unsigned short Readings[MAX_READINGS];
} urg_laser_readings_t;
class urg_laser
{
! public:
! urg_laser();
! ~urg_laser();
! int Open(const char * PortName, int use_serial, int baud);
! int Close();
! int ChangeBaud(int curr_baud, int new_baud, int timeout);
! int ReadUntil(int fd, unsigned char *buf,
! int len, int timeout);
! bool PortOpen();
! int GetReadings(urg_laser_readings_t * readings);
!
!
! private:
! FILE * laser_port;
!
};
--- 6,35 ----
typedef struct urg_laser_readings
{
! unsigned short Readings[MAX_READINGS];
} urg_laser_readings_t;
class urg_laser
{
! public:
! urg_laser();
! ~urg_laser();
! int Open(const char * PortName, int use_serial, int baud);
! int Close();
! int ChangeBaud(int curr_baud, int new_baud, int timeout);
! int ReadUntil(int fd, unsigned char *buf, int len, int timeout);
+ int ReadUntil_nthOccurence(int file, int n, char c);
! bool PortOpen();
! int GetReadings (urg_laser_readings_t * readings, int min_i, int
max_i);
! int GetIDInfo ();
! float GetMaxRange ();
! int GetSensorConfig (player_laser_config_t *cfg);
! int GetSCIPVersion ();
!
! private:
! int SCIP_Version;
! FILE * laser_port;
};
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
