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