From 638022f7ad6cd3bff61e264e8a80940ff23d084c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 22:02:26 +0100 Subject: fix headers and indentation --- integrationtests/demo_tests/direct_arm_test.py | 74 +++++-- .../demo_tests/direct_offboard_posctl_test.py | 232 ++++++++++++--------- .../demo_tests/flight_path_assertion.py | 36 ++++ integrationtests/demo_tests/manual_input.py | 222 +++++++++++--------- 4 files changed, 354 insertions(+), 210 deletions(-) diff --git a/integrationtests/demo_tests/direct_arm_test.py b/integrationtests/demo_tests/direct_arm_test.py index 569e0af7c..238f2d7e0 100755 --- a/integrationtests/demo_tests/direct_arm_test.py +++ b/integrationtests/demo_tests/direct_arm_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -10,28 +46,28 @@ 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) + # + # 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() + # method to test + arm = ManualInput() + arm.arm() - self.assertEquals(self.actuatorStatus.armed, True, "not armed") + self.assertEquals(self.actuatorStatus.armed, True, "not armed") - + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'arm_test', ArmTest) + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 6b28e0a18..42667757b 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -22,103 +58,103 @@ from flight_path_assertion import FlightPathAssertion class OffboardPosctlTest(unittest.TestCase): - def setUp(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) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - self.rate = rospy.Rate(10) # 10hz - - def tearDown(self): - if (self.fpa): - self.fpa.stop() - - # - # 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.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return linalg.norm(desired - pos) < offset - - def reach_position(self, x, y, z, timeout): - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = x - pos.y = y - pos.z = z - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - self.pubSpt.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)): - break - count = count + 1 - self.rate.sleep() - - self.assertTrue(count < timeout, "took too long to get to position") - - # - # Test offboard POSCTL - # - def test_posctl(self): - 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") - - # prepare flight path assertion - positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) - - self.fpa = FlightPathAssertion(positions, 1, 0) - self.fpa.start() - - for i in range(0, len(positions)): - self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - 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 - break - count = count + 1 - self.rate.sleep() - - self.assertTrue(count == timeout, "position could not be held") - self.fpa.stop() - + def setUp(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) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + + def tearDown(self): + if (self.fpa): + self.fpa.stop() + + # + # 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.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.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)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # + # Test offboard POSCTL + # + def test_posctl(self): + 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") + + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) + + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + 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 + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + self.fpa.stop() + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) - #unittest.main() + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 1d99b7e5a..1f5bf01fc 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy import threading diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index eb9144bbc..55911bede 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy @@ -14,97 +50,97 @@ from std_msgs.msg import Header # 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.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) - - def arm(self): - rate = rospy.Rate(10) # 10hz - - att = CommandAttitudeThrust() - att.header = Header() - - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 - - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("zeroing") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) - rate.sleep() - count = count + 1 - - pos.r = 1 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("arming") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 - - def posctl(self): - rate = rospy.Rate(10) # 10hz - - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 2 - pos.return_switch = 3 - pos.posctl_switch = 1 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 - - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 - - def offboard(self): - rate = rospy.Rate(10) # 10hz - - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 1 - - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + 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.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + + def arm(self): + rate = rospy.Rate(10) # 10hz + + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("zeroing") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) + rate.sleep() + count = count + 1 + + pos.r = 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("arming") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + def posctl(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + def offboard(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 -- cgit v1.2.3