Added ground truth tests to sitl gazebo CI.

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -48,11 +48,15 @@ import rosbag
 import sys
 import os
 import time
+import glob
+import json
 
 import mavros
 from pymavlink import mavutil
 from mavros import mavlink
 
+import px4tools
+
 from geometry_msgs.msg import PoseStamped
 from mavros_msgs.srv import CommandLong, WaypointPush
 from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
@@ -60,6 +64,35 @@ from sensor_msgs.msg import NavSatFix
 from mavros.mission import QGroundControlWP
 #from px4_test_helper import PX4TestHelper
 
+def get_last_log():
+    try:
+        log_path = os.environ['PX4_LOG_DIR']
+    except KeyError:
+        log_path = os.path.join(os.environ['HOME'], 'ros/rootfs/fs/microsd/log')
+    last_log_dir = sorted(
+        glob.glob(os.path.join(log_path, '*')))[-1]
+    last_log = sorted(glob.glob(os.path.join(last_log_dir, '*.ulg')))[-1]
+    return last_log
+
+def read_new_mission(f):
+    d = json.load(f)
+    current = True
+    for wp in d['items']:
+        yield Waypoint(
+                is_current = current,
+                frame = int(wp['frame']),
+                command = int(wp['command']),
+                param1 = float(wp['param1']),
+                param2 = float(wp['param2']),
+                param3 = float(wp['param3']),
+                param4 = float(wp['param4']),
+                x_lat = float(wp['coordinate'][0]),
+                y_long = float(wp['coordinate'][1]),
+                z_alt = float(wp['coordinate'][2]),
+                autocontinue = bool(wp['autoContinue']))
+        if current:
+            current = False
+
 class MavrosMissionTest(unittest.TestCase):
     """
     Run a mission
@@ -276,11 +309,23 @@ class MavrosMissionTest(unittest.TestCase):
         mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
 
         rospy.loginfo("reading mission %s", mission_file)
-        mission = QGroundControlWP()
         wps = []
-        for waypoint in mission.read(open(mission_file, 'r')):
-            wps.append(waypoint)
-            rospy.logdebug(waypoint)
+
+        with open(mission_file, 'r') as f:
+            mission_ext = os.path.splitext(mission_file)[1]
+            if mission_ext == '.mission':
+                rospy.loginfo("new style mission fiel detected")
+                for waypoint in read_new_mission(f):
+                    wps.append(waypoint)
+                    rospy.logdebug(waypoint)
+            elif mission_ext == '.txt':
+                rospy.loginfo("old style mission fiel detected")
+                mission = QGroundControlWP()
+                for waypoint in mission.read(f):
+                    wps.append(waypoint)
+                    rospy.logdebug(waypoint)
+            else:
+                raise IOError('unknown mission file extension', mission_ext)
 
         rospy.loginfo("wait until ready")
         self.wait_until_ready()
@@ -321,6 +366,19 @@ class MavrosMissionTest(unittest.TestCase):
 
             index += 1
 
+        rospy.loginfo("mission done, calculating performance metrics")
+        last_log = get_last_log()
+        rospy.loginfo("log file %s", last_log)
+        data = px4tools.ulog.read_ulog(last_log).resample_and_concat(0.1)
+        res = px4tools.estimator_analysis(data, False)
+
+        # enforce performance
+        self.assertTrue(abs(res['roll_error_mean'])  < 5.0, str(res))
+        self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res))
+        self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res))
+        self.assertTrue(res['roll_error_std'] < 5.0, str(res))
+        self.assertTrue(res['pitch_error_std'] < 5.0, str(res))
+        self.assertTrue(res['yaw_error_std'] < 5.0, str(res))
 
 if __name__ == '__main__':
     import rostest

integrationtests/python_src/px4_it/mavros/multirotor_box.mission

+{
+    "MAV_AUTOPILOT": 12,
+    "complexItems": [
+    ],
+    "groundStation": "QGroundControl",
+    "items": [
+        {
+            "autoContinue": true,
+            "command": 22,
+            "coordinate": [
+                47.397739410400391,
+                8.5455904006958008,
+                5
+            ],
+            "frame": 3,
+            "id": 1,
+            "param1": 15,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        },
+        {
+            "autoContinue": true,
+            "command": 16,
+            "coordinate": [
+                47.397872924804688,
+                8.54559326171875,
+                5
+            ],
+            "frame": 3,
+            "id": 2,
+            "param1": 0,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        },
+        {
+            "autoContinue": true,
+            "command": 16,
+            "coordinate": [
+                47.397872924804688,
+                8.5453414916992188,
+                5
+            ],
+            "frame": 3,
+            "id": 3,
+            "param1": 0,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        },
+        {
+            "autoContinue": true,
+            "command": 16,
+            "coordinate": [
+                47.397739410400391,
+                8.5453367233276367,
+                5
+            ],
+            "frame": 3,
+            "id": 4,
+            "param1": 0,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        },
+        {
+            "autoContinue": true,
+            "command": 16,
+            "coordinate": [
+                47.397739410400391,
+                8.5455904006958008,
+                5
+            ],
+            "frame": 3,
+            "id": 5,
+            "param1": 0,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        },
+        {
+            "autoContinue": true,
+            "command": 21,
+            "coordinate": [
+                47.397739410400391,
+                8.5455913543701172,
+                5
+            ],
+            "frame": 3,
+            "id": 6,
+            "param1": 0,
+            "param2": 0,
+            "param3": 0,
+            "param4": 0,
+            "type": "missionItem"
+        }
+    ],
+    "plannedHomePosition": {
+        "autoContinue": true,
+        "command": 16,
+        "coordinate": [
+            47.397806167602539,
+            8.5454649925231934,
+            0
+        ],
+        "frame": 0,
+        "id": 0,
+        "param1": 0,
+        "param2": 0,
+        "param3": 0,
+        "param4": 0,
+        "type": "missionItem"
+    },
+    "version": "1.0"
+}

integrationtests/run_tests.bash

@@ -5,6 +5,9 @@
 # License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 set -e
 
+# TODO move to docker image
+pip install px4tools pymavlink -q
+
 # handle cleaning command
 do_clean=true
 if [ "$1" = "-o" ]
@@ -39,8 +42,7 @@ fi
 export ROS_HOME=$JOB_DIR/.ros
 export ROS_LOG_DIR=$ROS_HOME/log
 export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
-
-PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
+export PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
 TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
 
 # TODO

launch/mavros_posix_tests_iris.launch

@@ -15,7 +15,10 @@
     </include>
 
     <group ns="$(arg ns)">
+        <test test-name="box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.mission" />
         <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" />
         <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
     </group>
 </launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

launch/mavros_posix_tests_standard_vtol.launch

@@ -22,3 +22,5 @@
         <test test-name="mission_test_old_3" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_3.txt" />
     </group>
 </launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->