Update of /cvsroot/playerstage/code/player/server/drivers/laser
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv28595/server/drivers/laser
Modified Files:
Makefile.am
Added Files:
RS4LeuzeLaserDriver.cc RS4Leuze_laser.cc RS4Leuze_laser.h
Log Message:
added rs4leuze driver
--- NEW FILE: RS4Leuze_laser.cc ---
/**
laser.cpp V 2.0 -> RS4Leuze_laser.cpp
Modified by Ernesto Homar Teniente Aviles
Date 20 APril 2007
*/
//include
#include "RS4Leuze_laser.h"
#include <string.h>
/**
Default constructor.
*/
//Claser::Claser(ClogMsg *lgMsg, bool *laser_ON, char dir_name[80]) //default
constructor.
Claser::Claser(int scan_points) //default constructor.
{
points_to_scan = scan_points;
}
/**
Destructor
*/
Claser::~Claser()
{
//close port
closeSerial();
}
void Claser::closeSerial()
{
close(serialFD);
}
void Claser::openSerial(bool *laser_ON, int Baud_rate, char * Port)
{
serialFD = open(Port, O_RDWR|O_NOCTTY);
if (serialFD<0)
{
cout << "Claser, Error opening serial port " << endl;
*laser_ON=0;
return;
}
// Configure Serial Port: termios settings: 57600(default), No Parity,
8 data bits, 1 stop Bit (8N1)
// values of masks in /usr/include/bits/termios.h
tcgetattr(serialFD,&ttyset);
ttyset.c_cflag = ( Baud_rate | CLOCAL | CREAD | CS8 );
ttyset.c_iflag = ( IGNBRK ); //Ignores break condition on input
ttyset.c_oflag = 0x0;
ttyset.c_lflag = 0x0;
// Set configuration immediately.
if (tcsetattr(serialFD, TCSANOW, &ttyset)<0)
{
cout << "Claser, Error opening serial port " << endl;
*laser_ON=0;
return;
}
else
FD_ZERO(&rfds); // Initialize the read set to zero
FD_SET(serialFD, &rfds); // Turn on the read set
// set timer
tv.tv_sec = 1;
tv.tv_usec = 0;
//Flush both pending input and untransmitted output.
tcflush(serialFD, TCIOFLUSH);
}
/**
Reads one byte from serialFD and updates checksum
*/
unsigned char Claser::readByte()
{
unsigned char localByte;
read (serialFD, &localByte, 1);
checksum = checksum ^ localByte;
return localByte;
}
/**
sync function synchronizes with the start of the laser data
*/
void Claser::sync()
{
int num_zeroes = 0;
// We must read three consecutive 0x00 for the end of the message
while (num_zeroes < 3)
{
read(serialFD, &byte, 1);
if(byte == 0x00) num_zeroes++;
else num_zeroes = 0;
}
// We are now, for a short time, at the beginning of the message.
}
/**
scanRead reads a whole message of a laser scan, loads it to scanData vector
and prints it to laserDataFile. If success returns 0 and returns 1 if failure.
*/
int Claser::scanRead()
{
unsigned int ii;
//******STEP 1: Reading message header
for (ii=0; ii<2; ii++)
{
byte = readByte();
//cout<<"Header: "<< byte << ";" <<endl;
if(byte != 0x00)
{
//cout << " Claser::scanRead(STEP 1), Error reading
Laser message header" << endl;
cout << "Error reading Laser message header" << endl;
return 1;
}
}
readByte(); //Reads but doesn't analyze command byte
option1=readByte(); //Reads Option1;
if (option1 & 0x03 > 1)
{
byte=readByte(); //Reads Option2
}
if (option1 & 0x03 > 2)
{;
byte=readByte(); //Reads Option3
}
// There's a bit that determines the existance of the password field
if ((option1 & 0x20) != 0x00)
{
for (ii=0; ii<8; ii++)
{
byte=readByte(); //Reads Password
}
}
//******STEP 2: Reading data header
scan_number = 0;
for (ii=0; ii<8; ii++)
{
if (ii%2 == 0)
{
scan_number = scan_number * 256 + readByte();
}
else
{
byte=readByte();
if (byte != 0xFE)
{
//cout << "Claser::scanRead(STEP 2), Error
reading Laser message header" << endl;
cout << "Error reading Laser message header" <<
endl;
return 1;
}
}
}
byte=readByte();//Resolution byte
output_start=readByte()*256;//ouput Start
output_start+=readByte();
output_stop=readByte()*256;//ouput Stop
output_stop+=readByte();
//******STEP 3: Reading laser scan /*data*/
//cout << "points_to_scan "<< points_to_scan << std::endl;
for (ii=0; ii<2*points_to_scan; ii++)
{
if (ii%2 == 0)
{
// 3.1: reading the first Byte of two from de scaned
point
scanedPoint = (readByte() * 256);
}
else
{
// 3.2: reading the second Byte of two from de scaned
point, and must mask the last bit
scanedPoint += readByte() & 0xFE;
// 3.3 joins the two Bytes froms the scaned point, and
they are converted from mm to meters
scanData.Reading[(ii-1)/2] =
(double)(scanedPoint/1000.0);
}
}
//******STEP 4: Reading Control Byte for checksum and ending message;
read(serialFD, &controlByte, 1);//just read without checksum
/*********************!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*****/
return(0);
}
void Claser::runLaser()
{
//Claser *thisLaser = (Claser*)thisPnt;
//zeroTimeStamp=clock(); //Initializes time stamp
FD_ZERO(&rfds); // Initialize the read set to zero
FD_SET(serialFD, &rfds); // Turn on the read set
// set timer
tv.tv_sec = 1;
tv.tv_usec = 0;
if(select(serialFD+1, &rfds, NULL, NULL, &tv))
{
this->sync(); //Synchronizes
this->checksum=0x00; //Reset Checksum
this->scanRead(); //Read all scan Message
}
else
{
cout<<"Laser disconnected!!!!!!!!!!!!!!!!"<<endl;
}
}
--- NEW FILE: RS4Leuze_laser.h ---
/**
laser.h V 2.0 -> RS4Leuze_laser.cpp
Modified by Ernesto Homar Teniente Aviles
Date 08 May 2007
*/
#ifndef RS4Leuze_laser_h
#define RS4Leuze_laser_h
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <sys/time.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
using namespace std;
#define MAX_SCAN_POINTS 529
typedef struct RS4Leuze_laser_readings {
double Reading[MAX_SCAN_POINTS];
} RS4Leuze_laser_readings_t;
//classes
/**
Claser implements functions to read data from a laser scanner ROTOSCAN RS4-4
(Leuze corp.) connected to a serial port.
*/
class Claser
{
private:
char *portName; /**<Serial Port where laser is connected*/
int serialFD; /**<Serial port file descriptor*/
termios ttyset; /**<termios variable to configure serial port*/
fd_set rfds;
int selectResult; /**<Laser Message fields*/
unsigned char byte;
unsigned char checksum;
unsigned char option1;
long unsigned int scan_number;
unsigned int output_start;
unsigned int output_stop;
unsigned int scanedPoint;
unsigned char controlByte;
unsigned int points_to_scan;
struct timeval tv;/**<termios variable time interval*/
timeval timeStamp; /**<Time in microseconds resolution*/
ofstream laserDataFile; /**<Laser Scanner Data file*/
public:
//Claser(ClogMsg *lgMsg, bool *laser_ON, char dir_name[80]);
/**<Opens serial port*/
Claser(int scan_points); /**<Opens serial port*/
~Claser();
void closeLaser();/**<Closes serial Port and data file*/
unsigned char readByte(); /**<Reads one byte and updates
checksum message*/
void sync();
//void readScan(); /**<Sets to scanData array values of last
laser scanner*/
void writeConfig(); /**<Write configuration parameters to laser
scanner device*/
int scanRead(); /**<reads one scan and puts it in scanData
array*/
void runLaser(); /**<Return the scan reading from the laser*/
void closeSerial(); /**<Closes serial Port */
void openSerial(bool *laser_ON,int Baud_rate, char * Port);
/**<Opens serial Port and get the default paarameters or those given in the
.cfg file*/
RS4Leuze_laser_readings_t scanData;
};
#endif
--- NEW FILE: RS4LeuzeLaserDriver.cc ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* 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 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
*
*/
/////////////////////////////////////////////////////////////////////////////////////
// Desc: RS4-Leuze range finder laser driver
// Author: Ernesto Homar Teniente Avilés
// (C) Institut de Robótica Industrial
// Universidad Politécnica de Catalunya
// Date: May, 2007
// Version: 1.5.001
//
//
// Usage:
// (empty)
//
// Theory of operation:
// Based on the
//
// Known Bugs:
// (empty)
//
// Possible enhancements:
// (empty)
//
/////////////////////////////////////////////////////////////////////////////////////
/** @ingroup drivers */
/** @{ */
/** @defgroup driver_RS4Leuzelaser RS4LeuzeLaser
* @brief RS4-E leuze laser range-finder
The RS4-LeuzeLaser driver acquire date from the Leuze RS4 scanning laser
range-finder.
Communication with the laser is via RS232.
@par Compile-time dependencies
- none
@par Provides
- @ref interface_laser
@par Requires
- none
@par Configuration requests
- PLAYER_LASER_REQ_GET_GEOM
- PLAYER_LASER_REQ_GET_CONFIG
- PLAYER_LASER_REQ_SET_CONFIG
@par Configuration file options
- port (string)
- Default: "/dev/ttyS1"
- Port to which the laser is connected. If you are using a USB/232 converter,
this will be "dev/ttyUSBx".
- pose (float tuple m m rad)
- Default: [0.0 0.0 0.0]
- Pose (x,y,theta) of the laser, relative to its parent object (e.g.,
the robot to which the laser is attached).
- min_angle, max_angle (angle float)
- Default: [] (or [-5.04 185.04] in degrees)
- Minimum and maximum scan angles to return
- baud (integer)
- Default: 57600
- Baud rate to use when communicating with the laser over RS232. Valid
rates are: 4800, 9600,
19200, 38400, 57600, and 115200. It should be chosen accordingly with the
RS4 laser settings.
- scan_points (integer)
- Default: 132
- Others : 176, 264, 528
- invert_data(bool)
- Default: 1 (invert_data_on) Leuze data must be inverted in order to be used
in player
- 0 (invert_data_off) - Is the laser physically inverted (i.e.,
upside-down)? Is so, scan data
will not be reversed accordingly.
@par Example
@verbatim
driver
(
name "rs4leuzelaser"
plugin "RS4LeuzeLaserDriver.la"
provides ["laser:0"]
port "/dev/ttyS1"
scan_points "528"
#invert_data "0"
)
@endverbatim
@author Ernesto Homar Teniente Avilés
IRI-UPC
CONACYT-Schoolarship
*/
/** @} */
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/select.h>
#include <sys/time.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <math.h>
#include <termios.h>
#include <vector>
#include <time.h>
#include <iostream>
#include <fstream>
#include "RS4Leuze_laser.h"
#include <libplayercore/playercore.h>
using namespace std;
//Header file. Player driver class for the RS4 Leuze Laser(1);
class RS4LeuzeLaserDriver : public Driver {
public:
// Constructor, teh constructor takes a CongifgFile parameter
//and an integer section parameter(2);
RS4LeuzeLaserDriver(ConfigFile* cf, int section);
// Destructor
~RS4LeuzeLaserDriver();
//The driver must implement the abstract Setup and Shutdown methods
(3);
// Implementations of virtual functions
int Setup();
int Shutdown();
//The drivers re-implements the ProcessMessage method to provide
support for
//handling request and commands(4)
// This method will be invoked on each incoming message
virtual int ProcessMessage(MessageQueue* resp_queue,
player_msghdr * hdr,
void * data);
private:
//The drivers re-implements Main, wich will be called when the dirver
thread
//is started(5)
// Main function for device thread.
virtual void Main();
RS4Leuze_laser_readings_t Readings;
Claser *myLaser;
bool laser_ON;
player_laser_data_t Data;
player_laser_geom_t Geom;
player_laser_config_t Conf;
//bool UseSerial;
int BaudRate;
char * Port;
bool invert;
int ScanPoints;
struct timeval tv;/**<termios variable time interval*/
timeval timeStamp; /**<Time in microseconds resolution*/
};
////////////////////////////////////////////////////////////////////////////////
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
RS4LeuzeLaserDriver::RS4LeuzeLaserDriver(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);
// 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.pyaw = (cf->ReadTupleAngle(section,"pose",2,0));
//set up config structure
Conf.min_angle = cf->ReadAngle(section,"min_angle",DTOR(-5.04));
Conf.max_angle = cf->ReadAngle(section,"max_angle",DTOR(185.04));
//Conf.resolution = depends on the laser resolution
Conf.max_range = 50;
Conf.range_res = 0.01;
Conf.intensity = 0;
// serial configuration
cout << "myLaser RS4 Leuze:"<< endl;
int b = cf->ReadInt(section, "baud", 57600);
switch(b)
{
case 115200:
BaudRate = B115200;
break;
case 57600:
BaudRate = B57600;
break;
case 38400:
BaudRate = B38400;
break;
case 19200:
BaudRate = B19200;
break;
case 9600:
BaudRate = B9600;
break;
case 4800:
BaudRate = B4800;
break;
default:
PLAYER_WARN1("ignoring invalid baud rate %d", b);
BaudRate = B57600;
b = 57600;
break;
}
cout << "baud rate : " << b << endl;
Port = strdup(cf->ReadString(section, "port", "/dev/ttyS1"));
cout << "port :" << Port << endl;
// Scan points configuration
int sc = cf->ReadInt(section, "scan_points", 132);
switch(sc)
{
case 132:
ScanPoints = 133;
Conf.resolution = DTOR(4*0.36);
break;
case 176:
ScanPoints = 177;
Conf.resolution = DTOR(3*0.36);
break;
case 264:
ScanPoints = 265;
Conf.resolution = DTOR(2*0.36);
break;
case 528:
ScanPoints = 529;
Conf.resolution = DTOR(0.36);
break;
default:
PLAYER_WARN1("ignoring invalid scan points %d", sc);
ScanPoints = 133;
Conf.resolution = DTOR(4*0.36);
break;
}
cout << "scan points : " << ScanPoints << endl;
//invert data from teh leuze. Check if the leuze is
upside-down.Normally dat must be inverted
//int sc = cf->ReadInt(section, "scan_points", 132);
invert = cf -> ReadInt(section, "invert_data", 1);
laser_ON = 1;
myLaser=new Claser(ScanPoints);
;
return;
}
RS4LeuzeLaserDriver::~RS4LeuzeLaserDriver()
{
//Reading are erased
delete myLaser;
//delete Readings;
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
int RS4LeuzeLaserDriver::Setup() {
//config data
// Si el el puerto no fue abierto, entonces mandar error. if(*mylaser.
//if(Laser.Open(Port,UseSerial,BaudRate) < 0)
//{
// this->SetError(1);
// return -1;
//}
cout << "S4LeuzeLaserDriver::Setup" << endl;
// Start the device thread; spawns a new thread and executes
// ExampleDriver::Main(), which contains the main loop for the driver.
StartThread();
return(0);
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int RS4LeuzeLaserDriver::Shutdown() {
// Stop and join the driver thread
StopThread();
myLaser->closeSerial();
return(0);
}
int RS4LeuzeLaserDriver::ProcessMessage(MessageQueue* resp_queue,
player_msghdr * hdr,
void * data)
{
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_GEOM,
this->device_addr))
{
Publish(device_addr,resp_queue,
PLAYER_MSGTYPE_RESP_ACK,hdr->subtype,&Geom,sizeof(Geom),NULL);
}
else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_CONFIG,
this->device_addr))
{
Publish(device_addr,resp_queue,
PLAYER_MSGTYPE_RESP_ACK,hdr->subtype,&Conf,sizeof(Conf),NULL);
}
else
{
return -1;
}
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void RS4LeuzeLaserDriver::Main()
{
cout << "RS4LeuzeLaserDriver::Main" << endl;
bool laser_ON=1;
//int i;
cout<<"Laser Ok"<<endl;
// The main loop; interact with the device here
for(int veces = 0;;veces++) {
// test if we are supposed to cancel
pthread_testcancel();
// Process any pending messages
ProcessMessages();
// update device data
myLaser->openSerial(&laser_ON, BaudRate, Port);
myLaser->runLaser();
Data.min_angle = Conf.min_angle;
Data.max_angle = Conf.max_angle;
Data.max_range = Conf.max_range;
Data.resolution = Conf.resolution;
Data.ranges_count = ScanPoints;
//cout << endl << "Data RS4leuze Player : ";
int top_ii = Data.ranges_count;
float tmp;
cout << "Data: ";
for (unsigned int i = 0; i < Data.ranges_count; ++i)
{
tmp = myLaser->scanData.Reading[i];
if (invert)
{
//Inverting the data, Laser upside
Data.ranges[top_ii] = tmp;
--top_ii;
}
else
{
//Laser upside-down
Data.ranges[i] = tmp;
}
cout << Data.ranges[i] << " ";
}
cout << endl;
Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCAN,
&Data, sizeof(player_laser_data_t), NULL);
//cout << endl << "Data RS4leuze passed ";
myLaser->closeSerial();
getchar();
//cout << endl << "end laser **************" <<endl;
}
}
////////////////////////////////////////////////////////////////////////////////
// Things for building shared object, and functions for registering and creating
// new instances of the driver
////////////////////////////////////////////////////////////////////////////////
//Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
Driver* RS4LeuzeLaserDriver_Init(ConfigFile* cf, int section) {
// Create and return a new instance of this driver
return((Driver*)(new RS4LeuzeLaserDriver(cf, section)));
}
//Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
int RS4LeuzeLaserDriver_Register(DriverTable* table) {
table->AddDriver("rs4leuze", RS4LeuzeLaserDriver_Init);
return 0;
}
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/Makefile.am,v
retrieving revision 1.19
retrieving revision 1.20
diff -C2 -d -r1.19 -r1.20
*** Makefile.am 31 Jul 2007 08:33:45 -0000 1.19
--- Makefile.am 22 Aug 2007 00:19:56 -0000 1.20
***************
*** 41,44 ****
--- 41,48 ----
endif
+ if INCLUDE_RS4LEUZE
+ noinst_LTLIBRARIES += librs4leuze.la
+ endif
+
EXTRA_DIST = lasercspace-1.jpg lasercspace-2.jpg
***************
*** 55,56 ****
--- 59,61 ----
liblasercutter_la_SOURCES = lasercutter.cc lasertransform.h lasertransform.cc
liburglaser_la_SOURCES = urg_laser.cc urg_laser.h urglaserdriver.cc
+ librs4leuze_la_SOURCES = RS4LeuzeLaserDriver.cc RS4Leuze_laser.cc
RS4Leuze_laser.h
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
