khepera.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000
4  * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
5  *
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program 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
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  */
22 
23 /* Copyright (C) 2004
24  * Toby Collett, University of Auckland Robotics Group
25  *
26  * Header for the khepera robot. The
27  * architecture is similar to the P2OS device, in that the position, IR and
28  * power services all need to go through a single serial port and
29  * base device class. So this code was copied from p2osdevice and
30  * modified to taste.
31  *
32  */
33 
34 #ifndef _KHEPERADEVICE_H
35 #define _KHEPERADEVICE_H
36 
37 #include <pthread.h>
38 #include <sys/time.h>
39 #include <errno.h>
40 
41 #include <libplayercore/playercore.h>
42 //#include <khepera_params.h>
43 #include "khepera_serial.h"
44 
45 #define KHEPERA_CONFIG_BUFFER_SIZE 1024
46 #define KHEPERA_BAUDRATE B38400
47 #define KHEPERA_DEFAULT_SERIAL_PORT "/dev/ttyUSB0"
48 #define KHEPERA_DEFAULT_SCALE 10
49 #define KHEPERA_DEFAULT_ENCODER_RES (1.0/12.0)
50 #define KHEPERA_DEFAULT_IR_CALIB_A (64.158)
51 #define KHEPERA_DEFAULT_IR_CALIB_B (-0.1238)
52 
53 #define KHEPERA_MOTOR_LEFT 0
54 #define KHEPERA_MOTOR_RIGHT 1
55 
56 #define KHEPERA_FIXED_FACTOR 10000
57 
58 #define CRLF "\r\n"
59 #define KHEPERA_COMMAND_PROMPT "\r\n"
60 
61 #ifndef ABS
62 #define ABS(x) ((x) < 0 ? -(x) : (x))
63 #endif
64 
65 #ifndef SGN
66 #define SGN(x) ((x) < 0 ? -1 : 1)
67 #endif
68 
69 typedef struct player_khepera_geom
70 {
71  char * PortName;
72  double scale;
74  double * ir_calib_a;
75  double * ir_calib_b;
76  player_position2d_geom_t position;
77  double encoder_res;
78 } __attribute__ ((packed)) player_khepera_geom_t;
79 
80 
81 class Khepera : public ThreadedDriver
82 {
83 public:
84 
85  Khepera(ConfigFile *cf, int section);
86  ~Khepera();
87 
88  /* the main thread */
89  virtual void Main();
90 
91  virtual int Subscribe(player_devaddr_t addr);
92  virtual int Unsubscribe(player_devaddr_t addr);
93 
94  virtual int MainSetup();
95  virtual void MainQuit();
96 
97  int ResetOdometry();
98 
99  // handle IR
100  void SetIRState(int);
101 
102  void UpdateData(void);
103 
104  void UpdateIRData(player_ir_data_t *);
105  void UpdatePosData(player_position2d_data_t *);
106 
107  // the following are all interface functions to the REB
108  // this handles the A/D device which deals with IR for us
109  //void ConfigAD(int, int);
110  unsigned short ReadAD(int);
111  int ReadAllIR(player_ir_data_t *);
112 
113  // this handles motor control
114  int SetSpeed(int, int);
115  int ReadSpeed(int*, int*);
116 
117  int SetPos(int, int);
118 
119  int SetPosCounter(int, int);
120  int ReadPos(int *, int*);
121 
122  //unsigned char ReadStatus(int, int *, int *);
123 
124  // MessageHandler
125  int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data);
126 
127 private:
128  player_devaddr_t ir_addr;
129  player_devaddr_t position_addr;
130  int position_subscriptions;
131  int ir_subscriptions;
132 
133  KheperaSerial * Serial;
134 
135  player_khepera_geom_t* geometry;
136 
137  int param_index; // index in the RobotParams table for this robot
138  int khepera_fd; // khepera device file descriptor
139 
140  struct timeval last_position; // last position update
141  bool refresh_last_position;
142  int last_lpos, last_rpos;
143  double x,y,yaw;
144  int last_x_f, last_y_f;
145  double last_theta;
146 
147  struct timeval last_pos_update; // time of last pos update
148  struct timeval last_ir_update;
149 
150  int pos_update_period;
151 
152  short desired_heading;
153 
154  int ir_sequence;
155  struct timeval last_ir;
156 
157  bool motors_enabled;
158  bool velocity_mode;
159  bool direct_velocity_control;
160 
161  // device used to communicate with reb
162  char khepera_serial_port[MAX_FILENAME_SIZE];
163 
164  //struct pollfd write_pfd, read_pfd;
165 
166 
167 };
168 
169 
170 #endif
position 2d velocity command
Definition: player_interfaces.h:617
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
#define PLAYER_LOG_REQ_SET_WRITE_STATE
Request/reply subtype: set write state.
Definition: player_interfaces.h:3233
uint32_t host
The "host" on which the device resides.
Definition: player.h:147
#define PLAYER_IR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:2108
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
Request/reply: Rewind playback.
Definition: player_interfaces.h:3286
virtual void Main()
Main method for driver thread.
Definition: khepera.cc:442
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: khepera.cc:426
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
#define PLAYER_WIFI_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2188
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:230
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
A pose in the plane.
Definition: player.h:216
#define PLAYER_LASER_DATA_SCANPOSE
Data subtype: pose-stamped scan.
Definition: player_interfaces.h:848
player_pose3d_t pose
Laser pose, in robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:948
uint32_t links_count
length of said list
Definition: player_interfaces.h:2250
Generic message header.
Definition: player.h:160
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:888
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
#define PLAYER_IR_REQ_POSE
Request/reply subtype: get pose.
Definition: player_interfaces.h:2102
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
double px
X [m].
Definition: player.h:219
Request/reply: get pose.
Definition: player_interfaces.h:2130
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
Definition: khepera.cc:352
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
Definition: khepera_serial.h:33
double pyaw
yaw [rad]
Definition: player.h:240
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
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
virtual void Main(void)=0
Main method for driver thread.
const char * ReadTupleString(int section, const char *name, int index, const char *value)
Read a string from a tuple field.
int ReadInt(int section, const char *name, int value)
Read an integer value.
Definition: khepera.h:69
#define PLAYER_LOG_REQ_GET_STATE
Request/reply subtype: get state.
Definition: player_interfaces.h:3239
void * GetPayload()
Get pointer to payload.
Definition: message.h:187
Definition: khepera.h:81
player_pose2d_t pose
The global pose of the laser at the time the scan was acquired.
Definition: player_interfaces.h:914
void ProcessMessages(void)
Process pending messages.
Available interfaces are stored in an array of these, defined in interface_util.c.
Definition: interface_util.h:70
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
double py
Y [m].
Definition: player.h:221
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:82
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: khepera.cc:402
uint8_t type
The type of log device, either PLAYER_LOG_TYPE_READ or PLAYER_LOG_TYPE_WRITE.
Definition: player_interfaces.h:3290
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
player_laser_data_t scan
The scan data.
Definition: player_interfaces.h:912
float * ranges
ranges [m]
Definition: player_interfaces.h:2124
#define PLAYER_LOG_TYPE_WRITE
Types of log device: write.
Definition: player_interfaces.h:3252
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: khepera.cc:162
uint8_t state
State: FALSE=disabled, TRUE=enabled.
Definition: player_interfaces.h:3262
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
Request/reply: Set write state.
Definition: player_interfaces.h:3259
#define PLAYER_LASER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:854
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:124
Request/reply: Get geometry.
Definition: player_interfaces.h:945
float * ranges
The range readings [m].
Definition: player_interfaces.h:776
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.
uint8_t state
Logging/playback state: FALSE=disabled, TRUE=enabled.
Definition: player_interfaces.h:3292
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
Class for loading configuration file information.
Definition: configfile.h:195
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
double py
Y [m].
Definition: player.h:232
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
position2d data
Definition: player_interfaces.h:606
player_wifi_link_t * links
A list of links.
Definition: player_interfaces.h:2252
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:81
position2d geom
Definition: player_interfaces.h:655
A pose in space.
Definition: player.h:227
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Definition: khepera.cc:375
#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
uint32_t ranges_count
The number of valid range readings.
Definition: player_interfaces.h:774
uint32_t voltages_count
number of samples
Definition: player_interfaces.h:2118
Messages between wsn and a robot.
Definition: er.h:86
uint32_t ranges_count
number of samples
Definition: player_interfaces.h:2122
Data: state (PLAYER_WIFI_DATA_STATE)
Definition: player_interfaces.h:2247
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:169
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
Data: ranges (PLAYER_IR_DATA_RANGES)
Definition: player_interfaces.h:2115
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
Reference-counted message objects.
Definition: message.h:131
#define PLAYER_POSITION2D_REQ_VELOCITY_MODE
Request/reply: Change velocity control.
Definition: player_interfaces.h:522
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
Data: pose-stamped scan (PLAYER_LASER_DATA_SCANPOSE)
Definition: player_interfaces.h:909
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
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_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
float * voltages
voltages [V]
Definition: player_interfaces.h:2120
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535