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 
64 typedef 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 
81 typedef struct PositionXY
82 {//position is in mm
83  int x;
84  int y;
85 }PositionXY;
86 
87 
88 typedef struct ReflectorData
89 {
90  uint8_t layer;
91  uint8_t number; // reflector number
92  PositionXY pos;
94 
95 
96 typedef 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 
104 typedef 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
110 }ErrorBytes;
111 
112 
113 
114 
115 class Nav200
116 {
117 public:
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 
170 protected:
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
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:113
Definition: nav200.h:105
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:90
Definition: nav200.h:82
Definition: mixed/mricp/include/map.h:36
uint32_t host
The "host" on which the device resides.
Definition: player.h:148
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Definition: nav200.h:89
Definition: nav200.h:97
Generic message header.
Definition: player.h:162
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
Definition: mixed/mricp/include/map.h:41
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
uint32_t robot
The "robot" or device collection in which the device resides.
Definition: player.h:151
int ReadInt(int section, const char *name, int value)
Read an integer value.
void ClearFilter(void)
Clear (i.e., turn off) message filter.
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:285
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:83
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:91
Definition: geometry2D.h:58
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
Class for loading configuration file information.
Definition: configfile.h:197
int ReadTupleInt(int section, const char *name, int index, int value)
Read an integer from a tuple field.
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
void SetFilter(int host, int robot, int interf, int index, int type, int subtype)
Set filter values.
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
Definition: icp.h:66
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Definition: Timer.h:34
Reference-counted message objects.
Definition: message.h:133
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Base class for all drivers.
Definition: driver.h:109
Definition: nav200.h:116
Definition: nav200.h:65
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:186
uint16_t index
Which device of that interface.
Definition: player.h:155
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
void ProcessMessages(int maxmsgs)
Process pending messages.
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:126
Definition: geometry2D.h:51