-
Notifications
You must be signed in to change notification settings - Fork 1
/
forward.py
86 lines (69 loc) · 2.4 KB
/
forward.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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
#!/usr/bin/env python
#import library ros
import rospy
import time
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from std_msgs.msg import Empty
from ardrone_autonomy.msg import Navdata
COMMAND_PERIOD = 1000
class AutonomousFlight():
def __init__(self):
self.status = ""
rospy.init_node('forward', anonymous=False)
self.rate = rospy.Rate(10)
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land",Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel',Twist, queue_size=10)
self.command = Twist()
#self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand)
self.state_change_time = rospy.Time.now()
rospy.on_shutdown(self.SendLand)
def SendTakeOff(self):
self.pubTakeoff.publish(Empty())
self.rate.sleep()
def SendLand(self):
self.pubLand.publish(Empty())
def SetCommand(self, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z):
self.command.linear.x = linear_x
self.command.linear.y = linear_y
self.command.linear.z = linear_z
self.command.angular.x = angular_x
self.command.angular.y = angular_y
self.command.angular.z = angular_z
self.pubCommand.publish(self.command)
self.rate.sleep()
if __name__ == '__main__':
try:
i = 0
drone= AutonomousFlight()
while not rospy.is_shutdown():
drone.SendTakeOff()
if i <= 30 :
drone.SetCommand(0,0,1,0,0,0)
i+=1
elif i<=60 :
drone.SetCommand(1,0,0,0,0,0)
i+=1
elif i<=63:
drone.SetCommand(0,0,0,0,1,0)
i+=1
elif i<=93:
drone.SetCommand(0,1,0,0,0,0)
i+=1
elif i<=96:
drone.SetCommand(0,0,0,-1,0,0)
i+=1
elif i<=126:
drone.SetCommand(-1,0,0,0,0,0)
i+=1
elif i<=129:
drone.SetCommand(0,0,0,0,-1,0)
i+=1
elif i<=159:
drone.SetCommand(0,-1,0,0,0,0)
i+=1
else:
drone.SetCommand(0,0,0,0,0,0)
except rospy.ROSInterruptException:
pass