added mavros offboard test

2 files changed, 148 insertions, 2 deletions

diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
new file mode 100755
index 000000000..7468ad53f
--- /dev/null
+++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
@@ -0,0 +1,146 @@
+#!/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 sys
+import unittest
+import rospy
+import math
+
+from numpy import linalg
+import numpy as np
+
+from px4.msg import vehicle_control_mode
+from std_msgs.msg import Header
+from geometry_msgs.msg import PoseStamped, Quaternion
+from tf.transformations import quaternion_from_euler
+
+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/mavros/position/local", PoseStamped, self.position_callback)
+        self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10)
+        self.rate = rospy.Rate(10) # 10hz
+        self.hasPos = False
+
+    #
+    # 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):
+        if(not self.hasPos):
+            return False
+
+        rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        desired = np.array((x, y, z))
+        pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z))
+        return linalg.norm(desired - pos) < offset
+
+    def reach_position(self, x, y, z, timeout):
+        # set a position setpoint
+        pos = PoseStamped()
+        pos.header = Header() 
+        pos.header.frame_id = "base_footprint"
+        pos.header.stamp = rospy.Time.now()
+        pos.pose.position.x = x
+        pos.pose.position.y = y
+        pos.pose.position.z = z
+
+        # For demo purposes we will lock yaw/heading to north.
+        yaw_degrees = 0  # North
+        yaw = math.radians(yaw_degrees)
+        quaternion = quaternion_from_euler(0, 0, yaw)
+        pos.pose.orientation = Quaternion(*quaternion)
+
+        # does it reach the position in X seconds?
+        count = 0
+        while(count < timeout):
+            self.pubSpt.publish(pos)
+            
+            if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.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):
+        # prepare flight path assertion
+        positions = (
+            (0,0,0),
+            (2,2,2),
+            (2,-2,2),
+            (-2,-2,2),
+            (2,2,2))
+
+        for i in range(0, len(positions)):
+            self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120)
+        
+        # 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")
+    
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest)
+    #unittest.main()
diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch
index f5ad3412d..8c1ad7b4d 100644
--- a/integrationtests/demo_tests/mavros_tests.launch
+++ b/integrationtests/demo_tests/mavros_tests.launch
@@ -1,5 +1,5 @@
 <launch>
-      <arg name="headless" default="true"/>
+    <arg name="headless" default="true"/>
     <arg name="gui" default="false"/>
     <arg name="enable_logging" default="false"/>
     <arg name="enable_ground_truth" default="false"/>
@@ -14,5 +14,5 @@
     </include>
     <include file="$(find px4)/launch/mavros_sitl.launch" />
 
-    <test test-name="direct_arm" pkg="px4" type="mavros_offboard_posctl_test.py" />
+    <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
 </launch>