38 #include <libplayercore/playercore.h>
39 #include <replace/replace.h>
42 #include "robot_params.h"
46 #define MOTOR_DEF_MAX_SPEED 0.5
47 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
54 #define P2OS_CYCLETIME_USEC 200000
58 #define P2OS_NOMINAL_VOLTAGE 12.0
103 #define ARM_AUTOPARK 80
104 #define ARM_GRIPPARK 81
109 #define STATUSSTOPPED 0x32
110 #define STATUSMOVING 0x33
116 #define ARMINFOPAC 161
138 #define CMUCAM_IMAGE_WIDTH 80
139 #define CMUCAM_IMAGE_HEIGHT 143
140 #define CMUCAM_MESSAGE_LEN 10
143 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
144 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
145 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
148 #define CAM_ERROR_NONE 0x30
149 #define CAM_ERROR_BUSY 0x31
150 #define CAM_ERROR_PARAM 0x35
151 #define CAM_ERROR_MODE 0x39
153 #define PTZ_SLEEP_TIME_USEC 100000
155 #define MAX_PTZ_COMMAND_LENGTH 19
156 #define MAX_PTZ_REQUEST_LENGTH 17
157 #define COMMAND_RESPONSE_BYTES 6
159 #define PTZ_PAN_MAX 98.0
160 #define PTZ_TILT_MAX 88.0
161 #define PTZ_TILT_MIN -30.0
162 #define MAX_ZOOM 1960
163 #define ZOOM_CONV_FACTOR 17
165 #define PT_BUFFER_INC 512
166 #define PT_READ_TIMEOUT 10000
167 #define PT_READ_TRIALS 2
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;
183 player_blobfinder_data_t blobfinder;
186 player_position2d_data_t gyro;
189 player_actarray_data_t actArray;
190 player_gripper_data_t armGripper;
196 #include "kinecalc.h"
205 void putOnBuf(
unsigned char c);
228 player_p2os_data_t p2os_data;
249 uint8_t lastGripperCmd;
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;
260 player_audio_sample_item_t last_audio_cmd;
262 int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
265 int position_subscriptions;
266 int sonar_subscriptions;
267 int actarray_subscriptions;
268 int ptz_subscriptions;
272 int SendReceive(
P2OSPacket* pkt,
bool publish_data=
true);
273 void ResetRawPositions();
275 void ToggleSonarPower(
unsigned char val);
277 void ToggleMotorPower(
unsigned char val);
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);
289 int HandleArmGripperCommand (
player_msghdr *hdr,
void *data);
290 void HandleAudioCommand(player_audio_sample_item_t audio_cmd);
293 void get_ptz_packet(
int s1,
int s2=0);
303 void OpenGripper (
void);
304 void CloseGripper (
void);
305 void StopGripper (
void);
306 void OpenArmGripper (
void);
307 void CloseArmGripper (
void);
308 void StopArmGripper (
void);
313 double aaOrients[18];
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);
324 void SetActArrayJointSpeed (
int joint,
double 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);
332 float armOffsetX, armOffsetY, armOffsetZ;
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);
343 int direct_wheel_vel_control;
345 const char* psos_serial_port;
347 const char* psos_tcp_host;
350 struct timeval lastblob_tv;
354 int motor_max_turnspeed;
360 short motor_max_trans_accel, motor_max_trans_decel;
361 short motor_max_rot_accel, motor_max_rot_decel;
366 bool ignore_checksum;
369 player_ptz_data_t ptz_data;
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();
390 int GetMaxZoom(
int * maxzoom);
394 double lastPulseTime;
395 void SendPulse (
void);
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);
Class for loading configuration file information.
Definition: configfile.h:197
Definition: kinecalc.h:45
Definition: p2os/packet.h:41
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
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
A point in 3D space.
Definition: player.h:195
A pose in space.
Definition: player.h:229