mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 07:27:34 +08:00
Andreas Antener
60 lines
1.4 KiB
Python
Executable File
60 lines
1.4 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import sys
|
|
import rospy
|
|
|
|
from sensor_msgs.msg import Joy
|
|
from std_msgs.msg import Header
|
|
|
|
|
|
#
|
|
# Manual input control helper, fakes joystick input
|
|
# > needs to correspond to default mapping in manual_input node
|
|
#
|
|
class ManualInput:
|
|
|
|
def __init__(self):
|
|
rospy.init_node('test_node', anonymous=True)
|
|
self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10)
|
|
self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10)
|
|
|
|
def arm(self):
|
|
rate = rospy.Rate(10) # 10hz
|
|
|
|
msg = Joy()
|
|
msg.header = Header()
|
|
msg.buttons = [0, 0, 0, 0, 0]
|
|
msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
|
|
count = 0
|
|
while not rospy.is_shutdown() and count < 10:
|
|
rospy.loginfo("zeroing")
|
|
self.joyPx4.publish(msg)
|
|
self.joyIris.publish(msg)
|
|
rate.sleep()
|
|
count = count + 1
|
|
|
|
msg.buttons = [0, 0, 0, 0, 0]
|
|
msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
|
|
count = 0
|
|
while not rospy.is_shutdown() and count < 10:
|
|
rospy.loginfo("arming")
|
|
self.joyPx4.publish(msg)
|
|
self.joyIris.publish(msg)
|
|
rate.sleep()
|
|
count = count + 1
|
|
|
|
def posctl(self):
|
|
rate = rospy.Rate(10) # 10hz
|
|
|
|
# triggers posctl
|
|
msg = Joy()
|
|
msg.header = Header()
|
|
msg.buttons = [0, 0, 1, 0, 0]
|
|
msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
|
|
count = 0
|
|
while not rospy.is_shutdown() and count < 10:
|
|
rospy.loginfo("triggering posctl")
|
|
self.joyPx4.publish(msg)
|
|
self.joyIris.publish(msg)
|
|
rate.sleep()
|
|
count = count + 1
|