diff options
author | Andreas Antener <antener_a@gmx.ch> | 2015-03-02 15:29:43 +0100 |
---|---|---|
committer | Andreas Antener <antener_a@gmx.ch> | 2015-03-02 15:29:43 +0100 |
commit | 5a402ed3f16c3bb989f9732be4efa9565a1c5170 (patch) | |
tree | 23d832e1f71c2a787bc42ea6655594c30a40ca5f /integrationtests/demo_tests/manual_input.py | |
parent | 9574c6dd2a2688a12850eaff10c5a1f3ff8d47c3 (diff) | |
download | px4-firmware-5a402ed3f16c3bb989f9732be4efa9565a1c5170.tar.gz px4-firmware-5a402ed3f16c3bb989f9732be4efa9565a1c5170.tar.bz2 px4-firmware-5a402ed3f16c3bb989f9732be4efa9565a1c5170.zip |
- publish offboard mode with manual input so the vehicle control status is correct
- trigger offboard in mavros attitude test
Diffstat (limited to 'integrationtests/demo_tests/manual_input.py')
-rwxr-xr-x | integrationtests/demo_tests/manual_input.py | 33 |
1 files changed, 31 insertions, 2 deletions
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index cf139bb1d..9b2471a00 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -39,6 +39,7 @@ import sys import rospy from px4.msg import manual_control_setpoint +from px4.msg import offboard_control_mode from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header @@ -46,13 +47,15 @@ from std_msgs.msg import Header # Manual input control helper # # FIXME: 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. +# the simulator does not instantiate our controller. Probably this whole class will disappear once +# arming works correctly. # 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.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): @@ -119,9 +122,35 @@ class ManualInput: rate.sleep() count = count + 1 - def offboard(self): + + def offboard_attctl(self): + self.offboard(False, False, True, True, True, True) + + def offboard_posctl(self): + self.offboard(False, False, True, False, True, True) + + # Trigger offboard and set offboard control mode before + def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True, + ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True): rate = rospy.Rate(10) # 10hz + mode = offboard_control_mode() + mode.ignore_thrust = ignore_thrust + mode.ignore_attitude = ignore_attitude + mode.ignore_bodyrate = ignore_bodyrate + mode.ignore_position = ignore_position + mode.ignore_velocity = ignore_velocity + mode.ignore_acceleration_force = ignore_acceleration_force + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("setting offboard mode") + time = rospy.get_rostime().now() + mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubOff.publish(mode) + rate.sleep() + count = count + 1 + # triggers offboard pos = manual_control_setpoint() pos.x = 0 |