205 lines
5.4 KiB
Python
205 lines
5.4 KiB
Python
from AriaPy import *
|
|
from ArNetworkingPy import *
|
|
import sys
|
|
from math import sin
|
|
|
|
|
|
# This is an example server that shows how to draw arbitrary figures in a
|
|
# client (e.g. MobileEyes).
|
|
|
|
|
|
# These are callbacks that respond to client requests for the drawings'
|
|
# geometry data.
|
|
|
|
|
|
def exampleHomeDrawingNetCallback(client, requestPkt):
|
|
print "exampleHomeDrawingNetCallback"
|
|
|
|
reply = ArNetPacket()
|
|
|
|
# 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)
|
|
print "exampleHomeDrawingNetCallback Done."
|
|
|
|
def exampleDotsDrawingNetCallback(client, requestPkt):
|
|
reply = ArNetPacket()
|
|
|
|
tik = ArUtil_getTime() % 200
|
|
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)
|
|
|
|
def exampleXDrawingNetCallback(client, requestPkt):
|
|
reply = ArNetPacket()
|
|
|
|
# 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)
|
|
|
|
def exampleArrowsDrawingNetCallback(client, requestPkt):
|
|
# 1 arrow that points at the robot
|
|
reply = ArNetPacket()
|
|
reply.byte4ToBuf(1) # 1 arrow
|
|
reply.byte4ToBuf(0); # Pos. X
|
|
reply.byte4ToBuf(700); # Pos. Y
|
|
client.sendPacketUdp(reply)
|
|
|
|
|
|
|
|
|
|
# Main program:
|
|
|
|
Aria_init()
|
|
robot = ArRobot()
|
|
server = ArServerBase()
|
|
|
|
parser = ArArgumentParser(sys.argv)
|
|
simpleConnector = ArSimpleConnector(parser)
|
|
simpleOpener = ArServerSimpleOpener(parser)
|
|
|
|
parser.loadDefaultArguments()
|
|
|
|
if not Aria_parseArgs() or not parser.checkHelpAndWarnUnparsed():
|
|
Aria_logOptions()
|
|
Aria_exit(1)
|
|
|
|
|
|
if not simpleOpener.open(server):
|
|
if simpleOpener.wasUserFileBad():
|
|
print "Error: Bad user/password/permissions file."
|
|
else:
|
|
print "Error: Could not open server port. Use -help to see options."
|
|
Aria_exit(1)
|
|
|
|
|
|
# Devices
|
|
sonarDev = ArSonarDevice()
|
|
robot.addRangeDevice(sonarDev)
|
|
|
|
irs = ArIRs()
|
|
robot.addRangeDevice(irs)
|
|
|
|
bumpers = ArBumpers()
|
|
robot.addRangeDevice(bumpers)
|
|
|
|
sick = ArSick()
|
|
robot.addRangeDevice(sick);
|
|
|
|
|
|
# attach services to the server
|
|
serverInfoRobot = ArServerInfoRobot(server, robot)
|
|
serverInfoSensor = ArServerInfoSensor(server, robot)
|
|
|
|
# This is the service that provides drawing data to the client.
|
|
drawings = ArServerInfoDrawings(server)
|
|
|
|
# Convenience function that sets up drawings for all the robot's current
|
|
# range devices (using default shape and color info)
|
|
drawings.addRobotsRangeDevices(robot)
|
|
|
|
# Add our custom drawings
|
|
|
|
linedd = ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49) # shape name, color, size, layer
|
|
drawings.addDrawing( linedd, "exampleDrawing_Home", exampleHomeDrawingNetCallback)
|
|
|
|
dotsdd = ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48)
|
|
drawings.addDrawing(dotsdd, "exampleDrawing_Dots", exampleDotsDrawingNetCallback)
|
|
|
|
segdd = ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52)
|
|
drawings.addDrawing( segdd, "exampleDrawing_XMarksTheSpot", exampleXDrawingNetCallback)
|
|
|
|
ardd = ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100)
|
|
drawings.addDrawing( ardd, "exampleDrawing_Arrows", exampleArrowsDrawingNetCallback)
|
|
|
|
# modes for moving the robot
|
|
modeStop = ArServerModeStop(server, robot)
|
|
modeDrive = ArServerModeDrive(server, robot)
|
|
modeRatioDrive = ArServerModeRatioDrive(server, robot)
|
|
modeWander = ArServerModeWander(server, robot)
|
|
modeStop.addAsDefaultMode()
|
|
modeStop.activate()
|
|
|
|
# set up some simple commands ("custom commands")
|
|
commands = ArServerHandlerCommands(server)
|
|
uCCommands = ArServerSimpleComUC(commands, robot)
|
|
loggingCommands = ArServerSimpleComMovementLogging(commands, robot)
|
|
configCommands = ArServerSimpleComLogRobotConfig(commands, robot)
|
|
|
|
# add the commands to enable and disable safe driving to the simple commands
|
|
modeDrive.addControlCommands(commands)
|
|
|
|
|
|
|
|
# Connect to the robot.
|
|
if not simpleConnector.connectRobot(robot):
|
|
print "Error: Could not connect to robot... exiting"
|
|
Aria_exit(1)
|
|
|
|
# set up the laser before handing it to the laser mode
|
|
simpleConnector.setupLaser(sick)
|
|
|
|
robot.enableMotors()
|
|
|
|
# start the robot cycle running in a background thread
|
|
robot.runAsync(True)
|
|
|
|
# start the laser processing cycle in a background thread
|
|
sick.runAsync()
|
|
|
|
# connect the laser if it was requested
|
|
if not simpleConnector.connectLaser(sick):
|
|
print "Error: Could not connect to laser... exiting"
|
|
Aria_exit(2)
|
|
|
|
# log whatever we wanted to before the runAsync
|
|
simpleOpener.checkAndLog()
|
|
|
|
# run the server thread in the background
|
|
server.runAsync()
|
|
|
|
print "Server is now running on port %d..." % (simpleOpener.getPort())
|
|
|
|
robot.waitForRunExit()
|
|
Aria_exit(0)
|
|
|