ci(mavros): merge mission+offboard into one workflow, migrate to noetic and Python 3

Consolidate mavros_mission_tests.yml and mavros_offboard_tests.yml into a
single mavros_tests.yml with a matrix strategy. Switch from docker-in-docker
with px4-dev-ros-melodic to a native container using px4-dev-ros-noetic,
enabling ccache and composite actions (setup-ccache, build-gazebo-sitl,
save-ccache). Migrate all five MAVROS Python test files from Python 2 to
Python 3 (remove six/xrange, from __future__ imports, replace px4tools
with pyulog for estimator analysis). Bump git-auto-commit-action from v4
to v7 in ekf_update_change_indicator.yml.

Signed-off-by: Ramon Roche <mrpollo@gmail.com>

integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,9 +35,6 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
-from __future__ import division
 
 PKG = 'px4'
 
@@ -46,7 +43,6 @@ from geometry_msgs.msg import Quaternion, Vector3
 from mavros_msgs.msg import AttitudeTarget
 from mavros_test_common import MavrosTestCommon
 from pymavlink import mavutil
-from six.moves import xrange
 from std_msgs.msg import Header
 from threading import Thread
 from tf.transformations import quaternion_from_euler
@@ -124,7 +120,7 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
         loop_freq = 2  # Hz
         rate = rospy.Rate(loop_freq)
         crossed = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if (self.local_position.pose.position.x > boundary_x and
                     self.local_position.pose.position.y > boundary_y and
                     self.local_position.pose.position.z > boundary_z):

integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,9 +35,6 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
-from __future__ import division
 
 PKG = 'px4'
 
@@ -48,7 +45,6 @@ from geometry_msgs.msg import PoseStamped, Quaternion
 from mavros_msgs.msg import ParamValue
 from mavros_test_common import MavrosTestCommon
 from pymavlink import mavutil
-from six.moves import xrange
 from std_msgs.msg import Header
 from threading import Thread
 from tf.transformations import quaternion_from_euler
@@ -132,7 +128,7 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
         loop_freq = 2  # Hz
         rate = rospy.Rate(loop_freq)
         reached = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if self.is_at_position(self.pos.pose.position.x,
                                    self.pos.pose.position.y,
                                    self.pos.pose.position.z, self.radius):
@@ -174,7 +170,7 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
         positions = ((0, 0, 0), (50, 50, 20), (50, -50, 20), (-50, -50, 20),
                      (0, 0, 20))
 
-        for i in xrange(len(positions)):
+        for i in range(len(positions)):
             self.reach_position(positions[i][0], positions[i][1],
                                 positions[i][2], 30)
 

integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 #***************************************************************************
 #
 #   Copyright (c) 2020 PX4 Development Team. All rights reserved.
@@ -35,8 +35,6 @@
 # @author Pedro Roque <padr@kth.se>
 #
 
-from __future__ import division
-
 PKG = 'px4'
 
 import rospy
@@ -44,7 +42,6 @@ from geometry_msgs.msg import Quaternion, Vector3
 from mavros_msgs.msg import AttitudeTarget
 from mavros_test_common import MavrosTestCommon
 from pymavlink import mavutil
-from six.moves import xrange
 from std_msgs.msg import Header
 from threading import Thread
 from tf.transformations import quaternion_from_euler
