Aria  2.7.5.2
rvisionCameraExample.cpp

This is an example using the RVision pan/tilt/zoom camera used on Seekur and Seekur Jr.

This program uses key commands to control the pan, tilt, and zoom of the camera.

It defaults to use COM3.

Commands:


UP,DOWN – tilt up/down by 1 degree LEFT,RIGHT – pan left/right by 1 degree X,C – zoom in/out by 10 units (80 total) I – initialize PTU to default settings >,< – increase/decrease the positional increment by one 1 degree +,- – increase/decrease the slew by 5 degrees/sec Z – move pan and tilt axes to zero H – Halt all motion S – Status of camera position and variable values P – Power on/off the camera ESC – Exit program

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012 Adept Technology
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
If you wish to redistribute ARIA under different terms, contact
MobileRobots for information about a commercial version of ARIA at
robots@mobilerobots.com or
MobileRobots Inc, 10 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/
#include "Aria.h"
#include "ArRVisionPTZ.h"
/*
This class is the core of this demo, it adds itself to the robot given
as a user task, then drives the PTZ camera from the keyboard
*/
class KeyPTU
{
public:
// constructor
KeyPTU(ArRobot *robot);
// destructor, its just empty
~KeyPTU(void) {}
void up(void);
void down(void);
void left(void);
void right(void);
void greater(void);
void less(void);
void question(void);
void status(void);
void c(void);
void h(void);
void i(void);
void p(void);
void x(void);
void z(void);
void r(void);
// the callback function
void drive(void);
void power(bool p);
protected:
ArTime myExerciseTime;
int myPosIncrement;
int myZoomIncrement;
bool myMonitor;
bool myInit;
bool myAbsolute;
bool myExercise;
void exercise(void) { myExercise = !myExercise; }
ArFunctorC<KeyPTU> myRightCB;
ArFunctorC<KeyPTU> myGreaterCB;
ArFunctorC<KeyPTU> myQuestionCB;
// ArFunctorC<KeyPTU> myHCB;
ArRVisionPTZ myPTU;
bool myPTUInitRequested;
ArRobot *myRobot;
ArFunctorC<KeyPTU> myDriveCB;
bool myPowerOn;
};
/*
Constructor, sets the robot pointer, and some initial values, also note the
use of constructor chaining on myPTU and myDriveCB.
*/
KeyPTU::KeyPTU(ArRobot *robot) :
myUpCB(this, &KeyPTU::up),
myDownCB(this, &KeyPTU::down),
myLeftCB(this, &KeyPTU::left),
myRightCB(this, &KeyPTU::right),
myGreaterCB(this, &KeyPTU::greater),
myLessCB(this, &KeyPTU::less),
myQuestionCB(this, &KeyPTU::question),
mySCB(this, &KeyPTU::status),
myECB(this, &KeyPTU::exercise),
myCCB(this, &KeyPTU::c),
// myHCB(this, &KeyPTU::h),
myICB(this, &KeyPTU::i),
myPCB(this, &KeyPTU::p),
myXCB(this, &KeyPTU::x),
myZCB(this, &KeyPTU::z),
myRCB(this, &KeyPTU::r),
myPTU(robot),
myDriveCB(this, &KeyPTU::drive)
{
// set the robot pointer and add a robot user task callback (called every
// loop)
ArKeyHandler *keyHandler;
myRobot = robot;
myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);
myExerciseTime.setToNow();
myExercise = false;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
myRobot->attachKeyHandler(keyHandler);
}
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('>', &myGreaterCB))
"The key handler already has a key for '>', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('<', &myLessCB))
"The key handler already has a key for '<', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('?', &myQuestionCB))
"The key handler already has a key for '?', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('c', &myCCB))
"The key handler already has a key for 'C', keydrive will not work correctly.");
/*
if (!keyHandler->addKeyHandler('h', &myHCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'H', keydrive will not work correctly.");
*/
if (!keyHandler->addKeyHandler('i', &myICB))
"The key handler already has a key for 'I', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('p', &myPCB))
"The key handler already has a key for 'P', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('r', &myRCB))
"The key handler already has a key for 'R', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('s', &mySCB))
"The key handler already has a key for 'S', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('x', &myXCB))
"The key handler already has a key for 'X', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('z', &myZCB))
"The key handler already has a key for 'Z', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('a', &myACB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'A', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('e', &myECB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'E', keydrive will not work correctly.");
myPTUInitRequested = false;
myPosIncrement = 1;
myZoomIncrement = 50;
myPowerOn = false;
}
void KeyPTU::power(bool state)
{
if(state)
{
myRobot->com2Bytes(116, 12, 1);
myPowerOn = true;
}
else
{
myRobot->com2Bytes(116, 12, 0);
myPowerOn = false;
}
}
void KeyPTU::right(void)
{
myPTU.panRel(myPosIncrement);
}
void KeyPTU::left(void)
{
myPTU.panRel(-myPosIncrement);
}
void KeyPTU::up(void)
{
myPTU.tiltRel(myPosIncrement);
}
void KeyPTU::down(void)
{
myPTU.tiltRel(-myPosIncrement);
}
void KeyPTU::x(void)
{
myPTU.zoom(myPTU.getZoom() + myZoomIncrement);
}
void KeyPTU::c(void)
{
myPTU.zoom(myPTU.getZoom() - myZoomIncrement);
}
void KeyPTU::i(void)
{
myPTU.init();
}
void KeyPTU::greater(void)
{
myPosIncrement += 1;
if (myPosIncrement > myPTU.getMaxPosPan()) //Use pan range as reference for largest allowable positional increment
myPosIncrement = myPTU.getMaxPosPan();
status();
}
void KeyPTU::less(void)
{
myPosIncrement -= 1;
if (myPosIncrement < 0)
myPosIncrement = 0;
status();
}
void KeyPTU::z(void)
{
myPTU.panTilt(0,0);
myPTU.zoom(0);
status();
}
void KeyPTU::r(void)
{
myPTU.reset();
}
void KeyPTU::question(void)
{
ArLog::log(ArLog::Normal, "\r\nCommands:\r\n_________________\r\n");
ArLog::log(ArLog::Normal, "UP,DOWN -- tilt up/down by 1 increment");
ArLog::log(ArLog::Normal, "LEFT,RIGHT -- pan left/right by 1 increment");
ArLog::log(ArLog::Normal, "X,C -- zoom in/out by 50 units (2140 max)");
ArLog::log(ArLog::Normal, "I -- initialize PTU to default settings");
ArLog::log(ArLog::Normal, ">,< -- increase/decrease the positional increment by 1 degree");
ArLog::log(ArLog::Normal, "+,- -- increase/decrease the slew by 5 degrees/sec");
ArLog::log(ArLog::Normal, "Z -- move pan and tilt axes to zero");
ArLog::log(ArLog::Normal, "E -- toggle exercise mode");
ArLog::log(ArLog::Normal, "H -- Halt all motion");
ArLog::log(ArLog::Normal, "S -- Status of camera position and variable values");
// ArLog::log(ArLog::Normal, "P -- Power on/off the camera");
ArLog::log(ArLog::Normal, "R -- Reset the camera");
ArLog::log(ArLog::Normal, "ESC -- Exit program");
}
void KeyPTU::status(void)
{
ArLog::log(ArLog::Normal, "\r\nStatus:\r\n_________________________\r\n");
ArLog::log(ArLog::Normal, "Pan Position = %.0f deg", myPTU.getPan());
ArLog::log(ArLog::Normal, "Tilt Position = %.0f deg", myPTU.getTilt());
ArLog::log(ArLog::Normal, "Zoom Position = %d", myPTU.getZoom());
ArLog::log(ArLog::Normal, "Position Increment = %d deg", myPosIncrement);
ArLog::log(ArLog::Normal, "Exercise = %d", myExercise);
if (myPowerOn)
ArLog::log(ArLog::Normal, "Power is ON");
else
ArLog::log(ArLog::Normal, "Power is OFF");
}
/* not implemented yet in ArRVision
void KeyPTU::h(void)
{
myPTU.haltPanTilt();
myPTU.haltZoom();
}
*/
void KeyPTU::p(void)
{
// toggle power state and print new status
power(!myPowerOn);
status();
}
// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInitRequested && myRobot->isConnected())
{
printf("\nInitializing ArRVision\n");
myAbsolute = true;
myPTUInitRequested = true;
myPTU.init();
question();
}
/*
// if the camera hasn't initialized yet, then just return
if (myPTUInitRequested && !myPTU.isInitted())
{
return;
}
*/
/*
if (myPTUInitRequested ) //&& myPTU.isInitted())
{
myPTUInitRequested = false;
printf("Done.\n");
question();
}
*/
if (myExerciseTime.secSince() > 5 && myExercise)
{
int pan,tilt;
if (ArMath::random()%2)
pan = ArMath::random()%((int)myPTU.getMaxPosPan());
else
pan = -ArMath::random()%((int)myPTU.getMaxNegPan());
if (ArMath::random()%2)
tilt = ArMath::random()%((int)myPTU.getMaxPosTilt());
else
tilt = -ArMath::random()%((int)myPTU.getMaxNegTilt());
myPTU.panTilt(pan, tilt);
//printf("** %d\n", myRobot->getEstop());
//printf("--> %x\n", myRobot->getFlags());
myExerciseTime.setToNow();
}
}
int main(int argc, char **argv)
{
std::string str;
ArArgumentParser parser(&argc, argv);
ArRobot robot;
ArRobotConnector con(&parser, &robot);
KeyPTU ptu(&robot);
if (!con.connectRobot())
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting\n");
if(parser.checkHelpAndWarnUnparsed())
return 1;
}
{
}
ArLog::log(ArLog::Normal, "connected to robot.");
ptu.power(true);
printf("Press '?' for available commands\r\n");
robot.run(true);
}