post_gbs_cmdarray.py 1.16 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
#!/usr/bin/env python

from gbs.srv import *
import rospy

def near(t, n):
    return n-0.01 < t and t < n+0.01

def prettyprint(array):
    for i in range(array.n):
       c0 = array.cmd[i].array[0]
       c1 = array.cmd[i].array[1]
       c2 = array.cmd[i].array[2]

       if near(c0,10) and near(c1,1) and near(c2,1):
           x = array.cmd[i].array[3]
           y = array.cmd[i].array[4]
           z = array.cmd[i].array[5]
           a = array.cmd[i].array[6]
           b = array.cmd[i].array[7]
           c = array.cmd[i].array[8]
           s = array.cmd[i].array[9]
           ac = array.cmd[i].array[10]
           print "linmove x=%f y=%f z=%f a=%f b=%f c=%f s=%f ac=%f"%(x,y,z,a,b,c,s,ac)
       else:
           print "Unknown GBS command %f %f %f"%(c0,c1,c2)


def handle_post_gbs_cmdarray(req):
    print "Received an GBS array"
    prettyprint(req.array)
    return PostGBSCmdArrayResponse()

def add_post_gbs_cmdarray():
    rospy.init_node('post_gbs_cmdarray_server')
    s = rospy.Service('post_gbs_cmdarray', PostGBSCmdArray, handle_post_gbs_cmdarray)
    print "Ready to receive GBS arrays"
    rospy.spin()

if __name__ == "__main__":
    add_post_gbs_cmdarray()