rosaria/Legacy/Aria/ArNetworking/examples/ptzCameraClientExample.cpp

335 lines
10 KiB
C++
Raw Permalink Normal View History

2021-12-16 15:07:59 +01:00
#include "Aria.h"
#include "ArNetworking.h"
#include <math.h>
/** @example ptzCameraClientExample.cpp Example client showing how to control a Pan/Tilt/Zoom
* camera remotely.
*
* To use, serverDemo, arnlServer from ARNL, sonarnlServer from SONARNL, or
* your own server program that includes an ArServerHandlerCamera object.
* 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
* Or use an IP address, for example, if the address is 10.0.126.32:
* ptzCameraClientExample -host 10.0.126.32
*
* 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.
*
* This program does not get any video images from the server. To do so, use
* a video server (e.g. SAV server or ACTS), and see getVideoExample for an
* example client.
*/
/** A request packet that is able to send a copy of itself, with data packed in,
* to a server.
*/
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);
};
/** Includes callbacks for receiving info and data packets from a
* camera server and storing the values parsed from them.
*/
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(); }
};
/** Requests a list of cameras if neccesary, maintains a list of cameras, and implements example pan/tilt movements */
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);
}
}