nav200.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2006
00004  *     Kathy Fung, Toby Collett
00005  *
00006  *
00007  *  This library is free software; you can redistribute it and/or
00008  *  modify it under the terms of the GNU Lesser General Public
00009  *  License as published by the Free Software Foundation; either
00010  *  version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *  This library is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *  Lesser General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU Lesser General Public
00018  *  License along with this library; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  */
00021 
00030 #ifndef _NAV200_H
00031 #define _NAV200_H
00032 
00033 #include <libplayercore/playercore.h>
00034 #include <sys/types.h>
00035 #include <sys/stat.h>
00036 #if !defined (WIN32)
00037   #include <sys/time.h>
00038   #include <termios.h>
00039   #include <strings.h>
00040   #include <unistd.h>
00041 #endif
00042 #include <fcntl.h>
00043 #include <stdio.h>
00044 #include <stdlib.h>
00045 #include <errno.h>
00046 #include <string.h>
00047 #include <pthread.h>
00048 #include <math.h>
00049 //#include <stdint.h>
00050 
00051 #if defined (WIN32)
00052   typedef unsigned int ssize_t;
00053 #endif
00054 
00055 #define STX     0x02
00056 #define MAXLEN  255
00057 #define BUFFER_SIZE 256
00058 #define HEADER_SIZE 4
00059 #define FOOTER_SIZE 1
00060 
00061 typedef struct Nav200Command
00062 {
00063         uint8_t header;
00064         uint8_t length;
00065         uint8_t mode;
00066         uint8_t function;
00067         uint8_t data [MAXLEN-HEADER_SIZE-FOOTER_SIZE+1];
00068         int dataLength;
00069         uint8_t BCC;
00070 }Nav200Command;
00071 
00072 // typedef struct ReflectorInfo
00073 // {
00074 //   uint8_t layer;
00075 //   uint8_t number;
00076 // }ReflectorInfo;
00077 
00078 typedef struct PositionXY
00079 {//position is in mm
00080   int x;
00081   int y;
00082 }PositionXY;
00083 
00084 
00085 typedef struct ReflectorData
00086 {
00087   uint8_t layer;
00088   uint8_t number; // reflector number
00089   PositionXY pos;
00090 }ReflectorData;
00091 
00092 
00093 typedef struct LaserPos
00094 {
00095   PositionXY pos; // position of the laser scanner
00096   short orientation;
00097   uint8_t quality;
00098   uint8_t number; // number of reflectors used
00099 }LaserPos;
00100 
00101 typedef struct ErrorBytes
00102 {
00103   uint8_t F0; // function byte of the last command
00104   uint8_t F1; // error class
00105   uint8_t F2; // error group
00106   uint8_t F3; // error specification
00107 }ErrorBytes;
00108 
00109 
00110 
00111 
00112 class Nav200
00113 {
00114 public:
00115 
00116   friend class SickNAV200;
00117   Nav200();
00118   ~Nav200();
00119 
00120   int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id);
00121   int Terminate();
00122 
00123   int ProcessData();
00124 
00125   // standby mode
00126   bool EnterStandby();
00127   int GetVersionNumber();
00128   char* GetVersionString(); //String pointer return is only valid till the next request to Nav200
00129   short GetDeviceSerial();
00130   bool rotateDirection(uint8_t direction);
00131   bool GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00132   bool ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int newY);
00133   bool InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int Y);
00134   bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00135 
00136   // read and set reflector radii
00137   int GetReflectorRadius(uint8_t layer);
00138   bool SetReflectorRadius(uint8_t layer, uint8_t radius);
00139 
00140   // mapping mode
00141   bool EnterMapping();
00142   int StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t radius);
00143   int StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00144   int StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00145   bool MappingPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00146 
00147   // positioning mode
00148   bool EnterPositioning();
00149   bool EnterPositioningInput(uint8_t NumberOfMeasurements);
00150   bool GetPositionAuto(LaserPos & laserPosition);
00151   bool GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition);
00152   bool GetPositionSpeedVelocity(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00153   bool GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00154   bool ChangeLayer(uint8_t layer);
00155   bool ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation);
00156   bool SetActionRadii(int min, int max);
00157   bool SelectNearest(uint8_t N_nearest);
00158 
00159   // upload mode
00160   bool EnterUpload();
00161   bool GetUploadTrans(uint8_t layer, ReflectorData & reflector);
00162   // download mode
00163   bool EnterDownload();
00164   bool DownloadReflector(uint8_t layer, uint8_t number, int X, int Y);
00165 
00166 
00167 protected:
00168   // serial port descriptor
00169   //int fd;
00170   //struct termios oldtio;
00171 
00172   uint8_t receivedBuffer[BUFFER_SIZE];
00173   int bytesReceived;
00174   Nav200Command packet;
00175   ErrorBytes error;
00176 
00177   void PrintErrorMsg(void);
00178 
00179   int ReadFromNav200(int timeout_usec=5000000);
00180   int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
00181   uint8_t CreateCRC(uint8_t* data, ssize_t len);
00182   
00183   // SickNav200 Driver info
00184   Driver *sn200;
00185   
00186   // Opaque info - for setting filter
00187   Device *opaque;
00188   player_devaddr_t opaque_id;
00189   
00190 };
00191 
00192 
00193 
00194 #endif

Last updated 12 September 2005 21:38:45