Merge pull request #1923 from UAVenture/write_rosbag

Write rosbags during SITL tests

4 files changed, 146 insertions, 1 deletions

diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py
index d61eec063..734bcf590 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 30e9fe9ba..ef875ce61 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 568c2cbd8..1383cd04c 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 000000000..4dc886634
--- /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 <andreas@uaventure.com>
+#
+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()
+