ptzCameraClientExample.cpp

Example client showing how to control a Pan/Tilt/Zoom camera remotely.

To use, first run SAVserver. Then run serverDemo, or guiServer from ARNL or guiServerSonar from SONARNL on the robot's onboard computer. Or run your own server program that includes ArServerHandlerCamera. Then from another host, run ptzCameraClientExample with the -host argument. For example, if the hostname of the robot's onboard computer is "robot", run: ptzCameraClientExample -host robot

This program connects to the server, gets a list of cameras if neccesary, and for each camera on the robot, it receives and prints out one information packet, then a continuous stream of data packets, and then sends requests to cycle through each of its pan and tilt limits.

#include "Aria.h"
#include "ArNetworking.h"
#include <math.h>


class ArNetCameraRequest : public ArNetPacket
{
  ArClientBase *myClient;
  std::string myAbsReqName;
public:
  ArNetCameraRequest(ArClientBase *client, const char *cameraName = "") : myClient(client), myAbsReqName(std::string("setCameraAbs")+cameraName)
  {}
  void setCameraName(const char *name) {
    myAbsReqName = std::string("setCameraAbs") + name;
  }
  bool requestPanTiltZoomAbs(double pan, double tilt, double zoom);
  bool requestPanTiltAbs(double pan, double tilt);
};


class ArClientHandlerCamera {
public:
  std::string name;
  std::string type;
  std::string displayName;
  std::string displayType;
  double minPan, maxPan, minTilt, maxTilt, minZoom, maxZoom;
  bool haveZoom;
  double pan, tilt, zoom;

  void handleCameraInfoReply(ArNetPacket *packet);
  void handleCameraDataReply(ArNetPacket *packet);

  ArClientBase *myClient;
  ArNetCameraRequest request;
  ArMutex myMutex;

  ArFunctor1C<ArClientHandlerCamera, ArNetPacket*> myCameraInfoReplyFunc;
  ArFunctor1C<ArClientHandlerCamera, ArNetPacket*> myCameraDataReplyFunc;

  ArClientHandlerCamera(ArClientBase *client, const char *cameraName) : 
    name(cameraName), myClient(client), request(client, cameraName) ,
    myCameraInfoReplyFunc(this, &ArClientHandlerCamera::handleCameraInfoReply),
    myCameraDataReplyFunc(this, &ArClientHandlerCamera::handleCameraDataReply)
  {
    myClient->addHandler((std::string("getCameraInfo")+name).c_str(), &myCameraInfoReplyFunc);
    myClient->addHandler((std::string("getCameraData")+name).c_str(), &myCameraDataReplyFunc);
  }

  void requestUpdates(int dataRequestFreq) {
    myClient->request((std::string("getCameraInfo")+name).c_str(), -1);
    myClient->request((std::string("getCameraData")+name).c_str(), 100);
  }

  void lock() { myMutex.lock(); }
  void unlock() { myMutex.unlock(); }
};


class PtzCameraExample
{
  ArClientBase *myClient;

  std::set<ArClientHandlerCamera*> myCameras;
  ArMutex mutex;

  ArFunctor1C<PtzCameraExample, ArNetPacket*> myCameraListReplyFunc;

  void handleCameraListReply(ArNetPacket *packet);


public:
  PtzCameraExample(ArClientBase *client) : 
    myClient(client), 
    myCameraListReplyFunc(this, &PtzCameraExample::handleCameraListReply)
  {
  }

  bool init();
  void run();
};



int main(int argc, char **argv)
{
  Aria::init();
  ArClientBase client;

  ArArgumentParser parser(&argc, argv);
  ArClientSimpleConnector clientConnector(&parser);
  parser.loadDefaultArguments();
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    return 1;
  }

  if (!clientConnector.connectClient(&client))
  {
    if(client.wasRejected())
      ArLog::log(ArLog::Terse, "Error, server '%s' rejected connection. Exiting.", client.getHost());
    else
      ArLog::log(ArLog::Terse, "Error, could not connect to server '%s'. Exiting.", client.getHost());
    return 2;
  }

  client.setRobotName(client.getHost());  // include server hostname in log messages

  client.runAsync();

  PtzCameraExample example(&client);
  if(!example.init())
    return 1;

  example.run();


  Aria::shutdown();
  return 0;
}


