diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index d61eec0632..734bcf5909 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -39,6 +39,7 @@ PKG = 'px4' import unittest import rospy +import rosbag from numpy import linalg import numpy as np @@ -47,9 +48,11 @@ from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint +from px4.msg import vehicle_local_position_setpoint from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by directly sending setpoints @@ -62,8 +65,11 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.helper = PX4TestHelper("direct_offboard_posctl_test") + self.helper.setUp() + 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.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) 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 @@ -74,9 +80,12 @@ class DirectOffboardPosctlTest(unittest.TestCase): if self.fpa: self.fpa.stop() + self.helper.tearDown() + # # General callback functions used in tests # + def position_callback(self, data): self.has_pos = True self.local_position = data @@ -105,6 +114,7 @@ class DirectOffboardPosctlTest(unittest.TestCase): stp = position_setpoint_triplet() stp.current = pos self.pub_spt.publish(stp) + self.helper.bag_write('px4/position_setpoint_triplet', stp) # does it reach the position in X seconds? count = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 30e9fe9baf..ef875ce61e 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -39,12 +39,17 @@ PKG = 'px4' import unittest import rospy +import rosbag from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending attitude and thrust setpoints @@ -57,6 +62,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) rospy.wait_for_service('iris/mavros/cmd/arming', 30) + self.helper = PX4TestHelper("mavros_offboard_attctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) @@ -66,6 +74,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.control_mode = vehicle_control_mode() self.local_position = PoseStamped() + def tearDown(self): + self.helper.tearDown() + # # General callback functions used in tests # @@ -99,7 +110,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pub_att.publish(att) + self.helper.bag_write('mavros/setpoint/attitude', att) self.pub_thr.publish(throttle) + self.helper.bag_write('mavros/setpoint/att_throttle', throttle) self.rate.sleep() if (self.local_position.pose.position.x > 5 diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 568c2cbd80..1383cd04cd 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -40,14 +40,18 @@ PKG = 'px4' import unittest import rospy import math +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_local_position_setpoint from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending position setpoints @@ -60,6 +64,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) + self.helper = PX4TestHelper("mavros_offboard_posctl_test") + self.helper.setUp() + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) @@ -68,6 +75,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.local_position = PoseStamped() self.control_mode = vehicle_control_mode() + def tearDown(self): + self.helper.tearDown() + # # General callback functions used in tests # @@ -118,6 +128,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pub_spt.publish(pos) + self.helper.bag_write('mavros/setpoint/local_position', pos) if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/demo_tests/px4_test_helper.py new file mode 100644 index 0000000000..4dc8866341 --- /dev/null +++ b/integrationtests/demo_tests/px4_test_helper.py @@ -0,0 +1,111 @@ +#!/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 unittest +import rospy +import rosbag + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude +from px4.msg import vehicle_local_position_setpoint + +from threading import Condition + +# +# Test helper +# +class PX4TestHelper(object): + + def __init__(self, test_name): + self.test_name = test_name + + def setUp(self): + self.condition = Condition() + self.closed = False + + rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4") + + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", + vehicle_local_position, self.vehicle_position_callback) + self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint", + vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback) + self.sub_va = rospy.Subscriber("iris/vehicle_attitude", + vehicle_attitude, self.vehicle_attitude_callback) + self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint", + vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback) + + + def tearDown(self): + try: + self.condition.acquire() + self.closed = True + + self.sub_vlp.unregister() + self.sub_vasp.unregister() + self.sub_va.unregister() + self.sub_vlps.unregister() + self.bag.close() + + finally: + self.condition.release() + + def vehicle_position_callback(self, data): + self.bag_write('px4/vehicle_local_position', data) + + def vehicle_attitude_setpoint_callback(self, data): + self.bag_write('px4/vehicle_attitude_setpoint', data) + + def vehicle_attitude_callback(self, data): + self.bag_write('px4/vehicle_attitude', data) + + def vehicle_local_position_setpoint_callback(self, data): + self.bag_write('px4/vehicle_local_position_setpoint', data) + + def bag_write(self, topic, data): + try: + self.condition.acquire() + if not self.closed: + self.bag.write(topic, data) + else: + rospy.logwarn("Trying to write to bag but it's already closed") + finally: + self.condition.release() +