Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv9485/server/drivers/position/nav200
Added Files:
Tag: release-2-0-patches
.cvsignore Makefile.am nav200.cc nav200.h sicknav200.cc
test.cpp test_program.cpp test_program_init.cpp
Log Message:
merged many changes from HEAD
--- NEW FILE: .cvsignore ---
Makefile
Makefile.in
test
test_program
test_program_init
--- NEW FILE: nav200.h ---
/** Basic interface code for the NAV200, player interface provided in
sicknav200.cc
This driver interface code was written by Kathy Fung, the player
interfaces was later added by Toby Collett.
*/
#ifndef _NAV200_H
#define _NAV200_H
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <strings.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <math.h>
#include <stdint.h>
#define DEFAULT_PORT "/dev/ttyS0"
#define DEFAULT_RATE B19200
#define STX 0x02
#define MAXLEN 255
#define BUFFER_SIZE 256
#define HEADER_SIZE 4
#define FOOTER_SIZE 1
typedef struct Nav200Command
{
uint8_t header;
uint8_t length;
uint8_t mode;
uint8_t function;
uint8_t data [MAXLEN-HEADER_SIZE-FOOTER_SIZE+1];
int dataLength;
uint8_t BCC;
}Nav200Command;
// typedef struct ReflectorInfo
// {
// uint8_t layer;
// uint8_t number;
// }ReflectorInfo;
typedef struct PositionXY
{//position is in mm
int x;
int y;
}PositionXY;
typedef struct ReflectorData
{
uint8_t layer;
uint8_t number; // reflector number
PositionXY pos;
}ReflectorData;
typedef struct LaserPos
{
PositionXY pos; // position of the laser scanner
short orientation;
uint8_t quality;
uint8_t number; // number of reflectors used
}LaserPos;
typedef struct ErrorBytes
{
uint8_t F0; // function byte of the last command
uint8_t F1; // error class
uint8_t F2; // error group
uint8_t F3; // error specification
}ErrorBytes;
class Nav200
{
public:
Nav200();
~Nav200();
int Initialise(const char * port = DEFAULT_PORT, int rate = DEFAULT_RATE);
int Terminate();
int ProcessData();
// standby mode
bool EnterStandby();
int GetVersionNumber();
char* GetVersionString(); //String pointer return is only valid till the next
request to Nav200
short GetDeviceSerial();
bool rotateDirection(uint8_t direction);
bool GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY &
reflector);
bool ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int
newY);
bool InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int Y);
bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY &
reflector);
// read and set reflector radii
int GetReflectorRadius(uint8_t layer);
bool SetReflectorRadius(uint8_t layer, uint8_t radius);
// mapping mode
bool EnterMapping();
int StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t
radius);
int StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short
orientation, uint8_t radius);
int StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int
Y, short orientation, uint8_t radius);
bool MappingPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
// positioning mode
bool EnterPositioning();
bool EnterPositioningInput(uint8_t NumberOfMeasurements);
bool GetPositionAuto(LaserPos & laserPosition);
bool GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition);
bool GetPositionSpeedVelocity(short speedX, short speedY, short velocity,
LaserPos & laserPosition);
bool GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short
velocity, LaserPos & laserPosition);
bool ChangeLayer(uint8_t layer);
bool ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation);
bool SetActionRadii(int min, int max);
bool SelectNearest(uint8_t N_nearest);
// upload mode
bool EnterUpload();
bool GetUploadTrans(uint8_t layer, ReflectorData & reflector);
// download mode
bool EnterDownload();
bool DownloadReflector(uint8_t layer, uint8_t number, int X, int Y);
protected:
// serial port descriptor
int fd;
struct termios oldtio;
uint8_t receivedBuffer[BUFFER_SIZE];
int bytesReceived;
Nav200Command packet;
ErrorBytes error;
void PrintErrorMsg(void);
int ReadFromNav200(int timeout_usec=5000000);
int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
uint8_t CreateCRC(uint8_t* data, ssize_t len);
};
#endif
--- NEW FILE: test_program_init.cpp ---
#include <stdlib.h>
#include <time.h>
#include "nav200.h"
#define LAYER 0
#define R_RADIUS 100
int main(int argc,char ** argv)
{
//Standby mode
PositionXY reflector;
Nav200 testing;
testing.Initialise();
if (!testing.EnterStandby())
{
fprintf(stderr,"unable to enter standby mode\n");
EXIT_FAILURE;
}
PositionXY reflectordata[256];
int max = 0;
if(testing.EnterMapping())
{
printf("Entered mapping, scanning please wait...");
max = testing.StartMapping(LAYER, 0, 0, 0, R_RADIUS);
printf("Number of reflectors scanned = %d\n",max);
for(int i=0; i<max; i++)
{
if(testing.MappingPosition(LAYER,i,reflectordata[i]))
printf("position of Reflector %d: position X=%d, Y=%d\n",i,
reflectordata[i].x,reflectordata[i].y);
}
}
else
{
fprintf(stderr,"unable to enter mapping mode\n");
EXIT_FAILURE;
}
// upload mode
// if (!testing.EnterStandby())
// {
// fprintf(stderr,"unable to enter standby mode\n");
// EXIT_FAILURE;
// }
// ReflectorData reflectors[numOfReflector];
// ReflectorData temp;
//
// printf("Number of reflectors scanned = %d\n",numOfReflector);
// if(testing.EnterUpload())
// {
// printf("\n\n\nEntered upload mode\n\n\n");
// // int i=0;
// // while(1)
// for(int i=0; i<numOfReflector; i++)
// {
// if(testing.GetUploadTrans(0,temp))
// printf("Uploaded reflector is: X = %d, Y = %d, layer = %d, number
= %d\n",temp.pos.x,temp.pos.y,temp.layer,temp.number);
// if(temp.number==255)
// break;
// reflectors[i].pos.x = temp.pos.x;
// reflectors[i].pos.y = temp.pos.y;
// reflectors[i].layer = temp.layer;
// reflectors[i].number = temp.number;
// // i++;
// }
// }
// else
// {
// fprintf(stderr,"unable to enter upload mode\n");
// EXIT_FAILURE;
// }
// download mode
if (!testing.EnterStandby())
{
fprintf(stderr,"unable to enter standby mode\n");
EXIT_FAILURE;
}
if(testing.EnterDownload())
{
printf("\n\n\nEntered download mode\n\n\n");
for(int i=0; i<max; i++)
{
testing.DownloadReflector(LAYER,i,reflectordata[i].x,reflectordata[i].y);
}
testing.DownloadReflector(LAYER,255,0,0);
}
else
{
fprintf(stderr,"unable to enter download mode\n");
EXIT_FAILURE;
}
for(int i=0; i<max; i++)
{
printf("downloaded reflectors %d: X=%d,
Y=%d\n",i,reflectordata[i].x,reflectordata[i].y);
}
return 0;
}
--- NEW FILE: Makefile.am ---
noinst_LTLIBRARIES =
if INCLUDE_SICKNAV200
noinst_LTLIBRARIES += libsicknav200.la
noinst_PROGRAMS = test_program_init test_program test
endif
AM_CPPFLAGS = -Wall -I$(top_srcdir)
libsicknav200_la_SOURCES = sicknav200.cc nav200.h nav200.cc
test_program_init_SOURCES = test_program_init.cpp
test_program_init_LDADD = libsicknav200.la
test_program_SOURCES = test_program.cpp
test_program_LDADD = libsicknav200.la
test_SOURCES = test.cpp
test_LDADD = libsicknav200.la
--- NEW FILE: nav200.cc ---
#include "nav200.h"
Nav200::Nav200()
{
}
Nav200::~Nav200()
{
}
int Nav200::Initialise(const char * port, int rate)
{
bytesReceived = 0;
fd = -1;
// open the serial port
fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
if ( fd<0 )
{
fprintf(stderr, "Could not open serial device %s\n",port);
return -1;
}
// save the current io settings
tcgetattr(fd, &oldtio);
// set up new settings
struct termios newtio;
memset(&newtio, 0,sizeof(newtio));
newtio.c_cflag = CS8 | CREAD | PARENB;
newtio.c_iflag = INPCK;
newtio.c_oflag = 0;
newtio.c_lflag = 0;
// activate new settings
tcflush(fd, TCIFLUSH);
if (cfsetispeed(&newtio, rate) < 0 || cfsetospeed(&newtio, rate) < 0)
{
fprintf(stderr,"Failed to set serial baud rate: %d\n", rate);
tcsetattr(fd, TCSANOW, &oldtio);
close(fd);
fd = -1;
return -1;
}
tcsetattr(fd, TCSANOW, &newtio);
tcflush(fd, TCIOFLUSH);
// clear the input buffer in case junk data is on the port
usleep(10000);
tcflush(fd, TCIFLUSH);
return 0;
}
int Nav200::Terminate()
{
// restore old port settings
if (fd > 0)
{
tcsetattr(fd, TCSANOW, &oldtio);
close(fd);
}
return 0;
}
/*****************
* Standby mode
*******************/
bool Nav200::EnterStandby()
{
WriteCommand('S','A',0,NULL);
if (ReadFromNav200() > 0)
{
if (packet.length==5 && packet.mode == 'S' && packet.function == 'A')
return true;
}
return false;
}
int Nav200::GetVersionNumber()
{
WriteCommand('S','V',0,NULL);
if (ReadFromNav200() > 0)
{
if (packet.length==8 && packet.mode == 'S' && packet.function == 'V')
{
//printf("Version:
%d.%d.%d\n",packet.data[0],packet.data[1],packet.data[2]);
return (*reinterpret_cast<int*> (packet.data))&0xFFFFFF;
}
}
return 0;
}
char* Nav200::GetVersionString()
{
WriteCommand('S','T',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.mode == 'S' && packet.function == 'T')
{
packet.data[packet.dataLength]='\0';
return(reinterpret_cast<char*>(packet.data));
}
}
return NULL;
}
short Nav200::GetDeviceSerial()
{
WriteCommand('S','S',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==7 && packet.mode == 'S' && packet.function == 'S')
return *reinterpret_cast<short*> (packet.data);
}
return 0;
}
bool Nav200::rotateDirection(uint8_t direction)
{
WriteCommand('S','U',1,&direction);
if(ReadFromNav200()>0)
{
if(packet.length==6 && packet.mode == 'S' && packet.function == 'U' /*&&
*reinterpret_cast<uint8_t*> (packet.data) == direction*/)
{
return true;
}
}
return false;
}
bool Nav200::GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY &
reflector)
{
uint8_t data[2] = {layer,number};
WriteCommand('S','R',2,data);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'S' && packet.function == 'R' &&
packet.data[0]==layer && packet.data[1]==number)
{
reflector.x = *reinterpret_cast<int*> (packet.data+2);
reflector.y = *reinterpret_cast<int*> (packet.data+6);
return true;
}
}
return false;
}
bool Nav200::ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX,
int newY)
{
uint8_t command[10];
command[0] = layer;
command[1] = number;
*reinterpret_cast<int*> (&command[2]) = newX;
*reinterpret_cast<int*> (&command[6]) = newY;
WriteCommand('S','C',10,command);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'S' && packet.function == 'C' &&
packet.data[0]==layer && packet.data[1]==number && *reinterpret_cast<int*>
(packet.data+2) == newX && *reinterpret_cast<int*> (packet.data+6) == newY)
return true;
}
return false;
}
bool Nav200::InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int
Y)
{
uint8_t command[10];
command[0] = layer;
command[1] = number;
*reinterpret_cast<int*> (&command[2]) = X;
*reinterpret_cast<int*> (&command[6]) = Y;
WriteCommand('S','I',10,command);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'S' && packet.function == 'I' &&
packet.data[0]==layer && packet.data[1]==number && *reinterpret_cast<int*>
(packet.data+2) == X && *reinterpret_cast<int*> (packet.data+6) == Y)
return true;
}
return false;
}
bool Nav200::DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY
& reflector)
{
uint8_t data[2] = {layer,number};
WriteCommand('S','D',2,data);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'S' && packet.function == 'D' &&
packet.data[0]==layer && packet.data[1]==number)
{
reflector.x = *reinterpret_cast<int*> (packet.data+2);
reflector.y = *reinterpret_cast<int*> (packet.data+6);
// printf("deleted reflector is %d\n",reflector.x);
return true;
}
}
return false;
}
/*********************************
* Read and set reflector radii
***********************************/
int Nav200::GetReflectorRadius(uint8_t layer)
{
// ReflectorSize reflector;
WriteCommand('R','G',1,&layer);
if(ReadFromNav200()>0)
{
if(packet.length==7 && packet.mode == 'R' && packet.function == 'G' &&
packet.data[0] == layer)
{
return packet.data[1];
}
}
return -1;
}
bool Nav200::SetReflectorRadius(uint8_t layer, uint8_t radius)
{
uint8_t data[2] = {layer,radius};
WriteCommand('R','S',2,data);
if(ReadFromNav200()>0)
{
if(packet.length==7 && packet.mode == 'R' && packet.function == 'G' &&
packet.data[0] == layer && packet.data[1] == radius)
return true;
}
return false;
}
/*****************
* Mapping mode
*******************/
bool Nav200::EnterMapping()
{
WriteCommand('M','A',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==5 && packet.mode == 'M' && packet.function == 'A')
return true;
}
return false;
}
int Nav200::StartMapping(uint8_t layer, int X, int Y, short orientation,
uint8_t radius)
{
uint8_t data[12];
data[0] = layer;
*reinterpret_cast<int*> (&data[1]) = X;
*reinterpret_cast<int*> (&data[5]) = Y;
*reinterpret_cast<short*> (&data[9]) = orientation;
data[11] = radius;
WriteCommand('M','S',12,data);
if(ReadFromNav200(100000000)>0)
{
if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' &&
packet.data[0] == layer)
return packet.data[1];
}
return -1;
}
int Nav200::StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y,
short orientation, uint8_t radius)
{
uint8_t data[13];
data[0] = layer;
data[1] = scans;
*reinterpret_cast<int*> (&data[2]) = X;
*reinterpret_cast<int*> (&data[6]) = Y;
*reinterpret_cast<short*> (&data[10]) = orientation;
data[12] = radius;
WriteCommand('M','M',13,data);
if(ReadFromNav200(100000000)>0)
{
if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' &&
packet.data[0] == layer)
return packet.data[1];
}
return -1;
}
int Nav200::StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int
X, int Y, short orientation, uint8_t radius)
{
uint8_t data[13];
data[0] = layer;
data[1] = scans;
*reinterpret_cast<int*> (&data[2]) = X;
*reinterpret_cast<int*> (&data[6]) = Y;
*reinterpret_cast<short*> (&data[10]) = orientation;
data[12] = radius;
WriteCommand('M','N',13,data);
if(ReadFromNav200()>0)
{
if(packet.length==7 && packet.mode == 'M' && packet.function == 'S' &&
packet.data[0] == layer)
return packet.data[1];
}
return -1;
}
bool Nav200::MappingPosition(uint8_t layer, uint8_t number, PositionXY &
reflector)
{
uint8_t data[2] = {layer,number};
WriteCommand('M','R',2,data);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'M' && packet.function == 'R' &&
packet.data[0] == layer && packet.data[1] == number)
{
reflector.x = *reinterpret_cast<int*> (packet.data+2);
reflector.y = *reinterpret_cast<int*> (packet.data+6);
return true;
}
}
return false;
}
/*****************
* Positioning mode
*******************/
bool Nav200::EnterPositioning()
{
WriteCommand('P','A',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==5 && packet.mode == 'P' && packet.function == 'A')
return true;
}
return false;
}
bool Nav200::EnterPositioningInput(uint8_t NumberOfMeasurements)
{
WriteCommand('P','N',1,&NumberOfMeasurements);
if(ReadFromNav200()>0)
{
if(packet.length==5 && packet.mode == 'P' && packet.function == 'A')
return true;
}
return false;
}
bool Nav200::GetPositionAuto(LaserPos & laserPosition)
{
WriteCommand('P','P',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
{
laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
laserPosition.quality = packet.data[10];
laserPosition.number = packet.data[11];
return true;
}
}
return false;
}
bool Nav200::GetPositionSpeed(short speedX, short speedY, LaserPos &
laserPosition)
{
uint8_t data[4];
*reinterpret_cast<short*> (&data[0]) = speedX;
*reinterpret_cast<short*> (&data[2]) = speedY;
WriteCommand('P','v',4,data);
if(ReadFromNav200()>0)
{
if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
{
laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
laserPosition.quality = packet.data[10];
laserPosition.number = packet.data[11];
return true;
}
}
return false;
}
bool Nav200::GetPositionSpeedVelocity(short speedX, short speedY, short
velocity, LaserPos & laserPosition)
{
uint8_t data[6];
*reinterpret_cast<short*> (&data[0]) = speedX;
*reinterpret_cast<short*> (&data[2]) = speedY;
*reinterpret_cast<short*> (&data[4]) = velocity;
WriteCommand('P','w',6,data);
if(ReadFromNav200()>0)
{
if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
{
laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
laserPosition.quality = packet.data[10];
laserPosition.number = packet.data[11];
return true;
}
}
return false;
}
bool Nav200::GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short
velocity, LaserPos & laserPosition)
{
uint8_t data[6];
*reinterpret_cast<short*> (&data[0]) = speedX;
*reinterpret_cast<short*> (&data[2]) = speedY;
*reinterpret_cast<short*> (&data[4]) = velocity;
WriteCommand('P','V',6,data);
if(ReadFromNav200()>0)
{
if(packet.length==17 && packet.mode == 'P' && packet.function == 'P')
{
laserPosition.pos.x = *reinterpret_cast<int*> (packet.data);
laserPosition.pos.y = *reinterpret_cast<int*> (packet.data+4);
laserPosition.orientation = *reinterpret_cast<short*> (packet.data+8);
laserPosition.quality = packet.data[10];
laserPosition.number = packet.data[11];
return true;
}
}
return false;
}
bool Nav200::ChangeLayer(uint8_t layer)
{
WriteCommand('P','L',1,&layer);
if(ReadFromNav200()>0)
{
if(packet.length==6 && packet.mode=='P' && packet.function=='L' &&
packet.data[0] == layer)
return true;
}
return false;
}
bool Nav200::ChangeLayerDefPosition(uint8_t layer, int X, int Y, short
orientation)
{
uint8_t data[11];
data[0] = layer;
*reinterpret_cast<int*> (&data[1]) = X;
*reinterpret_cast<int*> (&data[5]) = Y;
*reinterpret_cast<short*> (&data[9]) = orientation;
WriteCommand('P','M',11,data);
if(ReadFromNav200()>0)
{
if(packet.length==16 && packet.mode == 'P' && packet.function == 'M' &&
packet.data[0] == layer && *reinterpret_cast<int*> (packet.data+1) == X &&
*reinterpret_cast<int*> (packet.data+5) == Y && *reinterpret_cast<short*>
(packet.data+9) == orientation)
return true;
}
return false;
}
bool Nav200::SetActionRadii(int min, int max)
{
uint8_t data[8];
*reinterpret_cast<int*> (&data[0]) = min;
*reinterpret_cast<int*> (&data[4]) = max;
WriteCommand('P','O',8,data);
if(ReadFromNav200()>0)
{
if(packet.length==13 && packet.mode == 'P' && packet.function == 'O' &&
*reinterpret_cast<int*> (packet.data)==min && *reinterpret_cast<int*>
(packet.data+4)==max)
return true;
}
return false;
}
bool Nav200::SelectNearest(uint8_t N_nearest)
{
WriteCommand('P','C',1,&N_nearest);
if(ReadFromNav200()>0)
{
if(packet.length==6 && packet.mode == 'P' && packet.function == 'C' &&
packet.data[0] == N_nearest)
return true;
}
return false;
}
/*****************
* upload mode
*******************/
bool Nav200::EnterUpload()
{
WriteCommand('U','A',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==5 && packet.mode == 'U' && packet.function == 'A')
return true;
}
return false;
}
bool Nav200::GetUploadTrans(uint8_t layer, ReflectorData & reflector)
{
WriteCommand('U','R',1,&layer);
if(ReadFromNav200()>0)
{
if(packet.length==15 && packet.mode == 'U' && packet.function == 'R' &&
packet.data[0] == layer)
{
reflector.layer = packet.data[0];
reflector.number = packet.data[1];
reflector.pos.x = *reinterpret_cast<int*> (packet.data+2);
reflector.pos.y = *reinterpret_cast<int*> (packet.data+6);
return true;
}
}
return false;
}
/*****************
* download mode
******************/
bool Nav200::EnterDownload()
{
WriteCommand('D','A',0,NULL);
if(ReadFromNav200()>0)
{
if(packet.length==5 && packet.mode == 'D' && packet.function == 'A')
return true;
}
return false;
}
bool Nav200::DownloadReflector(uint8_t layer, uint8_t number, int X, int Y)
{
uint8_t data[10];
data[0] = layer;
data[1] = number;
*reinterpret_cast<int*> (&data[2]) = X;
*reinterpret_cast<int*> (&data[6]) = Y;
WriteCommand('D','R',10,data);
if(ReadFromNav200()>0)
{
if(packet.length == 7 && packet.mode == 'D' && packet.function == 'R' &&
packet.data[0] == layer && packet.data[1] == number)
{
return true;
}
}
return false;
}
/*****************
* Error Messages
******************/
void Nav200::PrintErrorMsg(void)
{
fprintf(stderr,"ERROR: Last command entry %c:", error.F0);
switch(error.F1)
{
case 1: fprintf(stderr,"User's software error: "); break;
case 2: fprintf(stderr,"Error class: Transputer: "); break;
case 3: fprintf(stderr,"Error in the connection between TPU and sensor:
"); break;
case 4: fprintf(stderr,"Spurious measurement: "); break;
case 5: fprintf(stderr,"Spurious angular measurement: "); break;
case 6: fprintf(stderr,"Error in connection to reflector memory: ");
break;
case 7: fprintf(stderr,"Error in connection between user computer and TPU
(RS-232 interface): "); break;
}
switch(error.F2)
{
case 1: fprintf(stderr,"input/output, telegram traffic. "); break;
case 2: fprintf(stderr,"standby mode. "); break;
case 3: fprintf(stderr,"handling reference reflectors. "); break;
case 4: fprintf(stderr,"download mode. "); break;
case 5: fprintf(stderr,"upload mode. "); break;
case 6: fprintf(stderr,"mapping mode. "); break;
case 7: fprintf(stderr,"positioning mode. "); break;
case 8: fprintf(stderr,"test mode. "); break;
case 9: fprintf(stderr,"navigation operation, general. "); break;
}
switch(error.F3)
{
case 1: fprintf(stderr,"Unknown command.\n"); break;
case 2: fprintf(stderr,"Command(function) not implemented.\n"); break;
case 3: fprintf(stderr,"Wrong Command\n"); break;
case 4: fprintf(stderr,"Reflector not available\n"); break;
case 5: fprintf(stderr,"Wrong reflector number\n"); break;
case 6: fprintf(stderr,"Wrong data block\n"); break;
case 7: fprintf(stderr,"Input not possible\n"); break;
case 8: fprintf(stderr,"Invalid layer\n"); break;
case 9: fprintf(stderr,"No reflectors in layer\n"); break;
case 10: fprintf(stderr,"All reflectors transferred\n"); break;
case 11: fprintf(stderr,"Communication error\n"); break;
case 12: fprintf(stderr,"Error in the initialisation of reference
reflectors\n"); break;
case 13: fprintf(stderr,"Wrong radii during input of effective range\n");
break;
case 14: fprintf(stderr,"Flash EPROM not functional\n"); break;
case 15: fprintf(stderr,"Wrong reflector radius\n"); break;
}
}
////////////////////////////////////////////////////////////////////////////////
// Write a packet to Nav200
int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t *
data)
{
if (fd < 0)
return -1;
uint8_t * buffer;
int length = dataLength+5;
buffer = new uint8_t[length];
// Create header
buffer[0] = STX;
buffer[1] = length;
buffer[2] = mode;
buffer[3] = function;
// Copy data
memcpy(buffer + 4, data, dataLength);
// Create footer (CRC)
buffer[length-1] = CreateCRC(buffer, 4 + dataLength);
// Make sure both input and output queues are empty
tcflush(fd, TCIOFLUSH);
// switch to blocking IO for the write
int flags = fcntl(fd, F_GETFL);
if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0)
{
fprintf(stderr,"Error changing to blocking write (%d - %s),
disabling\n",errno,strerror(errno));
tcsetattr(fd, TCSANOW, &oldtio);
fd = -1;
return -1;
}
if((length && (write(fd, buffer, length)) < length))
{
fprintf(stderr,"Error writing to FOB (%d - %s),
disabling\n",errno,strerror(errno));
tcsetattr(fd, TCSANOW, &oldtio);
fd = -1;
return -1;
}
// restore flags
if (fcntl(fd,F_SETFL,flags) < 0)
{
fprintf(stderr,"Error restoring file mode (%d - %s),
disabling\n",errno,strerror(errno));
tcsetattr(fd, TCSANOW, &oldtio);
fd = -1;
return -1;
}
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Read a packet from the Nav200
int Nav200::ReadFromNav200(int timeout_usec)
{
int ret;
int dataLength = 0;
struct timeval start, now;
gettimeofday(&start,NULL);
for (;;)
{
gettimeofday(&now,NULL);
if ((now.tv_sec - start.tv_sec) * 1e6 + now.tv_usec - start.tv_usec >
timeout_usec)
{
fprintf(stderr,"Timed out waiting for packet %d uSecs
passed\n",static_cast<int>((now.tv_sec - start.tv_sec) * 1e6 + now.tv_usec -
start.tv_usec));
return -1;
}
ret = read(fd, &receivedBuffer[bytesReceived], BUFFER_SIZE - bytesReceived);
if (ret < 0)
{
fprintf(stderr,"Got error while reading %d %s\n",errno, strerror(errno));
return ret;
}
bytesReceived += ret;
if (ret == 0)
{
usleep(1000);
continue;
}
// do we have enough for a header?
while (bytesReceived > 4)
{
if (receivedBuffer[0] != STX)
{
// bad thing, we dont have the correct start to a message
for(int i=1; i<bytesReceived; i++)
{ // find where STX is
if(receivedBuffer[i]==STX)
{ // move so STX is at start
memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
bytesReceived-=i;
}
}
continue;
}
// we have valid start byte
// get length
dataLength = receivedBuffer[1];
// check we have enough data for full length, if not then break
if (dataLength > bytesReceived)
{
break;
}
// calc CRC and check it
// if bad CRC, assume same as bad STX
if (CreateCRC(receivedBuffer, dataLength)) // change that later!!
{// bad thing, we dont have the correct start to a message
fprintf(stderr,"bad CRC!!!\n");
for(int i=1; i<bytesReceived; i++)
{ // find where STX is
if(receivedBuffer[i]==STX)
{ // move so STX is at start
memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i);
bytesReceived-=i;
}
}
continue;
}
// if good CRC we actually have a packet so assemble our NAV200 message
and return it to the application
else
{
if(receivedBuffer[3]=='E'){
error.F0 = receivedBuffer[4];
error.F1 = receivedBuffer[5];
error.F2 = receivedBuffer[6];
error.F3 = receivedBuffer[7];
//check out what the error is and it out
PrintErrorMsg();
return -2;
}
packet.header = receivedBuffer[0];
packet.length = receivedBuffer[1];
packet.mode = receivedBuffer[2];
packet.function = receivedBuffer[3];
packet.dataLength = packet.length-HEADER_SIZE-FOOTER_SIZE;
memcpy(packet.data, receivedBuffer+4, packet.dataLength);
packet.BCC = receivedBuffer[4+packet.dataLength];
memmove(receivedBuffer, receivedBuffer+dataLength,
bytesReceived-dataLength);
bytesReceived-=dataLength;
return 1;
}
}
}
return 0;
}
uint8_t Nav200::CreateCRC(uint8_t* data, ssize_t len)
{
uint8_t result = 0;
for (int ii = 0; ii < len; ++ii)
result ^= data[ii];
return result;
}
--- NEW FILE: sicknav200.cc ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* 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: Driver for the SICK S3000 laser
Author: Toby Collett (based on lms200 by Andrew Howard)
Date: 7 Nov 2000
CVS: $Id: sicknav200.cc,v 1.1.2.1 2006/09/22 23:58:35 gerkey Exp $
*/
/** @ingroup drivers Drivers */
/** @{ */
/** @defgroup driver_sicknav200 sicknav200
* @brief SICK NAV200 laser localisation unit
The sicknav200 driver interfaces to the NAV200 localiation unit and provides
the current position
output of the device.
Currently the driver assumes the nav200 has been correctly initialised and
loaded with the
reflector layers.
@par Compile-time dependencies
- none
@par Provides
- @ref interface_laser
@par Requires
- none
@par Configuration requests
- PLAYER_POSITION2D_REQ_GET_GEOM
@par Configuration file options
- port (string)
- Default: "/dev/ttyS0"
- Serial port to which laser is attached. If you are using a
USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".
- pose (length tuple)
- 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).
- size (length tuple)
- Default: [0.15 0.15]
- Footprint (x,y) of the laser.
@par Example
@verbatim
driver
(
name "sicknav200"
provides ["position2d:0"]
port "/dev/ttyS0"
)
@endverbatim
@author Kathy Fung, Toby Collett, inro technologies
*/
/** @} */
#if HAVE_CONFIG_H
#include <config.h>
#endif
#include <assert.h>
#include <math.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <arpa/inet.h> // for htons etc
#include <libplayercore/playercore.h>
#include <replace/replace.h>
extern PlayerTime* GlobalTime;
#include "nav200.h"
// The laser device class.
class SickNAV200 : public Driver
{
public:
// Constructor
SickNAV200(ConfigFile* cf, int section);
~SickNAV200();
int Setup();
int Shutdown();
// MessageHandler
int ProcessMessage(MessageQueue * resp_queue,
player_msghdr * hdr,
void * data);
private:
// Main function for device thread.
virtual void Main();
protected:
// Laser pose in robot cs.
double pose[3];
double size[2];
// Name of device used to communicate with the laser
const char *device_name;
// storage for outgoing data
player_position2d_data_t data_packet;
// nav200 parameters
Nav200 Laser;
int min_radius, max_radius;
};
// a factory creation function
Driver* SickNAV200_Init(ConfigFile* cf, int section)
{
return((Driver*)(new SickNAV200(cf, section)));
}
// a driver registration function
void SickNAV200_Register(DriverTable* table)
{
table->AddDriver("sicknav200", SickNAV200_Init);
}
////////////////////////////////////////////////////////////////////////////////
// Error macros
#define RETURN_ERROR(erc, m) {PLAYER_ERROR(m); return erc;}
////////////////////////////////////////////////////////////////////////////////
// Constructor
SickNAV200::SickNAV200(ConfigFile* cf, int section)
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_POSITION2D_CODE)
{
// Laser geometry.
this->pose[0] = cf->ReadTupleLength(section, "pose", 0, 0.0);
this->pose[1] = cf->ReadTupleLength(section, "pose", 1, 0.0);;
this->pose[2] = cf->ReadTupleLength(section, "pose", 2, 0.0);;
this->size[0] = 0.15;
this->size[1] = 0.15;
// Serial port
this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT));
// nav200 parameters, conver to cm
this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius", 1)
* 1000);
this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius",
30) * 1000);
return;
}
SickNAV200::~SickNAV200()
{
delete device_name;
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device
int SickNAV200::Setup()
{
PLAYER_MSG1(2, "NAV200 initialising (%s)", this->device_name);
// Open the terminal
Laser.Initialise(this->device_name);
if (!Laser.EnterStandby() || !Laser.EnterPositioning())
{
PLAYER_ERROR("unable to enter standby or position mode\n");
return -1;;
}
if (!Laser.SetActionRadii(min_radius, max_radius))
{
PLAYER_ERROR("failed to set action radii\n");
return -1;;
}
PLAYER_MSG0(2, "NAV200 ready");
// Start the device thread
StartThread();
return 0;
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int SickNAV200::Shutdown()
{
// shutdown laser device
StopThread();
PLAYER_MSG0(2, "laser shutdown");
return(0);
}
int
SickNAV200::ProcessMessage(MessageQueue * resp_queue,
player_msghdr * hdr,
void * data)
{
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
this->device_addr))
{
if(hdr->size != 0)
{
PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
hdr->size, 0);
return(PLAYER_MSGTYPE_RESP_NACK);
}
player_position2d_geom_t geom;
geom.pose.px = this->pose[0];
geom.pose.py = this->pose[1];
geom.pose.pa = this->pose[2];
geom.size.sl = this->size[0];
geom.size.sw = this->size[1];
this->Publish(this->device_addr,
resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM,
(void*)&geom, sizeof(geom), NULL);
return(0);
}
// Don't know how to handle this message.
return(-1);
}
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void SickNAV200::Main()
{
LaserPos Reading;
for(;;)
{
// test if we are supposed to cancel
pthread_testcancel();
// process any pending messages
ProcessMessages();
// get update and publish result
if(Laser.GetPositionAuto(Reading))
{
data_packet.pos.px = static_cast<double> (Reading.pos.x)/1000;
data_packet.pos.py = static_cast<double> (Reading.pos.y)/1000;
data_packet.pos.pa = static_cast<double> (Reading.orientation)/1000;
this->Publish(this->device_addr,
NULL,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
(void*)&data_packet, sizeof(data_packet), NULL);
}
else
{
PLAYER_WARN("Failed to get reading from laser scanner\n");
usleep(100000);
}
}
}
--- NEW FILE: test_program.cpp ---
#include <stdlib.h>
#include "nav200.h"
#define MIN 100
#define MAX 1000
int main(int argc,char ** argv)
{
Nav200 testing;
testing.Initialise();
if (!testing.EnterStandby())
{
fprintf(stderr,"unable to enter standby mode\n");
return EXIT_FAILURE;
}
LaserPos laser;
while(1)
{
if(testing.EnterPositioning())
{
printf("\n\n\nEntered positioning mode\n\n");
if(testing.SetActionRadii(MIN,MAX))
printf("changed operation radii\n");
if(testing.GetPositionAuto(laser))
printf("Position of the laser scanner: X=%d, Y=%d, orientation=%d,
quality=%d, number of reflectors =
%d\n",laser.pos.x,laser.pos.y,laser.orientation,laser.quality,laser.number);
}
else
{
fprintf(stderr,"unable to enter positioning mode\n");
return EXIT_FAILURE;
}
}
return 0;
}
--- NEW FILE: test.cpp ---
#include <stdlib.h>
#include <time.h>
#include "nav200.h"
int main(int argc,char ** argv)
{
//Standby mode
PositionXY reflector;
Nav200 testing;
testing.Initialise();
if (testing.EnterStandby())
{
printf("\n\n\nEntered Standby\n\n");
// if(testing.rotateDirection(0))
// printf("direction of rotation is anti-clockwise\n");
// return 0;
printf("Version = %08X\n",testing.GetVersionNumber());
printf("Serial = %d\n", testing.GetDeviceSerial());
printf("Version = %s\n",testing.GetVersionString());
if(testing.InsertReflectorPosition(1,0,100,200))
printf("reflectors added in layer 1\n");
if(testing.InsertReflectorPosition(1,0,300,400))
printf("reflectors added in layer 1\n");
if(testing.InsertReflectorPosition(1,0,500,600))
printf("reflectors added in layer 1\n");
// {
// printf("reflector added: %d: %d,
%d\n",i,reflectordata[i].x,reflectordata[i].y);
// }
// if(testing.GetReflectorPosition(0, 0, reflector)) // get reflector
// printf("\nGot Reflector 01: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// if(testing.GetReflectorPosition(0, 1, reflector)) // get reflector
// printf("Got Reflector 02: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// if(testing.GetReflectorPosition(0, 2, reflector)) // get reflector
// printf("Got Reflector 03: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// if(testing.GetReflectorPosition(0, 3, reflector)) // get reflector
// printf("Got Reflector 04: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// // delete reflectors
// if(testing.DeleteReflectorPosition(0,0,reflector))
// // printf("\n");
// printf("\n1:Got Reflector: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// if(testing.DeleteReflectorPosition(0,0,reflector))
// // printf("\n");
// printf("2:Got Reflector: position X=%d,
Y=%d\n",reflector.x,reflector.y);
// if(testing.DeleteReflectorPosition(0,0,reflector))
// // printf("\n");
// printf("3:Got Reflector: position X=%d,
Y=%d\n",reflector.x,reflector.y);
}
//Mapping mode
PositionXY reflectordata[4];
int max = 0;
if(testing.EnterMapping())
{
printf("\n\n\nEntered mapping\n\n");
max = testing.StartMapping(0, 0, 0, 0, 100);
printf("Number of reflectors scanned = %d\n",max);
for(int i=0; i<max; i++)
{
if(testing.MappingPosition(0,i,reflectordata[i]))
printf("position of Reflector %d: position X=%d, Y=%d\n",i,
reflectordata[i].x,reflectordata[i].y);
}
}
if(testing.EnterStandby())
{
for(int i=max-1; i>=0; i--)
if(testing.InsertReflectorPosition(0,0,reflectordata[i].x,reflectordata[i].y))
{
printf("reflector added: %d: %d,
%d\n",i,reflectordata[i].x,reflectordata[i].y);
}
}
// printf("\ntesting on mapping mode is done\n");
LaserPos laser;
if(testing.EnterPositioning())
{
printf("\n\n\nEntered positioning mode\n\n");
if(testing.EnterPositioningInput(3))
printf("\nactive position mode with sliding\n\n");
// if(testing.ChangeLayer(0))
// printf("\n\nlayer changed\n");
//
// if(testing.ChangeLayerDefPosition(0, 0, 0, 0))
// printf("\nselect new layer with position definition\n");
if(testing.SetActionRadii(200,1000))
printf("changed operation radii\n");
if(testing.SelectNearest(max))
printf("selected N nearest\n");
if(testing.GetPositionAuto(laser))
printf("Position of the laser scanner: X=%d, Y=%d, orientation=%d,
quality=%d, number of reflectors =
%d\n",laser.pos.x,laser.pos.y,laser.orientation,laser.quality,laser.number);
if(testing.GetPositionSpeed(1000,1000,laser))
printf("Position of the laser scanner: X=%d, Y=%d, orientation=%d,
quality=%d, number of reflectors =
%d\n",laser.pos.x,laser.pos.y,laser.orientation,laser.quality,laser.number);
if(testing.GetPositionSpeedVelocity(1000,1000,25,laser))
printf("Position of the laser scanner: X=%d, Y=%d, orientation=%d,
quality=%d, number of reflectors =
%d\n",laser.pos.x,laser.pos.y,laser.orientation,laser.quality,laser.number);
if(testing.GetPositionSpeedVelocityAbsolute(1000,1000,25,laser))
printf("Position of the laser scanner: X=%d, Y=%d, orientation=%d,
quality=%d, number of reflectors =
%d\n",laser.pos.x,laser.pos.y,laser.orientation,laser.quality,laser.number);
if(testing.ChangeLayer(1))
printf("\n\nlayer changed\n");
if(testing.ChangeLayerDefPosition(0, 0, 0, 0))
printf("\nselect new layer with position definition\n");
}
printf("\nEnd of positioning mode\n\n\n\n");
// upload mode
ReflectorData upload_reflector;
testing.EnterStandby();
if(testing.EnterUpload())
{
printf("\n\n\nEntered upload mode\n\n\n");
if(testing.GetUploadTrans(0,upload_reflector))
printf("Uploaded reflector is: X = %d, Y = %d, layer = %d, number =
%d\n",upload_reflector.pos.x,upload_reflector.pos.y,upload_reflector.layer,upload_reflector.number);
if(testing.GetUploadTrans(0,upload_reflector))
printf("Uploaded reflector is: X = %d, Y = %d, layer = %d, number =
%d\n",upload_reflector.pos.x,upload_reflector.pos.y,upload_reflector.layer,upload_reflector.number);
if(testing.GetUploadTrans(0,upload_reflector))
printf("Uploaded reflector is: X = %d, Y = %d, layer = %d, number =
%d\n",upload_reflector.pos.x,upload_reflector.pos.y,upload_reflector.layer,upload_reflector.number);
}
// download mode
testing.EnterStandby();
if(testing.EnterDownload())
{
printf("\n\n\nEntered download mode\n\n\n");
if(testing.DownloadReflector(2,0,111,222))
printf("Downloaded reflector\n");
if(testing.DownloadReflector(2,1,333,444))
printf("Downloaded reflector\n");
if(testing.DownloadReflector(2,2,555,666))
printf("Downloaded reflector\n");
if(testing.DownloadReflector(2,255,0,0))
printf("Downloaded is done\n\n");
}
// upload mode
ReflectorData upload_reflectors[5];
ReflectorData temp;
testing.EnterStandby();
if(testing.EnterUpload())
{
printf("\n\n\nEntered upload mode\n\n\n");
int i=0;
while(1)
{
if(testing.GetUploadTrans(2,temp))
printf("Uploaded reflector is: X = %d, Y = %d, layer = %d, number =
%d\n",temp.pos.x,temp.pos.y,temp.layer,temp.number);
if(temp.number==255)
break;
upload_reflectors[i].pos.x = temp.pos.x;
upload_reflectors[i].pos.y = temp.pos.y;
upload_reflectors[i].layer = temp.layer;
upload_reflectors[i].number = temp.number;
i++;
}
}
printf("download mode ends\n\n\n");
if(testing.EnterStandby())
{
for(int ii=0; ii<max; ii++)
if(testing.DeleteReflectorPosition(0,0,reflector))
printf("Delete Reflector: position X=%d,
Y=%d\n",reflector.x,reflector.y);
for(int ii=0; ii<3; ii++)
if(testing.DeleteReflectorPosition(1,0,reflector))
printf("Delete Reflector in layer 1: position X=%d,
Y=%d\n",reflector.x,reflector.y);
for(int ii=0; ii<3; ii++)
if(testing.DeleteReflectorPosition(2,0,reflector))
printf("Delete Reflector in layer 2: position X=%d,
Y=%d\n",reflector.x,reflector.y);
}
return 0;
}
//not working list
// bool rotateDirection(uint8_t direction); //absolutely not working for some
unknown reason
// bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY &
reflector); <-- return incorrect X value
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys -- and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
