diff options
Diffstat (limited to 'integrationtests/demo_tests/offboard_posctl_test.py')
-rwxr-xr-x | integrationtests/demo_tests/offboard_posctl_test.py | 96 |
1 files changed, 96 insertions, 0 deletions
diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py new file mode 100755 index 000000000..0d5608245 --- /dev/null +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_control_mode +from px4.msg import actuator_armed +from px4.msg import position_setpoint_triplet +from px4.msg import position_setpoint +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + +from manual_input import ManualInput + + +class OffboardPosctlTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + + # + # Test offboard POSCTL + # + def test_posctl(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rate = rospy.Rate(10) # 10hz + + manIn = ManualInput() + + # arm and go into offboard + manIn.arm() + 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") + + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = 2 + pos.z = -2 + pos.y = 2 + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + positionHeld = False + break + count = count + 1 + rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) |