blob: eb9144bbcb8d4c1d1e85b833bc021a0ce9b92c2b (
plain) (
blame)
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
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
|
#!/usr/bin/env python
import sys
import rospy
from px4.msg import manual_control_setpoint
from mav_msgs.msg import CommandAttitudeThrust
from std_msgs.msg import Header
#
# Manual input control helper
#
# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else
# the simulator does not instantiate our controller.
#
class ManualInput:
def __init__(self):
rospy.init_node('test_node', anonymous=True)
self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
def arm(self):
rate = rospy.Rate(10) # 10hz
att = CommandAttitudeThrust()
att.header = Header()
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("zeroing")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
# Fake input to iris commander
self.pubAtt.publish(att)
rate.sleep()
count = count + 1
pos.r = 1
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("arming")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
def posctl(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 2
pos.return_switch = 3
pos.posctl_switch = 1
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
def offboard(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 1
count = 0
while not rospy.is_shutdown() and count < 10:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
|
