288 lines
8.4 KiB
C++
288 lines
8.4 KiB
C++
#include "Aria.h"
|
|
#include "ArNetworking.h"
|
|
#include <math.h>
|
|
|
|
|
|
/** @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<Circle, ArServerClient*, ArNetPacket*> 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<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
|
|
);
|
|
drawings.addDrawing(
|
|
new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
|
|
"exampleDrawing_Dots",
|
|
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
|
|
);
|
|
drawings.addDrawing(
|
|
new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
|
|
"exampleDrawing_XMarksTheSpot",
|
|
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
|
|
);
|
|
drawings.addDrawing(
|
|
new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
|
|
"exampleDrawing_Arrows",
|
|
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&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);
|
|
}
|
|
|