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
169typedef 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;
175 player_actarray_data_t lift;
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
186 player_position2d_data_t 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
198class SIP;
199
200// Circular Buffer Used by PTZ camera
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
225class 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
Class for loading configuration file information.
Definition configfile.h:197
Definition kinecalc.h:45
Definition p2os/packet.h:41
Definition p2os.h:226
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition p2os.cc:698
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition p2os.cc:1388
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition p2os.cc:2218
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition p2os.cc:1327
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition p2os.cc:1412
virtual void Main()
Main method for driver thread.
Definition p2os.cc:1581
An autopointer for the message queue.
Definition message.h:74
Definition p2os/sip.h:47
Base class for drivers which oeprate with a thread.
Definition driver.h:553
Definition p2os.h:201
Messages between wsn and a robot.
Definition er.h:87
A rectangular bounding box, used to define the size of an object.
Definition player.h:255
A device address.
Definition player.h:146
Generic message header.
Definition player.h:162
An angle in 3D space.
Definition player.h:207
Definition p2os.h:170
A point in 3D space.
Definition player.h:195
A pose in space.
Definition player.h:229