From a54849eeffde2cfefd5b8274fc2d7ef2da4e92e6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 14:54:32 +0100 Subject: adding previous integration demo tests --- integrationtests/demo_tests/arm_test.py | 37 +++++++++++ integrationtests/demo_tests/demo_tests.launch | 18 +++++ integrationtests/demo_tests/manual_input.py | 59 ++++++++++++++++ integrationtests/demo_tests/posctl_test.py | 96 +++++++++++++++++++++++++++ integrationtests/integrationtests.launch | 15 +++++ 5 files changed, 225 insertions(+) create mode 100755 integrationtests/demo_tests/arm_test.py create mode 100644 integrationtests/demo_tests/demo_tests.launch create mode 100755 integrationtests/demo_tests/manual_input.py create mode 100755 integrationtests/demo_tests/posctl_test.py create mode 100644 integrationtests/integrationtests.launch (limited to 'integrationtests') diff --git a/integrationtests/demo_tests/arm_test.py b/integrationtests/demo_tests/arm_test.py new file mode 100755 index 000000000..569e0af7c --- /dev/null +++ b/integrationtests/demo_tests/arm_test.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import actuator_armed +from manual_input import ManualInput + +class ArmTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + # + # Test arming + # + def test_arm(self): + rospy.init_node('test_node', anonymous=True) + sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + + # method to test + arm = ManualInput() + arm.arm() + + self.assertEquals(self.actuatorStatus.armed, True, "not armed") + + + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch new file mode 100644 index 000000000..b5377659f --- /dev/null +++ b/integrationtests/demo_tests/demo_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py new file mode 100755 index 000000000..4bf508509 --- /dev/null +++ b/integrationtests/demo_tests/manual_input.py @@ -0,0 +1,59 @@ +#!/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 diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/posctl_test.py new file mode 100755 index 000000000..1cbf09cf7 --- /dev/null +++ b/integrationtests/demo_tests/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 PosctlTest(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 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 POSCTL + manIn.arm() + manIn.posctl() + 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', PosctlTest) diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch new file mode 100644 index 000000000..2e59b5615 --- /dev/null +++ b/integrationtests/integrationtests.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + -- cgit v1.2.3