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:109
Definition: player_interfaces.h:3039
Definition: nav200.h:104
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
position 2d velocity command
Definition: player_interfaces.h:617
uint32_t data_count
The number of cells.
Definition: player_interfaces.h:3071
Request/reply: Get/set scan properties.
Definition: player_interfaces.h:961
uint32_t data_count
Size of data as stored in buffer (bytes)
Definition: player_interfaces.h:3452
Definition: nav200.h:81
Definition: mixed/mricp/include/map.h:34
uint32_t host
The "host" on which the device resides.
Definition: player.h:147
#define PLAYER_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Definition: nav200.h:88
Definition: nav200.h:96
int8_t * data
Cell occupancy value.
Definition: player_interfaces.h:3075
Generic message header.
Definition: player.h:160
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
#define PLAYER_POSITION2D_REQ_MOTOR_POWER
Request/reply: Motor power.
Definition: player_interfaces.h:496
position2d power config
Definition: player_interfaces.h:664
Definition: mixed/mricp/include/map.h:39
double px
X [m].
Definition: player.h:219
Request/reply: get grid map tile.
Definition: player_interfaces.h:3060
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
uint32_t robot
The "robot" or device collection in which the device resides.
Definition: player.h:150
struct player_position2d_geom player_position2d_geom_t
position2d geom
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:187
#define PLAYER_OPAQUE_DATA_STATE
Data subtype: generic state.
Definition: player_interfaces.h:3434
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:284
double py
Y [m].
Definition: player.h:221
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:82
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
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:90
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
Definition: geometry2D.h:57
uint32_t row
The tile origin [pixels].
Definition: player_interfaces.h:3065
uint32_t width
The size of the tile [pixels].
Definition: player_interfaces.h:3067
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
uint32_t height
The size of the tile [pixels].
Definition: player_interfaces.h:3069
#define PLAYER_LASER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:854
Request/reply: Get geometry.
Definition: player_interfaces.h:945
int8_t data_range
Maximum value for each cell (-range <= EMPTY < 0, unknown = 0, 0 < OCCUPIED <= range)
Definition: player_interfaces.h:3073
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:195
int ReadTupleInt(int section, const char *name, int index, int value)
Read an integer from a tuple field.
#define PLAYER_MAP_REQ_GET_DATA
Request/reply subtype: get grid map tile
Definition: player_interfaces.h:3024
uint32_t col
The tile origin [pixels].
Definition: player_interfaces.h:3063
A device address.
Definition: player.h:144
#define PLAYER_POSITION2D_REQ_RESET_ODOM
Request/reply: Reset odometry.
Definition: player_interfaces.h:541
An autopointer for the message queue.
Definition: message.h:72
void SetFilter(int host, int robot, int interf, int index, int type, int subtype)
Set filter values.
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
Definition: icp.h:66
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
Definition: Timer.h:32
Reference-counted message objects.
Definition: message.h:131
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
#define PLAYER_OPAQUE_CMD_DATA
Cmd subtype: generic command.
Definition: player_interfaces.h:3437
data
Definition: player_interfaces.h:3449
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
Definition: nav200.h:115
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
Definition: nav200.h:64
set odometry
Definition: player_interfaces.h:686
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:185
uint16_t index
Which device of that interface.
Definition: player.h:154
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:152
#define PLAYER_MAP_REQ_GET_INFO
Request/reply subtype: get grid map metadata
Definition: player_interfaces.h:3021
void ProcessMessages(int maxmsgs)
Process pending messages.
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:122
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
uint8_t * data
The data we will be sending.
Definition: player_interfaces.h:3454
Definition: geometry2D.h:50