diff options
Diffstat (limited to 'integrationtests/demo_tests/direct_offboard_posctl_test.py')
-rwxr-xr-x | integrationtests/demo_tests/direct_offboard_posctl_test.py | 56 |
1 files changed, 27 insertions, 29 deletions
diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 18239e926..d61eec063 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -46,11 +45,8 @@ import numpy as np 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 from flight_path_assertion import FlightPathAssertion @@ -68,31 +64,34 @@ class DirectOffboardPosctlTest(unittest.TestCase): rospy.init_node('test_node', anonymous=True) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + self.has_pos = False + self.local_position = vehicle_local_position() + self.control_mode = vehicle_control_mode() def tearDown(self): - if (self.fpa): + if self.fpa: self.fpa.stop() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -105,12 +104,12 @@ class DirectOffboardPosctlTest(unittest.TestCase): pos.position_valid = True stp = position_setpoint_triplet() stp.current = pos - self.pubSpt.publish(stp) + self.pub_spt.publish(stp) # does it reach the position in X seconds? count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + while count < timeout: + if self.is_at_position(pos.x, pos.y, pos.z, 0.5): break count = count + 1 self.rate.sleep() @@ -121,22 +120,23 @@ class DirectOffboardPosctlTest(unittest.TestCase): # Test offboard position control # def test_posctl(self): - manIn = ManualInput() + man_in = 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") + man_in.arm() + man_in.offboard() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # prepare flight path positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + (0, 0, 0), + (0, 0, -2), + (2, 2, -2), + (2, -2, -2), + (-2, -2, -2), + (2, 2, -2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) @@ -147,12 +147,10 @@ class DirectOffboardPosctlTest(unittest.TestCase): self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, -2, 0.5): break count = count + 1 self.rate.sleep() |