bool PtzCameraExample::init()
{
  // If the server has the "getCameraList" request, then it's using
  // ArServerHandlerCameraCollection, and migth have multiple PTZs/cameras each with
  // its own set of requests. So send a "getCameraList" request, and when its
  // reply is received, the handler will send "getCameraInfo" requests for each.
  // If the server does not have "getCameraList", it only has one PTZ camera, just
  // send "getCameraInfo". The handler for that will send various control
  // commands.
  // If the server does not have "getCameraInfo", then it doesn't provide any
  // access to PTZ cameras.
  if(myClient->dataExists("getCameraList"))
  {
    ArLog::log(ArLog::Normal, "Server may have multiple cameras. Requesting list.");
    myClient->addHandler("getCameraList", &myCameraListReplyFunc);
    myClient->requestOnce("getCameraList");
  }
  else if(myClient->dataExists("getCameraInfo"))
  {
    ArLog::log(ArLog::Normal, "Server does not support multiple cameras. Requesting info for its camera.");
    ArClientHandlerCamera *camClient = new ArClientHandlerCamera(myClient, "");
    camClient->requestUpdates(100);
    mutex.lock();
    myCameras.insert(camClient);
    mutex.unlock();
  }
  else
  {
    ArLog::log(ArLog::Terse, "Error, server does not have any camera control requests. (Was the server run with video features enabled or video forwarding active?)");
    return false;
  }
  return true;
}

void PtzCameraExample::handleCameraListReply(ArNetPacket *pkt)
{
  ArTypes::Byte2 numCams = pkt->bufToByte2();
  ArLog::log(ArLog::Normal, "%d cameras in list.", numCams);
  char camName[128];
  char camType[128];
  char displayName[128];
  char displayType[128];
  char cmdDesc[128];
  char cmdName[128];
  ArTypes::Byte4 cmdFreq;
  int dataReqFreq = 100;
  for(ArTypes::Byte2 i = 0; i < numCams; ++i)
  {
    memset(camName, 0, 128);
    memset(camType, 0, 128);
    memset(displayName, 0, 128);
    memset(displayType, 0, 128);
    pkt->bufToStr(camName, 128); // name

    ArClientHandlerCamera *cam = new ArClientHandlerCamera(myClient, camName);

    pkt->bufToStr(camType, 128); // type
    cam->type = camType;
    pkt->bufToStr(displayName, 128); // description
    cam->displayName = displayName;
    pkt->bufToStr(displayType, 128); // description
    cam->displayType = displayType;
    ArTypes::Byte2 numCmds = pkt->bufToByte2();
    ArLog::log(ArLog::Normal, "%d commands for camera \'%s\' (%s) / \'%s\' (%s)", numCmds, camName, camType, displayName, displayType);
    for(ArTypes::Byte2 c = 0; c < numCmds; ++c)
    {
      memset(cmdDesc, 0, 128);
      memset(cmdName, 0, 128);
      char cmdDesc[128];
      char cmdName[128];
      pkt->bufToStr(cmdDesc, 128); // description
      pkt->bufToStr(cmdName, 128); // request name
      cmdFreq = pkt->bufToByte4(); // recommended request frequency
      ArLog::log(ArLog::Normal, "Camera %s has %s command named %s with recommended request frequency %d.", camName, cmdDesc, cmdName, cmdFreq);

      if(strcmp(cmdDesc, "getCameraData") == 0)
        dataReqFreq = cmdFreq;
    }
    ArTypes::Byte2 numParams = pkt->bufToByte2();
    ArLog::log(ArLog::Normal, "Camera %s has %d parameters.", camName, numParams);
    for(ArTypes::Byte2 p = 0; p < numParams; ++p)
    {
      ArClientArg carg;
      ArConfigArg arg;
      if(!carg.bufToArgValue(pkt, arg))
        ArLog::log(ArLog::Normal, "Hmm, error getting ArClientArg for camera %s's parameter #%d.", camName, p);
    }

    cam->requestUpdates(dataReqFreq);
    mutex.lock();
    myCameras.insert(cam);
    mutex.unlock();
  }
}

