p2os.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 /*
24  * $Id$
25  *
26  * the P2OS device. it's the parent device for all the P2 'sub-devices',
27  * like gripper, position, sonar, etc. there's a thread here that
28  * actually interacts with P2OS via the serial line. the other
29  * "devices" communicate with this thread by putting into and getting
30  * data out of shared buffers.
31  */
32 #ifndef _P2OSDEVICE_H
33 #define _P2OSDEVICE_H
34 
35 #include <pthread.h>
36 #include <sys/time.h>
37 
38 #include <libplayercore/playercore.h>
39 #include <replace/replace.h>
40 
41 #include "packet.h"
42 #include "robot_params.h"
43 
44 
45 // Default max speeds
46 #define MOTOR_DEF_MAX_SPEED 0.5
47 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
48 
49 /*
50  * Apparently, newer kernel require a large value (200000) here. It only
51  * makes the initialization phase take a bit longer, and doesn't have any
52  * impact on the speed at which packets are received from P2OS
53  */
54 #define P2OS_CYCLETIME_USEC 200000
55 
56 /* p2os constants */
57 
58 #define P2OS_NOMINAL_VOLTAGE 12.0
59 
60 /* Command numbers */
61 #define SYNC0 0
62 #define SYNC1 1
63 #define SYNC2 2
64 
65 #define PULSE 0
66 #define OPEN 1
67 #define CLOSE 2
68 #define ENABLE 4
69 #define SETA 5
70 #define SETV 6
71 #define SETO 7
72 #define VEL 11
73 #define RVEL 21
74 #define SETRA 23
75 #define SONAR 28
76 #define STOP 29
77 #define VEL2 32
78 #define GRIPPER 33
79 #define GRIPPERVAL 36
80 #define TTY2 42 // Added in AmigOS 1.2
81 #define GETAUX 43 // Added in AmigOS 1.2
82 #define BUMP_STALL 44
83 #define JOYDRIVE 47
84 #define GYRO 58 // Added in AROS 1.8
85 #define ROTKP 82 // Added in P2OS1.M
86 #define ROTKV 83 // Added in P2OS1.M
87 #define ROTKI 84 // Added in P2OS1.M
88 #define TRANSKP 85 // Added in P2OS1.M
89 #define TRANSKV 86 // Added in P2OS1.M
90 #define TRANSKI 87 // Added in P2OS1.M
91 #define TTY3 66 // Added in AmigOS 1.3
92 #define GETAUX2 67 // Added in AmigOS 1.3
93 #define ARM_INFO 70
94 #define ARM_STATUS 71
95 #define ARM_INIT 72
96 #define ARM_CHECK 73
97 #define ARM_POWER 74
98 #define ARM_HOME 75
99 #define ARM_PARK 76
100 #define ARM_POS 77
101 #define ARM_SPEED 78
102 #define ARM_STOP 79
103 #define ARM_AUTOPARK 80
104 #define ARM_GRIPPARK 81
105 #define SOUND 90
106 #define PLAYLIST 91
107 
108 /* Server Information Packet (SIP) types */
109 #define STATUSSTOPPED 0x32
110 #define STATUSMOVING 0x33
111 #define ENCODER 0x90
112 #define SERAUX 0xB0
113 #define SERAUX2 0xB8 // Added in AmigOS 1.3
114 #define GYROPAC 0x98 // Added AROS 1.8
115 #define ARMPAC 160 // ARMpac
116 #define ARMINFOPAC 161 // ARMINFOpac
117 //#define PLAYLIST 0xD0
118 
119 /* Argument types */
120 #define ARGINT 0x3B // Positive int (LSB, MSB)
121 #define ARGNINT 0x1B // Negative int (LSB, MSB)
122 #define ARGSTR 0x2B // String (Note: 1st byte is length!!)
123 
124 /* gripper stuff */
125 #define GRIPopen 1
126 #define GRIPclose 2
127 #define GRIPstop 3
128 #define LIFTup 4
129 #define LIFTdown 5
130 #define LIFTstop 6
131 #define GRIPstore 7
132 #define GRIPdeploy 8
133 #define GRIPhalt 15
134 #define GRIPpress 16
135 #define LIFTcarry 17
136 
137 /* CMUcam stuff */
138 #define CMUCAM_IMAGE_WIDTH 80
139 #define CMUCAM_IMAGE_HEIGHT 143
140 #define CMUCAM_MESSAGE_LEN 10
141 
142 /* conection stuff */
143 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
144 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
145 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
146 
147 /* Canon PTZ (VC-C4) Stuff */
148 #define CAM_ERROR_NONE 0x30
149 #define CAM_ERROR_BUSY 0x31
150 #define CAM_ERROR_PARAM 0x35
151 #define CAM_ERROR_MODE 0x39
152 
153 #define PTZ_SLEEP_TIME_USEC 100000
154 
155 #define MAX_PTZ_COMMAND_LENGTH 19
156 #define MAX_PTZ_REQUEST_LENGTH 17
157 #define COMMAND_RESPONSE_BYTES 6
158 
159 #define PTZ_PAN_MAX 98.0 // 875 units 0x36B
160 #define PTZ_TILT_MAX 88.0 // 790 units 0x316
161 #define PTZ_TILT_MIN -30.0 // -267 units 0x10B
162 #define MAX_ZOOM 1960 //1900
163 #define ZOOM_CONV_FACTOR 17
164 
165 #define PT_BUFFER_INC 512
166 #define PT_READ_TIMEOUT 10000
167 #define PT_READ_TRIALS 2
168 
169 typedef struct player_p2os_data
170 {
171  //Standard SIP
172  player_position2d_data_t position;
173  player_sonar_data_t sonar;
174  player_gripper_data_t gripper;
176  player_power_data_t power;
177  player_bumper_data_t bumper;
178  player_position2d_data_t compass;
179  player_dio_data_t dio;
180  player_aio_data_t aio;
181 
182  //Blobfinder
183  player_blobfinder_data_t blobfinder;
184 
185  //Gyro
187 
188  //ARMPAC
189  player_actarray_data_t actArray;
190  player_gripper_data_t armGripper;
191 } __attribute__ ((packed)) player_p2os_data_t;
192 
193 // this is here because we need the above typedef's before including it.
194 #include "sip.h"
195 
196 #include "kinecalc.h"
197 
198 class SIP;
199 
200 // Circular Buffer Used by PTZ camera
201 class circbuf{
202  public:
203  circbuf(int size=512);
204 
205  void putOnBuf(unsigned char c);
206  int getFromBuf();
207  bool haveData();
208  int size();
209  void printBuf();
210 
211  bool gotPacket();
212  void reset();
213 
214  private:
215  unsigned char* buf;
216  int start;
217  int end;
218  int mysize;
219  bool gotPack;
220 };
221 
222 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
223 //class KineCalc;
224 
225 class P2OS : public ThreadedDriver
226 {
227  private:
228  player_p2os_data_t p2os_data;
229 
230  player_devaddr_t position_id;
231  player_devaddr_t sonar_id;
232  player_devaddr_t aio_id;
233  player_devaddr_t dio_id;
234  player_devaddr_t gripper_id;
235  player_devaddr_t lift_id;
236  player_devaddr_t bumper_id;
237  player_devaddr_t power_id;
238  player_devaddr_t compass_id;
239  player_devaddr_t gyro_id;
240  player_devaddr_t blobfinder_id;
241  player_devaddr_t audio_id;
242  player_devaddr_t actarray_id;
243  player_devaddr_t limb_id;
244  player_devaddr_t ptz_id;
245  player_devaddr_t armgripper_id;
246 
247  // Book keeping to only send new commands
248  bool sentGripperCmd;
249  uint8_t lastGripperCmd;
250  uint8_t lastLiftCmd;
251  player_actarray_position_cmd_t lastLiftPosCmd;
252  bool sentArmGripperCmd;
253  uint8_t lastArmGripperCmd;
254  uint8_t lastActArrayCmd;
255  player_actarray_position_cmd_t lastActArrayPosCmd;
256  player_actarray_home_cmd_t lastActArrayHomeCmd;
257 
258  // bookkeeping to only send new audio I/O commands
259  bool sent_audio_cmd;
260  player_audio_sample_item_t last_audio_cmd;
261  // PID settings
262  int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
263 
264 
265  int position_subscriptions;
266  int sonar_subscriptions;
267  int actarray_subscriptions;
268  int ptz_subscriptions;
269 
270  SIP* sippacket;
271 
272  int SendReceive(P2OSPacket* pkt, bool publish_data=true);
273  void ResetRawPositions();
274  /* toggle sonars on/off, according to val */
275  void ToggleSonarPower(unsigned char val);
276  /* toggle motors on/off, according to val */
277  void ToggleMotorPower(unsigned char val);
278  int HandleConfig(QueuePointer & resp_queue,
279  player_msghdr * hdr,
280  void* data);
281  int HandleCommand(player_msghdr * hdr, void * data);
282  void StandardSIPPutData(double timestampStandardSIP);
283  void GyroPutData(double timestampGyro);
284  void BlobfinderPutData(double timestampSERAUX);
285  void ActarrayPutData(double timestampArm);
286  void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
287  int HandleGripperCommand (player_msghdr *hdr, void *data);
288  int HandleLiftCommand (player_msghdr *hdr, void *data);
289  int HandleArmGripperCommand (player_msghdr *hdr, void *data);
290  void HandleAudioCommand(player_audio_sample_item_t audio_cmd);
291 
293  void get_ptz_packet(int s1, int s2=0);
294  int SetupPtz();
295 
297  // Gripper stuff
298  player_pose3d_t gripperPose;
299  player_bbox3d_t gripperOuterSize;
300  player_bbox3d_t gripperInnerSize;
301  player_bbox3d_t armGripperOuterSize;
302  player_bbox3d_t armGripperInnerSize;
303  void OpenGripper (void);
304  void CloseGripper (void);
305  void StopGripper (void);
306  void OpenArmGripper (void);
307  void CloseArmGripper (void);
308  void StopArmGripper (void);
309 
311  // Actarray stuff
312  double aaLengths[6];
313  double aaOrients[18];
314  double aaAxes[18];
315  player_point_3d_t aaBasePos;
316  player_orientation_3d_t aaBaseOrient;
317  inline double TicksToDegrees (int joint, unsigned char ticks);
318  inline unsigned char DegreesToTicks (int joint, double degrees);
319  inline double TicksToRadians (int joint, unsigned char ticks);
320  inline unsigned char RadiansToTicks (int joint, double rads);
321  inline double RadsPerSectoSecsPerTick (int joint, double speed);
322  inline double SecsPerTicktoRadsPerSec (int joint, double secs);
323  void ToggleActArrayPower (unsigned char val, bool lock = true); // Toggle actarray power on/off
324  void SetActArrayJointSpeed (int joint, double speed); // Set a joint speed
325  void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
326  void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
327  int HandleActArrayCommand (player_msghdr * hdr, void * data);
328 
330  // Limb stuff
331  KineCalc *kineCalc;
332  float armOffsetX, armOffsetY, armOffsetZ;
333  // This is here because we don't want it zeroed every time someone fills in some other data
334  player_limb_data_t limb_data;
335  void HandleLimbHomeCmd (void);
336  void HandleLimbStopCmd (void);
337  void HandleLimbSetPoseCmd (player_limb_setpose_cmd_t cmd);
338  void HandleLimbSetPositionCmd (player_limb_setposition_cmd_t cmd);
339  void HandleLimbVecMoveCmd (player_limb_vecmove_cmd_t cmd);
340  int HandleLimbCommand (player_msghdr * hdr, void * data);
341 
342  int param_idx; // index in the RobotParams table for this robot
343  int direct_wheel_vel_control; // false -> separate trans and rot vel
344  int psos_fd; // p2os device file descriptor
345  const char* psos_serial_port; // name of serial port device
346  bool psos_use_tcp; // use TCP port instead of serial port
347  const char* psos_tcp_host; // hostname to use if using TCP
348  int psos_tcp_port; // remote port to use if using TCP
349 
350  struct timeval lastblob_tv;
351 
352  // Max motor speeds (mm/sec,deg/sec)
353  int motor_max_speed;
354  int motor_max_turnspeed;
355 
356  // Bound the command velocities
357  bool use_vel_band;
358 
359  // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
360  short motor_max_trans_accel, motor_max_trans_decel;
361  short motor_max_rot_accel, motor_max_rot_decel;
362 
363  int radio_modemp; // are we using a radio modem?
364  int joystickp; // are we using a joystick?
365  int bumpstall; // should we change the bumper-stall behavior?
366  bool ignore_checksum;
367 
368  // PTZ Camera Stuff
369  player_ptz_data_t ptz_data;
370  int maxfov, minfov;
371  int maxzoom;
372  int pandemand, tiltdemand, zoomdemand;
373  int SendCommand(unsigned char *str, int len);
374  int SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1);
375  void PrintPacket(char* str, unsigned char* cmd, int len);
376  int SendAbsPanTilt(int pan, int tilt);
377  int setDefaultTiltRange();
378  int GetAbsPanTilt(int* pan, int* tilt);
379  int GetAbsZoom(int* zoom);
380  int SendAbsZoom(int zoom);
381  int read_ptz(unsigned char *reply, int size);
382  int ReceiveCommandAnswer(int asize);
383  int ReceiveRequestAnswer(unsigned char *data, int s1, int s2);
384  int setControlMode();
385  int setNotifyCommand();
386  int setPower(int on);
387  int setOnScreenOff();
388  int CheckHostControlMode();
389  int sendInit();
390  int GetMaxZoom(int * maxzoom);
391  circbuf cb;
392 
393  float pulse; // Pulse time
394  double lastPulseTime; // Last time of sending a pulse or command to the robot
395  void SendPulse (void);
396 
397  public:
398 
399  P2OS(ConfigFile* cf, int section);
400  ~P2OS (void);
401 
402  virtual int Subscribe(player_devaddr_t id);
403  virtual int Unsubscribe(player_devaddr_t id);
404 
405  /* the main thread */
406  virtual void Main();
407 
408  virtual int MainSetup();
409  virtual void MainQuit();
410 
411  // MessageHandler
412  virtual int ProcessMessage(QueuePointer & resp_queue,
413  player_msghdr * hdr,
414  void * data);
415 
416  void CMUcamReset(bool doLock = true);
417  void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
418  int gmax=0, int bmin=0, int bmax=0);
419  void CMUcamStartTracking(bool doLock = true);
420  void CMUcamStopTracking(bool doLock = true);
421 };
422 
423 
424 #endif
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:113
float position
The position to move to.
Definition: player_interfaces.h:3872
Command: Multiple Joint position control (PLAYER_ACTARRAY_CMD_MULTI_POS)
Definition: player_interfaces.h:3878
#define PLAYER_LIMB_REQ_BRAKES
Request/reply: brakes.
Definition: player_interfaces.h:4054
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
#define PLAYER_BUMPER_REQ_GET_GEOM
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1916
player_bbox3d_t outer_size
Outside dimensions of gripper (m, m, m).
Definition: player_interfaces.h:438
position 2d velocity command
Definition: player_interfaces.h:617
#define PLAYER_ACTARRAY_DATA_STATE
Idle state code.
Definition: player_interfaces.h:3769
Data: detected blobs (PLAYER_BLOBFINDER_DATA_BLOBS)
Definition: player_interfaces.h:1096
uint32_t color
A descriptive color for the blob (useful for gui's).
Definition: player_interfaces.h:1074
#define PLAYER_MSG1(level, msg, a)
Error message macros.
Definition: error.h:106
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
float centre
The range of motion of the actuator, in m or rad depending on the type.
Definition: player_interfaces.h:3834
double py
Y [m].
Definition: player.h:199
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
float pan
Pan [rad].
Definition: player_interfaces.h:1231
#define PLAYER_LIMB_REQ_SPEED
Request/reply: speed.
Definition: player_interfaces.h:4060
#define PLAYER_LIMB_STATE_OOR
Target was out of reach.
Definition: player_interfaces.h:4024
player_actarray_actuatorgeom_t * actuators
The geometry information for each actuator in the array.
Definition: player_interfaces.h:3854
player_point_3d_t basePos
The base position of the end-effector in robot coordinates.
Definition: player_interfaces.h:4145
Data: state (PLAYER_LIMB_DATA_STATE)
Definition: player_interfaces.h:4067
player_point_3d_t position
Position of the end effector.
Definition: player_interfaces.h:4103
A rectangular bounding box, used to define the size of an object.
Definition: player.h:254
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
uint32_t right
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1084
virtual void Main()
Main method for driver thread.
Definition: p2os.cc:1581
#define PLAYER_ACTARRAY_CMD_SPEED
Idle state code.
Definition: player_interfaces.h:3754
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Definition: p2os.h:225
#define PLAYER_PTZ_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:1208
#define PLAYER_ACTARRAY_CMD_MULTI_POS
Idle state code.
Definition: player_interfaces.h:3751
player_bumper_define_t * bumper_def
geometry of each bumper
Definition: player_interfaces.h:1953
Player audio sample selection.
Definition: player_interfaces.h:1602
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: p2os.cc:2218
double px
X [m].
Definition: player.h:231
float max
The range of motion of the actuator, in m or rad depending on the type.
Definition: player_interfaces.h:3836
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
float tilt
Tilt [rad].
Definition: player_interfaces.h:1233
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
uint32_t bmax
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1135
float home
The range of motion of the actuator, in m or rad depending on the type.
Definition: player_interfaces.h:3838
int32_t index
index of the sample
Definition: player_interfaces.h:1605
Definition: camera.h:110
An angle in 3D space.
Definition: player.h:206
position2d velocity mode config
Definition: player_interfaces.h:671
uint8_t num_beams
Number of breakbeams the gripper has.
Definition: player_interfaces.h:443
Request/reply: get geometry.
Definition: player_interfaces.h:433
uint32_t bmin
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1133
uint32_t left
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1082
Data AND Request/reply: bumper geometry.
Definition: player_interfaces.h:1948
double ppitch
pitch [rad]
Definition: player.h:211
Generic message header.
Definition: player.h:161
uint32_t positions_count
The number of actuators in the array.
Definition: player_interfaces.h:3881
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
Definition: cmvision.h:105
uint32_t compression
Image compression; PLAYER_CAMERA_COMPRESS_RAW indicates no compression.
Definition: player_interfaces.h:2966
Request/reply: get geometry.
Definition: player_interfaces.h:3849
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
uint32_t width
Image dimensions [pixels].
Definition: player_interfaces.h:2953
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1228
#define PLAYER_CAMERA_FORMAT_RGB888
Image format : 24-bit color (8 bits R, 8 bits G, 8 bits B).
Definition: player_interfaces.h:2942
uint8_t state
The state of the limb.
Definition: player_interfaces.h:4078
#define PLAYER_LIMB_STATE_COLL
Target was blocked by collision.
Definition: player_interfaces.h:4026
uint8_t capacity
Capacity for storing objects - if 0, then the gripper can't store.
Definition: player_interfaces.h:445
#define PLAYER_LIMB_STATE_IDLE
Idle.
Definition: player_interfaces.h:4018
int32_t joint
The joint/actuator to command.
Definition: player_interfaces.h:3870
player_bbox3d_t inner_size
Inside dimensions of gripper, i.e.
Definition: player_interfaces.h:441
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Definition: p2os.h:169
uint32_t bottom
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1088
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
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:220
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
player_point_3d_t base_pos
The position of the base of the actarray.
Definition: player_interfaces.h:3857
Definition: cmvision.h:143
#define PLAYER_BLOBFINDER_DATA_BLOBS
Structure describing a single blob.
Definition: player_interfaces.h:1055
float zoom
Field of view [rad].
Definition: player_interfaces.h:1235
Definition: cmvision.h:122
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:241
A point in 3D space.
Definition: player.h:194
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
Command: Set end effector pose (PLAYER_LIMB_CMD_SETPOSE)
Definition: player_interfaces.h:4086
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
#define PLAYER_LIMB_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:4029
virtual void Main(void)=0
Main method for driver thread.
Request/reply: Power.
Definition: player_interfaces.h:3948
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
Command: Joint position control (PLAYER_ACTARRAY_CMD_POS)
Definition: player_interfaces.h:3867
#define PLAYER_GRIPPER_CMD_STOP
Command: Stop (PLAYER_GRIPPER_CMD_STOP)
Definition: player_interfaces.h:383
uint32_t image_count
Size of image data as stored in image buffer (bytes)
Definition: player_interfaces.h:2968
Structure containing a single actuator's information.
Definition: player_interfaces.h:3788
int ReadInt(int section, const char *name, int value)
Read an integer value.
#define PLAYER_ACTARRAY_REQ_POWER
Idle state code.
Definition: player_interfaces.h:3733
float * positions
The positions for each joint/actuator.
Definition: player_interfaces.h:3883
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
uint32_t y
The blob centroid [pixels].
Definition: player_interfaces.h:1080
#define PLAYER_PTZ_REQ_GENERIC
Request/reply subtype: generic.
Definition: player_interfaces.h:1196
void ProcessMessages(void)
Process pending messages.
Configuration request: Set imager params.
Definition: player_interfaces.h:1153
#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
double py
Y [m].
Definition: player.h:222
float radius
radius of curvature [m] - zero for straight lines
Definition: player_interfaces.h:1938
Request/reply: Speed.
Definition: player_interfaces.h:4152
float range
Range to the blob center [meters].
Definition: player_interfaces.h:1090
Definition: kinecalc.h:36
uint32_t bumper_def_count
The number of valid bumper definitions.
Definition: player_interfaces.h:1951
#define PLAYER_ACTARRAY_REQ_SPEED
Idle state code.
Definition: player_interfaces.h:3742
uint32_t area
The blob area [pixels].
Definition: player_interfaces.h:1076
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
uint8_t type
The type of the actuator - linear or rotary.
Definition: player_interfaces.h:3819
double px
X [m].
Definition: player.h:197
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
#define PLAYER_ACTARRAY_TYPE_ROTARY
Rotary type code.
Definition: player_interfaces.h:3784
#define PLAYER_ACTARRAY_CMD_HOME
Idle state code.
Definition: player_interfaces.h:3760
player_point_3d_t axis
The axis of rotation for this actuator if it is rotary, or axis along which it moves if it is linear.
Definition: player_interfaces.h:3830
#define PLAYER_CAMERA_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2922
uint32_t rmin
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1125
uint8_t * image
Compressed image data (byte-aligned, row major order).
Definition: player_interfaces.h:2972
Definition: p2os/robot_params.h:75
Data: state (PLAYER_GRIPPER_DATA_STATE)
Definition: player_interfaces.h:418
#define PLAYER_GRIPPER_CMD_OPEN
Command: Open (PLAYER_GRIPPER_CMD_OPEN)
Definition: player_interfaces.h:373
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
player_pose3d_t pose
Gripper pose, in robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:436
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:201
bool ReadBool(int section, const char *name, bool value)
Read a boolean value (one of: yes, no, true, false, 1, 0)
player_orientation_3d_t base_orientation
The orientation of the base of the actarray.
Definition: player_interfaces.h:3860
uint32_t value
driver-specific
Definition: player_interfaces.h:674
uint32_t * config
Buffer for command/reply.
Definition: player_interfaces.h:1292
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1922
uint32_t rmax
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1127
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_SONAR_REQ_POWER
Request/reply subtype: power.
Definition: player_interfaces.h:758
uint32_t gmax
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1131
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
#define PLAYER_LIMB_CMD_HOME
Command: home (PLAYER_LIMB_CMD_HOME)
Definition: player_interfaces.h:4034
#define PLAYER_LIMB_CMD_STOP
Command: stop (PLAYER_LIMB_CMD_STOP)
Definition: player_interfaces.h:4039
player_point_3d_t approach
The approach vector of the end effector.
Definition: player_interfaces.h:4072
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
Command: Vector move the end effector (PLAYER_LIMB_CMD_VECMOVE)
Definition: player_interfaces.h:4110
uint32_t actuators_count
The number of actuators in the array.
Definition: player_interfaces.h:3852
Definition: kinecalc.h:29
double sl
Length [m].
Definition: player.h:259
int32_t joint
The joint/actuator to command - set to -1 to command all.
Definition: player_interfaces.h:3914
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
#define PLAYER_LIMB_CMD_SETPOSE
Command subtype: set pose.
Definition: player_interfaces.h:4042
double proll
roll [rad]
Definition: player.h:209
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Data: state (PLAYER_CAMERA_DATA_STATE)
Definition: player_interfaces.h:2950
player_pose2d_t pose
(x, y, yaw) [m, m, rad]
Definition: player_interfaces.h:689
uint32_t top
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1086
Definition: kinecalc.h:44
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1994
#define PLAYER_ACTARRAY_TYPE_LINEAR
Linear type code.
Definition: player_interfaces.h:3782
Definition: p2os/robot_params.h:83
Definition: camera.h:128
#define PLAYER_BUMPER_DATA_STATE
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1910
#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:196
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
double sw
Width [m].
Definition: player.h:257
double pz
Z [m].
Definition: player.h:201
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:145
#define PLAYER_POSITION2D_REQ_RESET_ODOM
Request/reply: Reset odometry.
Definition: player_interfaces.h:541
An autopointer for the message queue.
Definition: message.h:73
double py
Y [m].
Definition: player.h:233
uint32_t format
Image format (must be compatible with depth).
Definition: player_interfaces.h:2959
#define PLAYER_ACTARRAY_CMD_POS
Idle state code.
Definition: player_interfaces.h:3748
float length
Distance to move.
Definition: player_interfaces.h:4115
#define PLAYER_BLOBFINDER_REQ_SET_COLOR
Structure describing a single blob.
Definition: player_interfaces.h:1058
Request/reply: Generic request.
Definition: player_interfaces.h:1287
position2d data
Definition: player_interfaces.h:606
#define PLAYER_AUDIO_CMD_SAMPLE_PLAY
Command subtype: sample_play_cmd, play a pre stored audio sample.
Definition: player_interfaces.h:1374
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:82
float config_speed
The configured speed setting of the actuator - different from current speed.
Definition: player_interfaces.h:3840
Definition: p2os.h:201
position2d geom
Definition: player_interfaces.h:655
bool Empty()
Check whether a queue is empty.
Definition: message.h:345
player_point_3d_t orientation
The orientation vector of the end effector (a vector in a predefined direction on the end effector,...
Definition: player_interfaces.h:4076
A pose in space.
Definition: player.h:228
player_point_3d_t direction
Direction vector to move in.
Definition: player_interfaces.h:4113
player_point_3d_t position
The position of the end effector (in robot coordiantes).
Definition: player_interfaces.h:4070
player_orientation_3d_t orientation
The orientation of this actuator when it is in its rest position.
Definition: player_interfaces.h:3827
uint32_t config_count
Length of data in config buffer.
Definition: player_interfaces.h:1290
player_point_3d_t orientation
Orientation vector.
Definition: player_interfaces.h:4093
Command: Set end effector position (PLAYER_LIMB_CMD_SETPOSITION)
Definition: player_interfaces.h:4100
uint8_t state
Turn power off TRUE or FALSE.
Definition: player_interfaces.h:800
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: p2os.cc:698
#define PLAYER_LIMB_STATE_MOVING
Moving to target.
Definition: player_interfaces.h:4022
#define PLAYER_BLOBFINDER_REQ_GET_COLOR
Structure describing a single blob.
Definition: player_interfaces.h:1064
player_devaddr_t device_addr
Default device address (single-interface drivers)
Definition: driver.h:269
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
Messages between wsn and a robot.
Definition: er.h:86
#define PLAYER_GRIPPER_CMD_CLOSE
Command: Close (PLAYER_GRIPPER_CMD_CLOSE)
Definition: player_interfaces.h:378
#define PLAYER_ACTARRAY_REQ_GET_GEOM
Idle state code.
Definition: player_interfaces.h:3739
Definition: cmvision.h:120
Definition: p2os/sip.h:46
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
float min
The range of motion of the actuator, in m or rad depending on the type.
Definition: player_interfaces.h:3832
player_pose3d_t * poses
Pose of each sonar, in robot cs.
Definition: player_interfaces.h:790
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: p2os.cc:1327
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition: p2os.cc:1388
Request/reply: Sonar power.
Definition: player_interfaces.h:797
#define PLAYER_LIMB_REQ_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:4057
#define PLAYER_AIO_DATA_STATE
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2046
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Request/reply: Speed.
Definition: player_interfaces.h:3968
Actuator geometry.
Definition: player_interfaces.h:3816
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
player_point_3d_t position
Position of the end effector.
Definition: player_interfaces.h:4089
float length
length of the sensor [m]
Definition: player_interfaces.h:1936
#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:99
double pyaw
yaw [rad]
Definition: player.h:213
#define PLAYER_BLOBFINDER_REQ_SET_IMAGER_PARAMS
Structure describing a single blob.
Definition: player_interfaces.h:1061
Command: Joint home (PLAYER_ACTARRAY_CMD_HOME)
Definition: player_interfaces.h:3911
Request/reply: Set tracking color.
Definition: player_interfaces.h:1118
#define PLAYER_DIO_DATA_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1984
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition: p2os.cc:1412
double pa
yaw [rad]
Definition: player.h:224
Definition: p2os/packet.h:40
Base class for all drivers.
Definition: driver.h:108
Structure describing a single blob.
Definition: player_interfaces.h:1068
Definition: cmvision.h:89
uint32_t channel
For devices that can track multiple colors, indicate which color channel we are defining with this st...
Definition: player_interfaces.h:1123
#define PLAYER_GRIPPER_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:365
#define PLAYER_LIMB_REQ_POWER
Request/reply: power.
Definition: player_interfaces.h:4051
uint32_t gmin
RGB minimum and max values (0-255)
Definition: player_interfaces.h:1129
uint32_t x
The blob centroid [pixels].
Definition: player_interfaces.h:1078
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
uint32_t height
Image dimensions [pixels].
Definition: player_interfaces.h:2955
#define PLAYER_GRIPPER_REQ_GET_GEOM
Request subtype: get geometry.
Definition: player_interfaces.h:368
set odometry
Definition: player_interfaces.h:686
float length
The length of this actuator's link to the next actuator.
Definition: player_interfaces.h:3822
uint8_t hasbrakes
If the actuator has brakes or not.
Definition: player_interfaces.h:3842
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
void ParseSERAUX(unsigned char *buffer)
Parse a SERAUX SIP packet.
Definition: p2os/sip.cc:488
Data: state (PLAYER_ACTARRAY_DATA_STATE)
Definition: player_interfaces.h:3805
player_point_3d_t approach
Approach vector.
Definition: player_interfaces.h:4091
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
Request/reply: get geometry.
Definition: player_interfaces.h:4142
The geometry of a single bumper.
Definition: player_interfaces.h:1931
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
player_pose3d_t pose
the local pose of a single bumper
Definition: player_interfaces.h:1934
#define PLAYER_CAMERA_COMPRESS_JPEG
Compression method: jpeg.
Definition: player_interfaces.h:2947
uint32_t bpp
Image bits-per-pixel (8, 16, 24, 32).
Definition: player_interfaces.h:2957