162 lines
3.7 KiB
C++
162 lines
3.7 KiB
C++
#include "Aria.h"
|
|
#include "ArNetworking.h"
|
|
|
|
class InputHandler
|
|
{
|
|
public:
|
|
InputHandler(ArClientBase *client, ArKeyHandler *keyHandler);
|
|
virtual ~InputHandler(void);
|
|
void up(void);
|
|
void down(void);
|
|
void left(void);
|
|
void right(void);
|
|
void space(void);
|
|
protected:
|
|
ArClientBase *myClient;
|
|
ArKeyHandler *myKeyHandler;
|
|
ArFunctorC<InputHandler> myUpCB;
|
|
ArFunctorC<InputHandler> myDownCB;
|
|
ArFunctorC<InputHandler> myLeftCB;
|
|
ArFunctorC<InputHandler> myRightCB;
|
|
ArFunctorC<InputHandler> mySpaceCB;
|
|
};
|
|
|
|
InputHandler::InputHandler(ArClientBase *client, ArKeyHandler *keyHandler) :
|
|
myUpCB(this, &InputHandler::up),
|
|
myDownCB(this, &InputHandler::down),
|
|
myLeftCB(this, &InputHandler::left),
|
|
myRightCB(this, &InputHandler::right),
|
|
mySpaceCB(this, &InputHandler::space)
|
|
{
|
|
myClient = client;
|
|
myKeyHandler = keyHandler;
|
|
myKeyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB);
|
|
myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
|
|
myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);
|
|
myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
|
|
myKeyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB);
|
|
}
|
|
|
|
InputHandler::~InputHandler(void)
|
|
{
|
|
|
|
}
|
|
|
|
void InputHandler::up(void)
|
|
{
|
|
ArNetPacket packet;
|
|
packet.doubleToBuf(25);
|
|
myClient->requestOnce("deltaVel", &packet);
|
|
}
|
|
|
|
void InputHandler::down(void)
|
|
{
|
|
ArNetPacket packet;
|
|
packet.doubleToBuf(-25);
|
|
myClient->requestOnce("deltaVel", &packet);
|
|
}
|
|
|
|
void InputHandler::left(void)
|
|
{
|
|
ArNetPacket packet;
|
|
packet.doubleToBuf(3.2);
|
|
myClient->requestOnce("deltaHeading", &packet);
|
|
}
|
|
|
|
void InputHandler::right(void)
|
|
{
|
|
ArNetPacket packet;
|
|
packet.doubleToBuf(-3.2);
|
|
myClient->requestOnce("deltaHeading", &packet);
|
|
}
|
|
|
|
void InputHandler::space(void)
|
|
{
|
|
ArNetPacket packet;
|
|
packet.doubleToBuf(0.00001);
|
|
myClient->requestOnce("setVel", &packet);
|
|
myClient->requestOnce("deltaHeading", &packet);
|
|
}
|
|
|
|
class OutputHandler
|
|
{
|
|
public:
|
|
OutputHandler(ArClientBase *client);
|
|
virtual ~OutputHandler(void);
|
|
void handleOutput(ArNetPacket *packet);
|
|
protected:
|
|
double myX;
|
|
double myY;
|
|
double myTh;
|
|
double myVel;
|
|
double myRotVel;
|
|
double myVoltage;
|
|
ArClientBase *myClient;
|
|
ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputCB;
|
|
|
|
};
|
|
|
|
OutputHandler::OutputHandler(ArClientBase *client) :
|
|
myHandleOutputCB(this, &OutputHandler::handleOutput)
|
|
{
|
|
myClient = client;
|
|
myClient->addHandler("output", &myHandleOutputCB);
|
|
myClient->request("output", 100);
|
|
}
|
|
|
|
OutputHandler::~OutputHandler(void)
|
|
{
|
|
myClient->requestStop("output");
|
|
}
|
|
|
|
void OutputHandler::handleOutput(ArNetPacket *packet)
|
|
{
|
|
myX = packet->bufToByte4();
|
|
myY = packet->bufToByte4();
|
|
myTh = packet->bufToByte2()/10.0;
|
|
myVel = packet->bufToByte2();
|
|
myRotVel = packet->bufToByte2();
|
|
myVoltage = packet->bufToByte2()/10.0;
|
|
printf("\rx: %6.0f y: %6.0f th: %5.1f vel: %4.0f rotVel: %3.0f voltage: %4.1f",
|
|
myX, myY, myTh, myVel, myRotVel, myVoltage);
|
|
fflush(stdout);
|
|
}
|
|
|
|
|
|
void escape(void)
|
|
{
|
|
Aria::shutdown();
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
char* host = "localhost";
|
|
if(argc > 1)
|
|
host = argv[1];
|
|
Aria::init();
|
|
ArClientBase client;
|
|
ArGlobalFunctor escapeCB(&escape);
|
|
ArKeyHandler keyHandler;
|
|
Aria::setKeyHandler(&keyHandler);
|
|
|
|
|
|
printf("Connecting to standaloneServerDemo at %s:%d...\n", host, 7272);
|
|
if (!client.blockingConnect(host, 7272))
|
|
{
|
|
printf("Could not connect to server, exiting\n");
|
|
exit(1);
|
|
}
|
|
InputHandler inputHandler(&client, &keyHandler);
|
|
OutputHandler outputHandler(&client);
|
|
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
|
|
client.runAsync();
|
|
while (client.getRunningWithLock())
|
|
{
|
|
keyHandler.checkKeys();
|
|
ArUtil::sleep(1);
|
|
}
|
|
keyHandler.restore();
|
|
Aria::shutdown();
|
|
return 0;
|
|
}
|