-
Notifications
You must be signed in to change notification settings - Fork 14
/
ardrone_action_client.py
executable file
·45 lines (35 loc) · 1.42 KB
/
ardrone_action_client.py
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
#! /usr/bin/env python
import rospy
import time
import actionlib
from ardrone_as.msg import ArdroneAction, ArdroneGoal, ArdroneResult, ArdroneFeedback
nImage = 1
# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
global nImage
print('[Feedback] image n.%d received'%nImage)
nImage += 1
# initializes the action client node
rospy.init_node('drone_action_client')
# create the connection to the action server
client = actionlib.SimpleActionClient('/ardrone_action_server', ArdroneAction)
# waits until the action server is up and running
client.wait_for_server()
# creates a goal to send to the action server
goal = ArdroneGoal()
goal.nseconds = 10 # indicates, take pictures along 10 seconds
# sends the goal to the action server, specifying which feedback function
# to call when feedback received
client.send_goal(goal, feedback_cb=feedback_callback)
# Uncomment these lines to test goal preemption:
#time.sleep(3.0)
#client.cancel_goal() # would cancel the goal 3 seconds after starting
# wait until the result is obtained
# you can do other stuff here instead of waiting
# and check for status from time to time
# status = client.get_state()
# check the client API link below for more info
client.wait_for_result()
print('[Result] State: %d'%(client.get_state()))