p2os.h
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* $Id: p2os.h 8184 2009-08-07 08:48:58Z gbiggs $
*
* the P2OS device. it's the parent device for all the P2 'sub-devices',
* like gripper, position, sonar, etc. there's a thread here that
* actually interacts with P2OS via the serial line. the other
* "devices" communicate with this thread by putting into and getting
* data out of shared buffers.
*/
#ifndef _P2OSDEVICE_H
#define _P2OSDEVICE_H
#include <pthread.h>
#include <sys/time.h>
#include <libplayercore/playercore.h>
#include <replace/replace.h>
#include "packet.h"
#include "robot_params.h"
// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
/*
* Apparently, newer kernel require a large value (200000) here. It only
* makes the initialization phase take a bit longer, and doesn't have any
* impact on the speed at which packets are received from P2OS
*/
#define P2OS_CYCLETIME_USEC 200000
/* p2os constants */
#define P2OS_NOMINAL_VOLTAGE 12.0
/* Command numbers */
#define SYNC0 0
#define SYNC1 1
#define SYNC2 2
#define PULSE 0
#define OPEN 1
#define CLOSE 2
#define ENABLE 4
#define SETA 5
#define SETV 6
#define SETO 7
#define VEL 11
#define RVEL 21
#define SETRA 23
#define SONAR 28
#define STOP 29
#define VEL2 32
#define GRIPPER 33
#define GRIPPERVAL 36
#define TTY2 42 // Added in AmigOS 1.2
#define GETAUX 43 // Added in AmigOS 1.2
#define BUMP_STALL 44
#define JOYDRIVE 47
#define GYRO 58 // Added in AROS 1.8
#define ROTKP 82 // Added in P2OS1.M
#define ROTKV 83 // Added in P2OS1.M
#define ROTKI 84 // Added in P2OS1.M
#define TRANSKP 85 // Added in P2OS1.M
#define TRANSKV 86 // Added in P2OS1.M
#define TRANSKI 87 // Added in P2OS1.M
#define TTY3 66 // Added in AmigOS 1.3
#define GETAUX2 67 // Added in AmigOS 1.3
#define ARM_INFO 70
#define ARM_STATUS 71
#define ARM_INIT 72
#define ARM_CHECK 73
#define ARM_POWER 74
#define ARM_HOME 75
#define ARM_PARK 76
#define ARM_POS 77
#define ARM_SPEED 78
#define ARM_STOP 79
#define ARM_AUTOPARK 80
#define ARM_GRIPPARK 81
#define SOUND 90
#define PLAYLIST 91
/* Server Information Packet (SIP) types */
#define STATUSSTOPPED 0x32
#define STATUSMOVING 0x33
#define ENCODER 0x90
#define SERAUX 0xB0
#define SERAUX2 0xB8 // Added in AmigOS 1.3
#define GYROPAC 0x98 // Added AROS 1.8
#define ARMPAC 160 // ARMpac
#define ARMINFOPAC 161 // ARMINFOpac
//#define PLAYLIST 0xD0
/* Argument types */
#define ARGINT 0x3B // Positive int (LSB, MSB)
#define ARGNINT 0x1B // Negative int (LSB, MSB)
#define ARGSTR 0x2B // String (Note: 1st byte is length!!)
/* gripper stuff */
#define GRIPopen 1
#define GRIPclose 2
#define GRIPstop 3
#define LIFTup 4
#define LIFTdown 5
#define LIFTstop 6
#define GRIPstore 7
#define GRIPdeploy 8
#define GRIPhalt 15
#define GRIPpress 16
#define LIFTcarry 17
/* CMUcam stuff */
#define CMUCAM_IMAGE_WIDTH 80
#define CMUCAM_IMAGE_HEIGHT 143
#define CMUCAM_MESSAGE_LEN 10
/* conection stuff */
#define DEFAULT_P2OS_PORT "/dev/ttyS0"
#define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
#define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
/* Canon PTZ (VC-C4) Stuff */
#define CAM_ERROR_NONE 0x30
#define CAM_ERROR_BUSY 0x31
#define CAM_ERROR_PARAM 0x35
#define CAM_ERROR_MODE 0x39
#define PTZ_SLEEP_TIME_USEC 100000
#define MAX_PTZ_COMMAND_LENGTH 19
#define MAX_PTZ_REQUEST_LENGTH 17
#define COMMAND_RESPONSE_BYTES 6
#define PTZ_PAN_MAX 98.0 // 875 units 0x36B
#define PTZ_TILT_MAX 88.0 // 790 units 0x316
#define PTZ_TILT_MIN -30.0 // -267 units 0x10B
#define MAX_ZOOM 1960 //1900
#define ZOOM_CONV_FACTOR 17
#define PT_BUFFER_INC 512
#define PT_READ_TIMEOUT 10000
#define PT_READ_TRIALS 2
typedef struct player_p2os_data
{
//Standard SIP
player_position2d_data_t position;
player_sonar_data_t sonar;
player_gripper_data_t gripper;
player_actarray_data_t lift;
player_power_data_t power;
player_bumper_data_t bumper;
player_position2d_data_t compass;
player_dio_data_t dio;
player_aio_data_t aio;
//Blobfinder
player_blobfinder_data_t blobfinder;
//Gyro
player_position2d_data_t gyro;
//ARMPAC
player_actarray_data_t actArray;
player_gripper_data_t armGripper;
} __attribute__ ((packed)) player_p2os_data_t;
// this is here because we need the above typedef's before including it.
#include "sip.h"
#include "kinecalc.h"
class SIP;
// Circular Buffer Used by PTZ camera
class circbuf{
public:
circbuf(int size=512);
void putOnBuf(unsigned char c);
int getFromBuf();
bool haveData();
int size();
void printBuf();
bool gotPacket();
void reset();
private:
unsigned char* buf;
int start;
int end;
int mysize;
bool gotPack;
};
// Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
//class KineCalc;
class P2OS : public ThreadedDriver
{
private:
player_p2os_data_t p2os_data;
player_devaddr_t position_id;
player_devaddr_t sonar_id;
player_devaddr_t aio_id;
player_devaddr_t dio_id;
player_devaddr_t gripper_id;
player_devaddr_t lift_id;
player_devaddr_t bumper_id;
player_devaddr_t power_id;
player_devaddr_t compass_id;
player_devaddr_t gyro_id;
player_devaddr_t blobfinder_id;
player_devaddr_t audio_id;
player_devaddr_t actarray_id;
player_devaddr_t limb_id;
player_devaddr_t ptz_id;
player_devaddr_t armgripper_id;
// Book keeping to only send new commands
bool sentGripperCmd;
uint8_t lastGripperCmd;
uint8_t lastLiftCmd;
player_actarray_position_cmd_t lastLiftPosCmd;
bool sentArmGripperCmd;
uint8_t lastArmGripperCmd;
uint8_t lastActArrayCmd;
player_actarray_position_cmd_t lastActArrayPosCmd;
player_actarray_home_cmd_t lastActArrayHomeCmd;
// bookkeeping to only send new audio I/O commands
bool sent_audio_cmd;
player_audio_sample_item_t last_audio_cmd;
// PID settings
int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
int position_subscriptions;
int sonar_subscriptions;
int actarray_subscriptions;
int ptz_subscriptions;
SIP* sippacket;
int SendReceive(P2OSPacket* pkt, bool publish_data=true);
void ResetRawPositions();
/* toggle sonars on/off, according to val */
void ToggleSonarPower(unsigned char val);
/* toggle motors on/off, according to val */
void ToggleMotorPower(unsigned char val);
int HandleConfig(QueuePointer & resp_queue,
player_msghdr * hdr,
void* data);
int HandleCommand(player_msghdr * hdr, void * data);
void StandardSIPPutData(double timestampStandardSIP);
void GyroPutData(double timestampGyro);
void BlobfinderPutData(double timestampSERAUX);
void ActarrayPutData(double timestampArm);
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
int HandleGripperCommand (player_msghdr *hdr, void *data);
int HandleLiftCommand (player_msghdr *hdr, void *data);
int HandleArmGripperCommand (player_msghdr *hdr, void *data);
void HandleAudioCommand(player_audio_sample_item_t audio_cmd);
void get_ptz_packet(int s1, int s2=0);
int SetupPtz();
// Gripper stuff
player_pose3d_t gripperPose;
player_bbox3d_t gripperOuterSize;
player_bbox3d_t gripperInnerSize;
player_bbox3d_t armGripperOuterSize;
player_bbox3d_t armGripperInnerSize;
void OpenGripper (void);
void CloseGripper (void);
void StopGripper (void);
void OpenArmGripper (void);
void CloseArmGripper (void);
void StopArmGripper (void);
// Actarray stuff
double aaLengths[6];
double aaOrients[18];
double aaAxes[18];
player_point_3d_t aaBasePos;
player_orientation_3d_t aaBaseOrient;
inline double TicksToDegrees (int joint, unsigned char ticks);
inline unsigned char DegreesToTicks (int joint, double degrees);
inline double TicksToRadians (int joint, unsigned char ticks);
inline unsigned char RadiansToTicks (int joint, double rads);
inline double RadsPerSectoSecsPerTick (int joint, double speed);
inline double SecsPerTicktoRadsPerSec (int joint, double secs);
void ToggleActArrayPower (unsigned char val, bool lock = true); // Toggle actarray power on/off
void SetActArrayJointSpeed (int joint, double speed); // Set a joint speed
void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
int HandleActArrayCommand (player_msghdr * hdr, void * data);
// Limb stuff
KineCalc *kineCalc;
float armOffsetX, armOffsetY, armOffsetZ;
// This is here because we don't want it zeroed every time someone fills in some other data
player_limb_data_t limb_data;
void HandleLimbHomeCmd (void);
void HandleLimbStopCmd (void);
void HandleLimbSetPoseCmd (player_limb_setpose_cmd_t cmd);
void HandleLimbSetPositionCmd (player_limb_setposition_cmd_t cmd);
void HandleLimbVecMoveCmd (player_limb_vecmove_cmd_t cmd);
int HandleLimbCommand (player_msghdr * hdr, void * data);
int param_idx; // index in the RobotParams table for this robot
int direct_wheel_vel_control; // false -> separate trans and rot vel
int psos_fd; // p2os device file descriptor
const char* psos_serial_port; // name of serial port device
bool psos_use_tcp; // use TCP port instead of serial port
const char* psos_tcp_host; // hostname to use if using TCP
int psos_tcp_port; // remote port to use if using TCP
struct timeval lastblob_tv;
// Max motor speeds (mm/sec,deg/sec)
int motor_max_speed;
int motor_max_turnspeed;
// Bound the command velocities
bool use_vel_band;
// Max motor accel/decel (mm/sec/sec, deg/sec/sec)
short motor_max_trans_accel, motor_max_trans_decel;
short motor_max_rot_accel, motor_max_rot_decel;
int radio_modemp; // are we using a radio modem?
int joystickp; // are we using a joystick?
int bumpstall; // should we change the bumper-stall behavior?
bool ignore_checksum;
// PTZ Camera Stuff
player_ptz_data_t ptz_data;
int maxfov, minfov;
int maxzoom;
int pandemand, tiltdemand, zoomdemand;
int SendCommand(unsigned char *str, int len);
int SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1);
void PrintPacket(char* str, unsigned char* cmd, int len);
int SendAbsPanTilt(int pan, int tilt);
int setDefaultTiltRange();
int GetAbsPanTilt(int* pan, int* tilt);
int GetAbsZoom(int* zoom);
int SendAbsZoom(int zoom);
int read_ptz(unsigned char *reply, int size);
int ReceiveCommandAnswer(int asize);
int ReceiveRequestAnswer(unsigned char *data, int s1, int s2);
int setControlMode();
int setNotifyCommand();
int setPower(int on);
int setOnScreenOff();
int CheckHostControlMode();
int sendInit();
int GetMaxZoom(int * maxzoom);
circbuf cb;
float pulse; // Pulse time
double lastPulseTime; // Last time of sending a pulse or command to the robot
void SendPulse (void);
public:
P2OS(ConfigFile* cf, int section);
~P2OS (void);
virtual int Subscribe(player_devaddr_t id);
virtual int Unsubscribe(player_devaddr_t id);
/* the main thread */
virtual void Main();
virtual int MainSetup();
virtual void MainQuit();
// MessageHandler
virtual int ProcessMessage(QueuePointer & resp_queue,
player_msghdr * hdr,
void * data);
void CMUcamReset(bool doLock = true);
void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
int gmax=0, int bmin=0, int bmax=0);
void CMUcamStartTracking(bool doLock = true);
void CMUcamStopTracking(bool doLock = true);
};
#endif
ConfigFile
Class for loading configuration file information.
Definition: configfile.h:196
player_position2d_cmd_vel
position 2d velocity command
Definition: player_interfaces.h:617
P2OS::MainQuit
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: p2os.cc:1327
player_bbox3d
A rectangular bounding box, used to define the size of an object.
Definition: player.h:250
player_msghdr
Generic message header.
Definition: player.h:157
player_audio_sample_item
Player audio sample selection.
Definition: player_interfaces.h:1605
P2OS::MainSetup
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: p2os.cc:698
player_actarray_position_cmd
Command: Joint position control (PLAYER_ACTARRAY_CMD_POS)
Definition: player_interfaces.h:3837
player_actarray_home_cmd
Command: Joint home (PLAYER_CMD_ACTARRAY_HOME)
Definition: player_interfaces.h:3881
player_limb_setpose_cmd
Command: Set end effector pose (PLAYER_LIMB_CMD_SETPOSE)
Definition: player_interfaces.h:4056
player_p2os_data
Definition: p2os.h:169
player_orientation_3d
An angle in 3D space.
Definition: player.h:202
KineCalc
Definition: kinecalc.h:44
player_devaddr
A device address.
Definition: player.h:141
ThreadedDriver
Base class for drivers which oeprate with a thread.
Definition: driver.h:544
player_limb_data
Data: state (PLAYER_LIMB_DATA)
Definition: player_interfaces.h:4037
__attribute__
Definition: er.h:86
player_power_data
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
QueuePointer
An autopointer for the message queue.
Definition: message.h:73
P2OS::Unsubscribe
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition: p2os.cc:1412
player_bumper_data
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1920
player_limb_vecmove_cmd
Command: Vector move the end effector (PLAYER_LIMB_CMD_VECMOVE)
Definition: player_interfaces.h:4080
P2OSPacket
Definition: p2os/packet.h:40
player_gripper_data
Data: state (PLAYER_GRIPPER_DATA_STATE)
Definition: player_interfaces.h:418
player_blobfinder_data
Data: detected blobs (PLAYER_BLOBFINDER_DATA_BLOBS)
Definition: player_interfaces.h:1099
circbuf
Definition: p2os.h:201
P2OS::Main
virtual void Main()
Main method for driver thread.
Definition: p2os.cc:1581
P2OS
Definition: p2os.h:225
P2OS::ProcessMessage
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: p2os.cc:2218
SIP
Definition: p2os/sip.h:46
P2OS::Subscribe
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition: p2os.cc:1388
player_limb_setposition_cmd
Command: Set end effector position (PLAYER_LIMB_CMD_SETPOSITION)
Definition: player_interfaces.h:4070
player_pose3d
A pose in space.
Definition: player.h:224
player_position2d_data
position2d data
Definition: player_interfaces.h:606
player_point_3d
A point in 3D space.
Definition: player.h:190
player_actarray_data
Data: state (PLAYER_ACTARRAY_DATA_STATE)
Definition: player_interfaces.h:3775
player_dio_data
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1992
player_aio_data
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2051
player_ptz_data
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1231
player_sonar_data
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:772

Last updated 12 September 2005 21:38:45