p2os_ptz.hpp

Go to the documentation of this file.

/*
 *  P2OS for ROS
 *  Copyright (C) 2004, 2005  ActivMedia Robotics LLC
 *  Copyright (C) 2006, 2007, 2008, 2009  MobileRobots Inc.
 *  Copyright (C) 2010  Tucker Hermans, David Feil-Seifer, Brian Gerkey, Kasper Stoy,
 *     Richard Vaughan, & Andrew Howard
 *  Copyright (C) 2018  Hunter L. Allen
 *
 *  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
 *
 */
#ifndef P2OS_DRIVER__P2OS_PTZ_HPP_
#define P2OS_DRIVER__P2OS_PTZ_HPP_

#include <p2os_msgs/PTZState.h>
#include <p2os_driver/packet.hpp>
#include <p2os_driver/robot_params.hpp>

class P2OSNode;

// Circular Buffer Used by PTZ camera
class circbuf
{
public:
  explicit 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;
};

class P2OSPtz
{
public:
  enum Command
  {
    DELIM = 0x00,  // Delimeter character
    DEVICEID = 0x30,  // Default device ID
    PANSLEW = 0x50,  // Sets the pan slew
    TILTSLEW = 0x51,  // Sets the tilt slew
    STOP = 0x53,  // Stops current pan/tilt motion
    INIT = 0x58,  // Initializes the camera
    SLEWREQ = 0x59,  // Request pan/tilt min/max slew
    ANGLEREQ = 0x5c,  // Request pan/tilt min/max angle
    PANTILT = 0x62,  // Pan/tilt command
    SETRANGE = 0x64,  // Pan/tilt min/max range assignment
    PANTILTREQ = 0x63,  // Request pan/tilt position
    INFRARED = 0x76,  // Controls operation of IR lighting
    PRODUCTNAME = 0x87,  // Requests the product name
    LEDCONTROL = 0x8E,  // Controls LED status
    CONTROL = 0x90,  // Puts camera in Control mode
    POWER = 0xA0,  // Turns on/off power
    AUTOFOCUS = 0xA1,  // Controls auto-focusing functions
    ZOOMSTOP = 0xA2,  // Stops zoom motion
    GAIN = 0xA5,  // Sets gain adjustment on camera
    FOCUS = 0xB0,  // Manual focus adjustment
    ZOOM = 0xB3,  // Zooms camera lens
    ZOOMREQ = 0xB4,  // Requests max zoom position
    IRCUTFILTER = 0xB5,  // Controls the IR cut filter
    DIGITALZOOM = 0xB7,  // Controls the digital zoom amount
    FOOTER = 0xEF,  // Packet Footer
    RESPONSE = 0xFE,  // Packet header for response
    HEADER = 0xFF  // Packet Header
  };

  // the states for communication
  enum CommState
  {
    COMM_UNKNOWN,
    COMM_BIDIRECTIONAL,
    COMM_UNIDIRECTIONAL
  };

  enum CameraType
  {
    CAMERA_VCC4,
    CAMERA_C50I
  };

protected:
  // preset limits on movements.  Based on empirical data
  enum Param
  {
    MAX_PAN = 98,               // 875 units is max pan assignment
    MIN_PAN = -98,              // -875 units is min pan assignment
    MAX_TILT = 88,              // 790 units is max tilt assignment
    MIN_TILT = -30,             // -267 units is min tilt assignment
    MAX_PAN_SLEW = 90,          // 800 positions per sec (PPS)
    MIN_PAN_SLEW = 1,           // 8 positions per sec (PPS)
    MAX_TILT_SLEW = 69,         // 662 positions per sec (PPS)
    MIN_TILT_SLEW = 1,          // 8 positions per sec (PPS)
    MAX_ZOOM_OPTIC = 1960,
    MIN_ZOOM = 0
  };

  // the various error states that the camera can return
  enum Error
  {
    CAM_ERROR_NONE = 0x30,  // No error
    CAM_ERROR_BUSY = 0x31,  // Camera busy, will not execute the command
    CAM_ERROR_PARAM = 0x35,  // Illegal parameters to function call
    CAM_ERROR_MODE = 0x39,  // Not in host control mode
    CAM_ERROR_UNKNOWN = 0xFF  // Unknown error condition.  Should never happen
  };

  // Types for turning on and off the camera
  enum Power
  {
    POWER_OFF = 0,
    POWER_ON = 1
  };

public:
  // Constructor
  explicit P2OSPtz(P2OSNode * p2os, bool bidirectional_com = false);

  // Core Functions
  int setup();
  void shutdown();
  void callback(const p2os_msgs::PTZStateConstPtr & msg);

  // Communication Functions
  int sendCommand(unsigned char * str, int len);
  int sendRequest(unsigned char * str, int len, unsigned char * reply);
  int receiveCommandAnswer(int asize);
  int receiveRequestAnswer(unsigned char * data, int s1, int s2);
  void getPtzPacket(int s1, int s2 = 0);

  // Device Command Functions
  int setPower(Power on);
  int setControlMode();
  int sendInit();

  int getMaxZoom(int * max_zoom);
  int getAbsZoom(int * zoom);
  int sendAbsZoom(int zoom);

  int setDefaultTiltRange();
  int getAbsPanTilt(int * pan, int * tilt);
  int sendAbsPanTilt(int pan, int tilt);

  // Simple getters and setters
  bool isOn() const {return is_on_;}
  p2os_msgs::PTZState getCurrentState() {return current_state_;}

  // Class members

protected:
  P2OSNode * p2os_;

public:
  circbuf cb_;

protected:
  int max_zoom_;
  int pan_, tilt_, zoom_;
  bool is_on_;
  int error_code_;
  bool bidirectional_com_;
  p2os_msgs::PTZState current_state_;

  // Class constants
  static const int MAX_COMMAND_LENGTH;
  static const int MAX_REQUEST_LENGTH;
  static const int COMMAND_RESPONSE_BYTES;
  static const int PACKET_TIMEOUT;
  static const int SLEEP_TIME_USEC;
  static const int PAN_THRESH;
  static const int TILT_THRESH;
  static const int ZOOM_THRESH;
};

#endif  //  P2OS_DRIVER__P2OS_PTZ_HPP_