Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest('rospy')
00004 roslib.load_manifest('joy')
00005 import rospy
00006 from joy.msg import Joy
00007 import SocketServer
00008 import math
00009
00010 PORTNO = 10552
00011
00012 class handler(SocketServer.DatagramRequestHandler):
00013 def handle(self):
00014 newmsg = self.rfile.readline().rstrip()
00015 print "Client %s said ``%s''" % (self.client_address[0], newmsg)
00016 self.wfile.write(self.server.oldmsg)
00017 self.server.oldmsg = newmsg
00018 args = newmsg.split(',')
00019 x = -float(args[2])
00020 y = float(args[3])
00021 z = float(args[4])
00022 d = math.sin(math.radians(5))
00023
00024 if abs(x) < d:
00025 x = 0
00026 else:
00027 x -= x/math.fabs(x)*d
00028
00029 if abs(z) < d:
00030 z = 0
00031 else:
00032 z -= z/math.fabs(z)*d
00033
00034 k = 1 / math.cos(math.radians(60))
00035 pub.publish(Joy([1.5*k*z, 0.0, 0.0, k*x], [1] ))
00036
00037 pub = rospy.Publisher('joy', Joy)
00038 rospy.init_node('pyoj', anonymous=True)
00039 s = SocketServer.UDPServer(('',PORTNO), handler)
00040 print "Awaiting UDP messages on port %d" % PORTNO
00041 s.oldmsg = "This is the starting message."
00042 s.serve_forever()
00043