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 | |
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
-rwxr-xr-x | integrationtests/demo_tests/manual_input.py | 33 | ||||
-rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_attctl_test.py | 3 | ||||
-rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_posctl_test.py | 2 |
3 files changed, 34 insertions, 4 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 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 27885635a..a52f7ffc1 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -100,6 +100,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() + manIn.offboard_attctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() @@ -111,7 +112,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() - quaternion = quaternion_from_euler(0.2, 0.2, 0) + quaternion = quaternion_from_euler(0.15, 0.15, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a1f1cf3c5..a3739ae5c 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -136,7 +136,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # FIXME: this must go ASAP when arming is implemented manIn = ManualInput() manIn.arm() - manIn.offboard() + manIn.offboard_posctl() self.assertTrue(self.arm(), "Could not arm") self.rateSec.sleep() |