#include "Aria.h" #include "ArNetworking.h" #include /** @example drawingsExample.cpp Example showing how to draw custom graphics in * clients such as MobileEyes * * This is an example server that shows how to draw arbitrary figures in a * client (e.g. MobileEyes) via ArServerInfoDrawings. It draws some lines, "arrows", and moving dots * with various sizes and colors. You can use these drawings for debugging * or visualization, for example, to represent sensor readings. In fact, * specific support for ArRobot, ArSick, ArSonarDevice, ArIRs and ArBumpers * are built in to ArServerInfoDrawings: see drawingsExampleWithRobot.cpp * or serverDemo.cpp. * * @sa drawingsExampleWithRobot.cpp */ /* These are some callbacks that respond to client requests for the drawings' * geometry data. */ void exampleHomeDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt); void exampleDotsDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt); void exampleXDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt); void exampleArrowsDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt); /* This class shows how to encapsulate the various aspects of a drawing into one * object that other parts of the program can easily access in a more general * (and thread-safe) way. It lets you change position, radius, resolution * (number of dots), and dot drawing properties. * The default radius is 1 meter and the default number of points is 360. * * In this example program, the radius is updated periodically by a loop in the * main thread (see main() function), while ArNetworking requests are handled * in the separate ArNetworking server thread, calling drawingServerCB() callback (this is * registered with the server when we register the drawing with the * ArServerInfoDrawings object). So note the use of an ArMutex object to * control this asyrchronous access to the reply packet. */ class Circle { private: ArNetPacket myReply; ArMutex myMutex; // mutex around myReply ArPose myPos; double myRadius; unsigned int myNumPoints; void drawingServerCB(ArServerClient *client, ArNetPacket *pkt); ArFunctor2C callback; void regenerate(); public: Circle(ArServerInfoDrawings *drawingsServer, const std::string& name, ArDrawingData *drawData); ~Circle(); void setPos(const ArPose& p) { myPos = p; regenerate(); } void setRadius(double r) { myRadius = r; regenerate(); } void setNumPoints(unsigned int r) { myNumPoints = r; regenerate(); } }; Circle::Circle(ArServerInfoDrawings *drawingsServer, const std::string& name, ArDrawingData* drawData) : myRadius(1000.0), myNumPoints(360), callback(this, &Circle::drawingServerCB) { drawingsServer->addDrawing(drawData, name.c_str(), &callback); } Circle::~Circle() { // TODO remove drawing from the server, but not implemented yet } void Circle::drawingServerCB(ArServerClient *client, ArNetPacket *pkt) { myMutex.lock(); client->sendPacketUdp(&myReply); myMutex.unlock(); } // Method called by accessor methods when properties changed. This reconstructs // the myReply packet sent in response to requests from clients void Circle::regenerate() { myMutex.lock(); myReply.empty(); myReply.byte4ToBuf(myNumPoints); double a = 360.0/myNumPoints; for(unsigned int i = 0; i < myNumPoints; ++i) { myReply.byte4ToBuf(ArMath::roundInt(myPos.getX()+ArMath::cos(i*a)*myRadius)); // X myReply.byte4ToBuf(ArMath::roundInt(myPos.getY()+ArMath::sin(i*a)*myRadius)); // Y } myMutex.unlock(); } int main(int argc, char **argv) { Aria::init(); ArServerBase server; ArArgumentParser parser(&argc, argv); ArServerSimpleOpener simpleOpener(&parser); // parse the command line... fail and print the help if the parsing fails // or if help was requested parser.loadDefaultArguments(); if (!simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleOpener.logOptions(); exit(1); } // first open the server up if (!simpleOpener.open(&server)) { if (simpleOpener.wasUserFileBad()) printf("Error: Bad user/password/permissions file.\n"); else printf("Error: Could not open server port. Use -help to see options.\n"); exit(1); } // This is the service that provides drawing data to the client. ArServerInfoDrawings drawings(&server); // Add our custom drawings drawings.addDrawing( // shape: color: size: layer: new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49), "exampleDrawing_Home", new ArGlobalFunctor2(&exampleHomeDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48), "exampleDrawing_Dots", new ArGlobalFunctor2(&exampleDotsDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52), "exampleDrawing_XMarksTheSpot", new ArGlobalFunctor2(&exampleXDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100), "exampleDrawing_Arrows", new ArGlobalFunctor2(&exampleArrowsDrawingNetCallback) ); Circle circle(&drawings, "exampleDrawing_circle", new ArDrawingData("polySegments", ArColor(255, 150, 0), 3, 120)); circle.setPos(ArPose(0, -5000)); circle.setRadius(1000); circle.setNumPoints(360); // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // run the server thread in the background server.runAsync(); printf("Server is now running...\n"); // Add a key handler mostly that windows can exit by pressing // escape, note that the key handler prevents you from running this program // in the background on Linux. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); printf("To exit, press escape.\n"); } double circleRadius = 1000; while(true) { ArUtil::sleep(100); circleRadius += 50; if(circleRadius > 5000) circleRadius = 0; circle.setRadius(circleRadius); } Aria::shutdown(); exit(0); } // Network callbacks for drawings' current geometry data: void exampleHomeDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt) { ArNetPacket reply; // 7 Vertices reply.byte4ToBuf(7); // Centered on 0,0. // X: Y: reply.byte4ToBuf(-500); reply.byte4ToBuf(500); // Vertex 1 reply.byte4ToBuf(-500); reply.byte4ToBuf(-500); // Vertex 2 reply.byte4ToBuf(500); reply.byte4ToBuf(-500); // Vertex 3 reply.byte4ToBuf(500); reply.byte4ToBuf(500); // Vertex 4 reply.byte4ToBuf(0); reply.byte4ToBuf(1000); // Vertex 5 reply.byte4ToBuf(-500); reply.byte4ToBuf(500); // Vertex 6 reply.byte4ToBuf(500); reply.byte4ToBuf(500); // Vertex 7 client->sendPacketUdp(&reply); } void exampleDotsDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt) { ArNetPacket reply; unsigned int tik = ArUtil::getTime() % 200; double t = tik / 5.0; // Three dots reply.byte4ToBuf(3); // Dot 1: reply.byte4ToBuf(3000); // X coordinate (mm) reply.byte4ToBuf((int) (sin(t) * 1000));// Y // Dot 2: reply.byte4ToBuf(3500); // X reply.byte4ToBuf((int) (sin(t+500) * 1000));// Y // Dot 3: reply.byte4ToBuf(4000); // X reply.byte4ToBuf((int) (sin(t+1000) * 1000));// Y client->sendPacketUdp(&reply); } void exampleXDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt) { ArNetPacket reply; // X marks the spot. 2 line segments, so 4 vertices: reply.byte4ToBuf(4); // Segment 1: reply.byte4ToBuf(-4250); // X1 reply.byte4ToBuf(250); // Y1 reply.byte4ToBuf(-3750); // X2 reply.byte4ToBuf(-250); // Y2 // Segment 2: reply.byte4ToBuf(-4250); // X1 reply.byte4ToBuf(-250); // Y1 reply.byte4ToBuf(-3750); // X2 reply.byte4ToBuf(250); // Y2 client->sendPacketUdp(&reply); } void exampleArrowsDrawingNetCallback(ArServerClient* client, ArNetPacket* requestPkt) { // 1 Arrow that points at the robot ArNetPacket reply; reply.byte4ToBuf(1); reply.byte4ToBuf(0); // Pos. X reply.byte4ToBuf(700); // Pos. Y client->sendPacketUdp(&reply); }