@@ -134,7 +131,7 @@ class MavrosOffboardYawrateTest(MavrosTestCommon):
         loop_freq = 2  # Hz
         rate = rospy.Rate(loop_freq)
         crossed = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if (self.local_position.pose.position.x < boundary_x and
 	    	    self.local_position.pose.position.x > -boundary_x and
                     self.local_position.pose.position.y < boundary_y and

integrationtests/python_src/px4_it/mavros/mavros_test_common.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python2
-from __future__ import division
+#!/usr/bin/env python3
 
 import unittest
 import rospy
@@ -11,7 +10,6 @@ from mavros_msgs.srv import CommandBool, ParamGet, ParamSet, SetMode, SetModeReq
                             WaypointPush
 from pymavlink import mavutil
 from sensor_msgs.msg import NavSatFix, Imu
-from six.moves import xrange
 
 
 class MavrosTestCommon(unittest.TestCase):
@@ -183,7 +181,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         arm_set = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if self.state.armed == arm:
                 arm_set = True
                 rospy.loginfo("set arm success | seconds: {0} of {1}".format(
@@ -213,7 +211,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         mode_set = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if self.state.mode == mode:
                 mode_set = True
                 rospy.loginfo("set mode success | seconds: {0} of {1}".format(
@@ -247,7 +245,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         param_set = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             try:
                 res = self.set_param_srv(param_id, param_value)
                 if res.success:
@@ -274,7 +272,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         simulation_ready = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if all(value for value in self.sub_topics_ready.values()):
                 simulation_ready = True
                 rospy.loginfo("simulation topics ready | seconds: {0} of {1}".
@@ -297,7 +295,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 10  # Hz
         rate = rospy.Rate(loop_freq)
         landed_state_confirmed = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if self.extended_state.landed_state == desired_landed_state:
                 landed_state_confirmed = True
                 rospy.loginfo("landed state confirmed | seconds: {0} of {1}".
@@ -325,7 +323,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 10  # Hz
         rate = rospy.Rate(loop_freq)
         transitioned = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if transition == self.extended_state.vtol_state:
                 rospy.loginfo("transitioned | seconds: {0} of {1}".format(
                     i / loop_freq, timeout))
@@ -348,7 +346,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         wps_cleared = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if not self.mission_wp.waypoints:
                 wps_cleared = True
                 rospy.loginfo("clear waypoints success | seconds: {0} of {1}".
@@ -381,7 +379,7 @@ class MavrosTestCommon(unittest.TestCase):
         rate = rospy.Rate(loop_freq)
         wps_sent = False
         wps_verified = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             if not wps_sent:
                 try:
                     res = self.wp_push_srv(start_index=0, waypoints=waypoints)
@@ -417,7 +415,7 @@ class MavrosTestCommon(unittest.TestCase):
         loop_freq = 1  # Hz
         rate = rospy.Rate(loop_freq)
         res = False
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             try:
                 res = self.get_param_srv('MAV_TYPE')
                 if res.success:

integrationtests/python_src/px4_it/mavros/mission_test.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 #***************************************************************************
 #
 #   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
@@ -36,10 +36,6 @@
 # @author Andreas Antener <andreas@uaventure.com>
 #
 
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
-from __future__ import division
-
 PKG = 'px4'
 
 import rospy
@@ -47,13 +43,13 @@ import glob
 import json
 import math
 import os
-from px4tools import ulog
+import numpy as np
+from pyulog import ULog
 import sys
 from mavros import mavlink
 from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached
 from mavros_test_common import MavrosTestCommon
 from pymavlink import mavutil
-from six.moves import xrange
 from threading import Thread
 
 
@@ -70,6 +66,49 @@ def get_last_log():
     return last_log
 
 
+def analyze_estimator_attitude(log_file):
+    """Compute attitude estimator error metrics from a ULog file."""
+    ulog = ULog(log_file)
+
+    att = ulog.get_dataset('vehicle_attitude').data
+    att_gt = ulog.get_dataset('vehicle_attitude_groundtruth').data
+
+    def quat_to_euler(q0, q1, q2, q3):
+        """Quaternion (w,x,y,z) to (roll, pitch, yaw) via ZYX Tait-Bryan."""
+        roll = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+        sinp = 2 * (q0 * q2 - q3 * q1)
+        pitch = np.where(np.abs(sinp) >= 1,
+                         np.copysign(np.pi / 2, sinp), np.arcsin(sinp))
+        yaw = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+        return roll, pitch, yaw
+
+    roll, pitch, yaw = quat_to_euler(
+        att['q[0]'], att['q[1]'], att['q[2]'], att['q[3]'])
+    roll_gt, pitch_gt, yaw_gt = quat_to_euler(
+        att_gt['q[0]'], att_gt['q[1]'], att_gt['q[2]'], att_gt['q[3]'])
+
+    # interpolate groundtruth onto attitude timestamps
+    ts = att['timestamp']
+    ts_gt = att_gt['timestamp']
+    roll_gt = np.interp(ts, ts_gt, roll_gt)
+    pitch_gt = np.interp(ts, ts_gt, pitch_gt)
+    yaw_gt = np.interp(ts, ts_gt, yaw_gt)
+
+    wrap = lambda x: np.arcsin(np.sin(x))
+    e_roll = wrap(roll - roll_gt)
+    e_pitch = wrap(pitch - pitch_gt)
+    e_yaw = wrap(yaw - yaw_gt)
+
+    return {
+        'roll_error_mean': np.rad2deg(np.mean(e_roll)),
+        'pitch_error_mean': np.rad2deg(np.mean(e_pitch)),
+        'yaw_error_mean': np.rad2deg(np.mean(e_yaw)),
+        'roll_error_std': np.rad2deg(np.std(e_roll)),
+        'pitch_error_std': np.rad2deg(np.std(e_pitch)),
+        'yaw_error_std': np.rad2deg(np.std(e_yaw)),
+    }
+
+
 def read_mission(mission_filename):
     wps = []
     with open(mission_filename, 'r') as f:
@@ -188,7 +227,7 @@ class MavrosMissionTest(MavrosTestCommon):
         # does it reach the position in 'timeout' seconds?
         loop_freq = 2  # Hz
         rate = rospy.Rate(loop_freq)
-        for i in xrange(timeout * loop_freq):
+        for i in range(timeout * loop_freq):
             pos_xy_d, pos_z_d = self.distance_to_wp(lat, lon, alt)
 
             # remember best distances
@@ -295,9 +334,7 @@ class MavrosMissionTest(MavrosTestCommon):
         rospy.loginfo("mission done, calculating performance metrics")
         last_log = get_last_log()
         rospy.loginfo("log file {0}".format(last_log))
-        data = ulog.read_ulog(last_log).concat(dt=0.1)
-        data = ulog.compute_data(data)
-        res = ulog.estimator_analysis(data, False)
+        res = analyze_estimator_attitude(last_log)
 
         # enforce performance
         self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res))