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
80 #define TTY2 42 // Added in AmigOS 1.2
81 #define GETAUX 43 // Added in AmigOS 1.2
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
103 #define ARM_AUTOPARK 80
104 #define ARM_GRIPPARK 81
109 #define STATUSSTOPPED 0x32
110 #define STATUSMOVING 0x33
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
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!!)
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 // 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
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);
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:113
#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.
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
A rectangular bounding box, used to define the size of an object.
Definition: player.h:255
virtual void Main()
Main method for driver thread.
Definition: p2os.cc:1581
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
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
An angle in 3D space.
Definition: player.h:207
double ppitch
pitch [rad]
Definition: player.h:211
Generic message header.
Definition: player.h:162
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
Definition: cmvision.h:105
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
Definition: cmvision.h:143
Definition: cmvision.h:122
A point in 3D space.
Definition: player.h:195
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
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.
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
int ReadInt(int section, const char *name, int value)
Read an integer value.
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
void ProcessMessages(void)
Process pending messages.
#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
Definition: kinecalc.h:37
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
double px
X [m].
Definition: player.h:197
Definition: p2os/robot_params.h:76
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)
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
Definition: kinecalc.h:30
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
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.
Definition: kinecalc.h:45
Definition: p2os/robot_params.h:84
Class for loading configuration file information.
Definition: configfile.h:197
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:146
An autopointer for the message queue.
Definition: message.h:74
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:82
bool Empty()
Check whether a queue is empty.
Definition: message.h:345
A pose in space.
Definition: player.h:229
#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
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:553
Messages between wsn and a robot.
Definition: er.h:87
Definition: cmvision.h:120
Definition: p2os/sip.h:47
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
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
double pyaw
yaw [rad]
Definition: player.h:213
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition: p2os.cc:1412
Definition: p2os/packet.h:41
Base class for all drivers.
Definition: driver.h:109
Definition: cmvision.h:89
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
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76