example.py 1.26 KB
Newer Older
1
2
3
4
5
#!/usr/bin/env python


import roslib; roslib.load_manifest('ros_extctrl')
import rospy
6
7
from ros_extctrl.srv import ExtCtrlStart
from geometry_msgs.msg import PoseStamped
8

9
def interact():
10
    while not rospy.is_shutdown():
11
12
        rospy.wait_for_service('ExtCtrlStart')
        start_controller = rospy.ServiceProxy('ExtCtrlStart', ExtCtrlStart)
13
14
15
16
17
18
        line = ''
        try:
            line = raw_input("Choose a controller to activate:")
        except EOFError:
            print('\nBye')
            break
19
        aua_id = 0
20
        try:
21
            aua_id = int(line)
22
23
24
        except ValueError:
            print("%s is not a number" % line)
            continue
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
        pose = PoseStamped()
        pose.pose.position.x = 0.0
        pose.pose.position.y = 0.0
        pose.pose.position.z = 0.0
        pose.pose.orientation.x = 0.0
        pose.pose.orientation.y = 0.0
        pose.pose.orientation.z = 0.0
        pose.pose.orientation.w = 0.0
        resp = None
        try:
            resp = start_controller(aua_id, pose)
        except rospy.ServiceException as exc:
            print("Service did not process request: " + str(exc))
            continue
        print("%s" % resp)
40
41
42


if __name__ == '__main__':
43
44
    rospy.init_node('extctrl_usage_example')
    interact()