example.py 1.34 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
43
44
45
46
47
#!/usr/bin/env python

# The bridge generator cannot make an external service look lika a
# ROS service, only the opposite. That would seem a better fit here
# though...

import roslib; roslib.load_manifest('ros_extctrl')
import rospy
from ros_extctrl.msg import ExtCtrlCmd, ExtCtrlStatus


def handle_extctrl_status(data):
    print("Controller responded with %d" % data.status)


def interact(pub):
    while not rospy.is_shutdown():
        line = ''
        try:
            line = raw_input("Choose a controller to activate:")
        except EOFError:
            print('\nBye')
            break
        ctrl = 0
        try:
            ctrl = int(line)
        except ValueError:
            print("%s is not a number" % line)
            continue
        msg = ExtCtrlCmd()
        msg.aua_id = ctrl
        msg.pose.pose.position.x = 0.0
        msg.pose.pose.position.y = 0.0
        msg.pose.pose.position.z = 0.0
        msg.pose.pose.orientation.x = 0.0
        msg.pose.pose.orientation.y = 0.0
        msg.pose.pose.orientation.z = 0.0
        msg.pose.pose.orientation.w = 0.0
        pub.publish(msg)


if __name__ == '__main__':
    rospy.init_node('extctrl_example')
    pub = rospy.Publisher('extctrl_cmd', ExtCtrlCmd)
    sub = rospy.Subscriber('extctrl_status', ExtCtrlStatus,
                           handle_extctrl_status)
    interact(pub)