mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
- updated manual input to publish directly to px4 and not over joystick topics
- updated tests to work with current setup
This commit is contained in:
Andreas Antener
Thomas Gubler
parent
a54849eeff
commit
e0e7f8c517
@ -1,8 +1,8 @@
|
||||
<launch>
|
||||
<arg name="headless" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="enable_logging" default="false"/>
|
||||
<arg name="enable_ground_truth" default="true"/>
|
||||
<arg name="enable_ground_truth" default="false"/>
|
||||
<arg name="log_file" default="iris"/>
|
||||
|
||||
<include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
|
||||
@ -12,7 +12,8 @@
|
||||
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
|
||||
<arg name="log_file" value="$(arg log_file)"/>
|
||||
</include>
|
||||
<include file="$(find px4)/launch/mavros_sitl.launch" />
|
||||
|
||||
<test test-name="arm" pkg="px4" type="arm_test.py" />
|
||||
<test test-name="posctl" pkg="px4" type="posctl_test.py" />
|
||||
<test test-name="offboard_posctl" pkg="px4" type="offboard_posctl_test.py" />
|
||||
</launch>
|
||||
|
||||
@ -2,43 +2,59 @@
|
||||
import sys
|
||||
import rospy
|
||||
|
||||
from sensor_msgs.msg import Joy
|
||||
from px4.msg import manual_control_setpoint
|
||||
from mav_msgs.msg import CommandAttitudeThrust
|
||||
from std_msgs.msg import Header
|
||||
|
||||
|
||||
#
|
||||
# Manual input control helper, fakes joystick input
|
||||
# > needs to correspond to default mapping in manual_input node
|
||||
# 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.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10)
|
||||
self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10)
|
||||
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
|
||||
|
||||
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]
|
||||
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")
|
||||
self.joyPx4.publish(msg)
|
||||
self.joyIris.publish(msg)
|
||||
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
|
||||
|
||||
msg.buttons = [0, 0, 0, 0, 0]
|
||||
msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0]
|
||||
pos.r = 1
|
||||
count = 0
|
||||
while not rospy.is_shutdown() and count < 10:
|
||||
rospy.loginfo("arming")
|
||||
self.joyPx4.publish(msg)
|
||||
self.joyIris.publish(msg)
|
||||
time = rospy.get_rostime().now()
|
||||
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
|
||||
self.pubMcsp.publish(pos)
|
||||
rate.sleep()
|
||||
count = count + 1
|
||||
|
||||
@ -46,14 +62,49 @@ class ManualInput:
|
||||
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]
|
||||
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")
|
||||
self.joyPx4.publish(msg)
|
||||
self.joyIris.publish(msg)
|
||||
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
|
||||
|
||||
|
||||
@ -16,7 +16,7 @@ from std_msgs.msg import Header
|
||||
from manual_input import ManualInput
|
||||
|
||||
|
||||
class PosctlTest(unittest.TestCase):
|
||||
class OffboardPosctlTest(unittest.TestCase):
|
||||
|
||||
#
|
||||
# General callback functions used in tests
|
||||
@ -37,7 +37,7 @@ class PosctlTest(unittest.TestCase):
|
||||
return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset)
|
||||
|
||||
#
|
||||
# Test POSCTL
|
||||
# Test offboard POSCTL
|
||||
#
|
||||
def test_posctl(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
@ -48,9 +48,9 @@ class PosctlTest(unittest.TestCase):
|
||||
|
||||
manIn = ManualInput()
|
||||
|
||||
# arm and go into POSCTL
|
||||
# arm and go into offboard
|
||||
manIn.arm()
|
||||
manIn.posctl()
|
||||
manIn.offboard()
|
||||
self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set")
|
||||
self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
|
||||
self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set")
|
||||
@ -93,4 +93,4 @@ class PosctlTest(unittest.TestCase):
|
||||
|
||||
if __name__ == '__main__':
|
||||
import rostest
|
||||
rostest.rosrun(PKG, 'posctl_test', PosctlTest)
|
||||
rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
|
||||
@ -3,7 +3,7 @@
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="enable_logging" default="false"/>
|
||||
<arg name="enable_ground_truth" default="true"/>
|
||||
<arg name="enable_ground_truth" default="false"/>
|
||||
<arg name="log_file" default="ardrone"/>
|
||||
|
||||
<include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch">
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user