void ArClientHandlerCamera::handleCameraInfoReply(ArNetPacket *pkt)
{
  lock();
  minPan = pkt->bufToByte2() / 1000.0;
  maxPan = pkt->bufToByte2() / 1000.0;
  minTilt = pkt->bufToByte2() / 1000.0;
  maxTilt = pkt->bufToByte2() / 1000.0;
  minZoom = pkt->bufToByte2() / 1000.0;
  maxZoom = pkt->bufToByte2() / 1000.0;
  haveZoom = pkt->bufToByte();
  ArLog::log(ArLog::Normal, "Got getCameraInfo reply with pan range (%f, %f), tilt range (%f, %f), zoom range (%f, %f), zoom valid %d.", minPan, maxPan, minTilt, maxTilt, minZoom, maxZoom, haveZoom);
  unlock();
}

void ArClientHandlerCamera::handleCameraDataReply(ArNetPacket *pkt)
{
  lock();
  pan = pkt->bufToByte2() / 1000.0;
  tilt = pkt->bufToByte2() / 1000.0;
  zoom = pkt->bufToByte2() /1000.0;
  ArLog::log(ArLog::Normal, "Got camera data from camera %s with current pan=%f, tilt=%f, zoom=%f.", name.c_str(), pan, tilt, zoom);
  unlock();
}

bool ArNetCameraRequest::requestPanTiltAbs(double pan, double tilt)
{
  empty();
  byte2ToBuf((ArTypes::Byte2)(pan * 1000.0));
  byte2ToBuf((ArTypes::Byte2)(tilt * 1000.0));
  finalizePacket();
  return myClient->requestOnce(myAbsReqName.c_str(), this);
}

bool ArNetCameraRequest::requestPanTiltZoomAbs(double pan, double tilt, double zoom)
{
  empty();
  byte2ToBuf((ArTypes::Byte2)(pan * 1000.0));
  byte2ToBuf((ArTypes::Byte2)(tilt * 1000.0));
  byte2ToBuf((ArTypes::Byte2)(zoom * 1000.0));
  finalizePacket();
  return myClient->requestOnce(myAbsReqName.c_str(), this);
}


void PtzCameraExample::run()
{
  enum { MinPan, MaxPan, Center1, MinTilt, MaxTilt, Center2} stage;
  stage = MinPan;
  while(myClient->isConnected()) 
  {
    mutex.lock();
    for(std::set<ArClientHandlerCamera*>::const_iterator i = myCameras.begin(); i != myCameras.end(); ++i)
    {
      ArClientHandlerCamera* c = (*i);
      c->lock();
      switch(stage)
      {
        case MinPan:
          c->request.requestPanTiltAbs(c->minPan, 0);
          stage = MaxPan;
          break;
        case MaxPan:
          c->request.requestPanTiltAbs(c->maxPan, 0);
          stage = Center1;
          break;
        case Center1:
          c->request.requestPanTiltAbs(0, 0);
          stage = MinTilt;
          break;
        case MinTilt:
          c->request.requestPanTiltAbs(0, c->minTilt);
          stage = MaxTilt;
          break;
        case MaxTilt:
          c->request.requestPanTiltAbs(0, c->maxTilt);
          stage = Center2;
          break;
        case Center2:
          c->request.requestPanTiltAbs(0, 0);
          stage = MinPan;
      }
      c->unlock();
    }
    mutex.unlock();
    ArUtil::sleep(3000);
  }
}

---