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