00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef P2OS_DRIVER__P2OS_PTZ_HPP_
00025 #define P2OS_DRIVER__P2OS_PTZ_HPP_
00026
00027 #include <p2os_msgs/PTZState.h>
00028 #include <p2os_driver/packet.hpp>
00029 #include <p2os_driver/robot_params.hpp>
00030
00031 class P2OSNode;
00032
00033
00034 class circbuf
00035 {
00036 public:
00037 explicit circbuf(int size = 512);
00038
00039 void putOnBuf(unsigned char c);
00040 int getFromBuf();
00041 bool haveData();
00042 int size();
00043 void printBuf();
00044
00045 bool gotPacket();
00046 void reset();
00047
00048 private:
00049 unsigned char * buf;
00050 int start;
00051 int end;
00052 int mysize;
00053 bool gotPack;
00054 };
00055
00056 class P2OSPtz
00057 {
00058 public:
00059 enum Command
00060 {
00061 DELIM = 0x00,
00062 DEVICEID = 0x30,
00063 PANSLEW = 0x50,
00064 TILTSLEW = 0x51,
00065 STOP = 0x53,
00066 INIT = 0x58,
00067 SLEWREQ = 0x59,
00068 ANGLEREQ = 0x5c,
00069 PANTILT = 0x62,
00070 SETRANGE = 0x64,
00071 PANTILTREQ = 0x63,
00072 INFRARED = 0x76,
00073 PRODUCTNAME = 0x87,
00074 LEDCONTROL = 0x8E,
00075 CONTROL = 0x90,
00076 POWER = 0xA0,
00077 AUTOFOCUS = 0xA1,
00078 ZOOMSTOP = 0xA2,
00079 GAIN = 0xA5,
00080 FOCUS = 0xB0,
00081 ZOOM = 0xB3,
00082 ZOOMREQ = 0xB4,
00083 IRCUTFILTER = 0xB5,
00084 DIGITALZOOM = 0xB7,
00085 FOOTER = 0xEF,
00086 RESPONSE = 0xFE,
00087 HEADER = 0xFF
00088 };
00089
00090
00091 enum CommState
00092 {
00093 COMM_UNKNOWN,
00094 COMM_BIDIRECTIONAL,
00095 COMM_UNIDIRECTIONAL
00096 };
00097
00098 enum CameraType
00099 {
00100 CAMERA_VCC4,
00101 CAMERA_C50I
00102 };
00103
00104 protected:
00105
00106 enum Param
00107 {
00108 MAX_PAN = 98,
00109 MIN_PAN = -98,
00110 MAX_TILT = 88,
00111 MIN_TILT = -30,
00112 MAX_PAN_SLEW = 90,
00113 MIN_PAN_SLEW = 1,
00114 MAX_TILT_SLEW = 69,
00115 MIN_TILT_SLEW = 1,
00116 MAX_ZOOM_OPTIC = 1960,
00117 MIN_ZOOM = 0
00118 };
00119
00120
00121 enum Error
00122 {
00123 CAM_ERROR_NONE = 0x30,
00124 CAM_ERROR_BUSY = 0x31,
00125 CAM_ERROR_PARAM = 0x35,
00126 CAM_ERROR_MODE = 0x39,
00127 CAM_ERROR_UNKNOWN = 0xFF
00128 };
00129
00130
00131 enum Power
00132 {
00133 POWER_OFF = 0,
00134 POWER_ON = 1
00135 };
00136
00137 public:
00138
00139 explicit P2OSPtz(P2OSNode * p2os, bool bidirectional_com = false);
00140
00141
00142 int setup();
00143 void shutdown();
00144 void callback(const p2os_msgs::PTZStateConstPtr & msg);
00145
00146
00147 int sendCommand(unsigned char * str, int len);
00148 int sendRequest(unsigned char * str, int len, unsigned char * reply);
00149 int receiveCommandAnswer(int asize);
00150 int receiveRequestAnswer(unsigned char * data, int s1, int s2);
00151 void getPtzPacket(int s1, int s2 = 0);
00152
00153
00154 int setPower(Power on);
00155 int setControlMode();
00156 int sendInit();
00157
00158 int getMaxZoom(int * max_zoom);
00159 int getAbsZoom(int * zoom);
00160 int sendAbsZoom(int zoom);
00161
00162 int setDefaultTiltRange();
00163 int getAbsPanTilt(int * pan, int * tilt);
00164 int sendAbsPanTilt(int pan, int tilt);
00165
00166
00167 bool isOn() const {return is_on_;}
00168 p2os_msgs::PTZState getCurrentState() {return current_state_;}
00169
00170
00171
00172 protected:
00173 P2OSNode * p2os_;
00174
00175 public:
00176 circbuf cb_;
00177
00178 protected:
00179 int max_zoom_;
00180 int pan_, tilt_, zoom_;
00181 bool is_on_;
00182 int error_code_;
00183 bool bidirectional_com_;
00184 p2os_msgs::PTZState current_state_;
00185
00186
00187 static const int MAX_COMMAND_LENGTH;
00188 static const int MAX_REQUEST_LENGTH;
00189 static const int COMMAND_RESPONSE_BYTES;
00190 static const int PACKET_TIMEOUT;
00191 static const int SLEEP_TIME_USEC;
00192 static const int PAN_THRESH;
00193 static const int TILT_THRESH;
00194 static const int ZOOM_THRESH;
00195 };
00196
00197 #endif // P2OS_DRIVER__P2OS_PTZ_HPP_