nav200.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2006
4 * Kathy Fung, Toby Collett
5 *
6 *
7 * This library is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public
9 * License as published by the Free Software Foundation; either
10 * version 2.1 of the License, or (at your option) any later version.
11 *
12 * This library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with this library; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21
30#ifndef _NAV200_H
31#define _NAV200_H
32
33#include <libplayercore/playercore.h>
34#include <sys/types.h>
35#include <sys/stat.h>
36#if !defined (WIN32) || defined (__MINGW32__)
37 #include <sys/time.h>
38 #include <strings.h>
39 #include <unistd.h>
40#endif
41#if !defined (WIN32)
42 #include <termios.h>
43#endif
44
45#include <fcntl.h>
46#include <stdio.h>
47#include <stdlib.h>
48#include <errno.h>
49#include <string.h>
50#include <pthread.h>
51#include <math.h>
52//#include <stdint.h>
53
54#if defined (WIN32) && !defined (__MINGW32__)
55 typedef unsigned int ssize_t;
56#endif
57
58#define STX 0x02
59#define MAXLEN 255
60#define BUFFER_SIZE 256
61#define HEADER_SIZE 4
62#define FOOTER_SIZE 1
63
64typedef struct Nav200Command
65{
66 uint8_t header;
67 uint8_t length;
68 uint8_t mode;
69 uint8_t function;
70 uint8_t data [MAXLEN-HEADER_SIZE-FOOTER_SIZE+1];
71 int dataLength;
72 uint8_t BCC;
74
75// typedef struct ReflectorInfo
76// {
77// uint8_t layer;
78// uint8_t number;
79// }ReflectorInfo;
80
81typedef struct PositionXY
82{//position is in mm
83 int x;
84 int y;
86
87
88typedef struct ReflectorData
89{
90 uint8_t layer;
91 uint8_t number; // reflector number
92 PositionXY pos;
94
95
96typedef struct LaserPos
97{
98 PositionXY pos; // position of the laser scanner
99 short orientation;
100 uint8_t quality;
101 uint8_t number; // number of reflectors used
102}LaserPos;
103
104typedef struct ErrorBytes
105{
106 uint8_t F0; // function byte of the last command
107 uint8_t F1; // error class
108 uint8_t F2; // error group
109 uint8_t F3; // error specification
111
112
113
114
116{
117public:
118
119 friend class SickNAV200;
120 Nav200();
121 ~Nav200();
122
123 int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id);
124 int Terminate();
125
126 int ProcessData();
127
128 // standby mode
129 bool EnterStandby();
130 int GetVersionNumber();
131 char* GetVersionString(); //String pointer return is only valid till the next request to Nav200
132 short GetDeviceSerial();
133 bool rotateDirection(uint8_t direction);
134 bool GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
135 bool ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int newY);
136 bool InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int Y);
137 bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
138
139 // read and set reflector radii
140 int GetReflectorRadius(uint8_t layer);
141 bool SetReflectorRadius(uint8_t layer, uint8_t radius);
142
143 // mapping mode
144 bool EnterMapping();
145 int StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t radius);
146 int StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
147 int StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
148 bool MappingPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
149
150 // positioning mode
151 bool EnterPositioning();
152 bool EnterPositioningInput(uint8_t NumberOfMeasurements);
153 bool GetPositionAuto(LaserPos & laserPosition);
154 bool GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition);
155 bool GetPositionSpeedVelocity(short speedX, short speedY, short velocity, LaserPos & laserPosition);
156 bool GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short velocity, LaserPos & laserPosition);
157 bool ChangeLayer(uint8_t layer);
158 bool ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation);
159 bool SetActionRadii(int min, int max);
160 bool SelectNearest(uint8_t N_nearest);
161
162 // upload mode
163 bool EnterUpload();
164 bool GetUploadTrans(uint8_t layer, ReflectorData & reflector);
165 // download mode
166 bool EnterDownload();
167 bool DownloadReflector(uint8_t layer, uint8_t number, int X, int Y);
168
169
170protected:
171 // serial port descriptor
172 //int fd;
173 //struct termios oldtio;
174
175 uint8_t receivedBuffer[BUFFER_SIZE];
176 int bytesReceived;
177 Nav200Command packet;
178 ErrorBytes error;
179
180 void PrintErrorMsg(void);
181
182 int ReadFromNav200(int timeout_usec=5000000);
183 int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
184 uint8_t CreateCRC(uint8_t* data, ssize_t len);
185
186 // SickNav200 Driver info
187 Driver *sn200;
188
189 // Opaque info - for setting filter
190 Device *opaque;
191 player_devaddr_t opaque_id;
192
193};
194
195
196
197#endif
Encapsulates a device (i.e., a driver bound to an interface)
Definition device.h:75
Base class for all drivers.
Definition driver.h:109
Definition nav200.h:116
Definition nav200.h:105
Definition nav200.h:97
Definition nav200.h:65
Definition nav200.h:82
Definition nav200.h:89
A device address.
Definition player.h:146