diff --git a/.github/workflows/ekf_update_change_indicator.yml b/.github/workflows/ekf_update_change_indicator.yml index bfccf8f9de..b01af805b6 100644 --- a/.github/workflows/ekf_update_change_indicator.yml +++ b/.github/workflows/ekf_update_change_indicator.yml @@ -17,7 +17,7 @@ jobs: GIT_COMMITTER_NAME: PX4BuildBot steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 with: fetch-depth: 0 @@ -39,7 +39,7 @@ jobs: - name: auto-commit any changes to change indication if: steps.diff-check.outputs.CHANGE_INDICATED == 'true' - uses: stefanzweifel/git-auto-commit-action@v4 + uses: stefanzweifel/git-auto-commit-action@v7 with: file_pattern: 'src/modules/ekf2/test/change_indication/*.csv' commit_user_name: ${{ env.GIT_COMMITTER_NAME }} diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml deleted file mode 100644 index 0e7a4fe17c..0000000000 --- a/.github/workflows/mavros_mission_tests.yml +++ /dev/null @@ -1,47 +0,0 @@ -name: MAVROS Mission Tests - -on: - push: - branches: - - 'main' - paths-ignore: - - 'docs/**' - pull_request: - branches: - - '**' - paths-ignore: - - 'docs/**' - -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: [runs-on,runner=4cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false,extras=s3-cache] - - strategy: - fail-fast: false - - steps: - - uses: runs-on/action@v2 - - - uses: actions/checkout@v4 - with: - fetch-depth: 1 - - - name: Build SITL and Run Tests (inside old ROS container) - run: | - docker run --rm \ - -v "${GITHUB_WORKSPACE}:/workspace" \ - -w /workspace \ - px4io/px4-dev-ros-melodic:2021-09-08 \ - bash -c ' - git config --global --add safe.directory /workspace - PX4_SBOM_DISABLE=1 make px4_sitl_default - PX4_SBOM_DISABLE=1 make px4_sitl_default sitl_gazebo-classic - ./test/rostest_px4_run.sh \ - mavros_posix_test_mission.test \ - mission:=MC_mission_box \ - vehicle:=iris - ' diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml deleted file mode 100644 index e0835a905b..0000000000 --- a/.github/workflows/mavros_offboard_tests.yml +++ /dev/null @@ -1,46 +0,0 @@ -name: MAVROS Offboard Tests - -on: - push: - branches: - - 'main' - paths-ignore: - - 'docs/**' - pull_request: - branches: - - '**' - paths-ignore: - - 'docs/**' - -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: [runs-on,runner=4cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false,extras=s3-cache] - - strategy: - fail-fast: false - - steps: - - uses: runs-on/action@v2 - - - uses: actions/checkout@v4 - with: - fetch-depth: 1 - - - name: Build SITL and Run Tests (inside old ROS container) - run: | - docker run --rm \ - -v "${GITHUB_WORKSPACE}:/workspace" \ - -w /workspace \ - px4io/px4-dev-ros-melodic:2021-09-08 \ - bash -c ' - git config --global --add safe.directory /workspace - PX4_SBOM_DISABLE=1 make px4_sitl_default - PX4_SBOM_DISABLE=1 make px4_sitl_default sitl_gazebo-classic - ./test/rostest_px4_run.sh \ - mavros_posix_tests_offboard_posctl.test \ - vehicle:=iris - ' diff --git a/.github/workflows/mavros_tests.yml b/.github/workflows/mavros_tests.yml new file mode 100644 index 0000000000..a2bff180ef --- /dev/null +++ b/.github/workflows/mavros_tests.yml @@ -0,0 +1,75 @@ +name: MAVROS Tests + +on: + push: + branches: + - 'main' + paths-ignore: + - 'docs/**' + pull_request: + branches: + - '**' + paths-ignore: + - 'docs/**' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + test: + name: "MAVROS ${{ matrix.config.name }}" + runs-on: [runs-on,runner=4cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false,extras=s3-cache] + permissions: + contents: read + container: + image: px4io/px4-dev-ros-noetic:2021-09-08 + env: + PX4_SBOM_DISABLE: 1 + strategy: + fail-fast: false + matrix: + config: + - {name: "Mission", test_file: "mavros_posix_test_mission.test", params: "mission:=MC_mission_box vehicle:=iris"} + - {name: "Offboard", test_file: "mavros_posix_tests_offboard_posctl.test", params: "vehicle:=iris"} + steps: + - uses: runs-on/action@v2 + + - uses: actions/checkout@v6 + with: + fetch-depth: 1 + + - name: Setup - Configure Git Safe Directory + run: git config --system --add safe.directory '*' + + - name: Setup - Install Python Test Dependencies + run: pip3 install -r Tools/setup/requirements.txt + + - uses: ./.github/actions/setup-ccache + id: ccache + with: + cache-key-prefix: ccache-sitl-gazebo-classic + max-size: 350M + + - uses: ./.github/actions/build-gazebo-sitl + + - name: Test - MAVROS ${{ matrix.config.name }} + run: | + ./test/rostest_px4_run.sh \ + ${{ matrix.config.test_file }} \ + ${{ matrix.config.params }} + timeout-minutes: 10 + + - uses: ./.github/actions/save-ccache + if: always() + with: + cache-primary-key: ${{ steps.ccache.outputs.cache-primary-key }} + + - name: Upload - Failed Test Logs + if: failure() + uses: actions/upload-artifact@v7 + with: + name: failed-mavros-${{ matrix.config.name }}-logs.zip + path: | + logs/**/**/**/*.log + logs/**/**/**/*.ulg diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py index 5e791be7b5..4432e466e7 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py +++ b/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 # -# 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): diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py index 81c6835a2b..8e88bf608b 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py +++ b/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 # -# 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) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py index f40c45b17f..88b646a19b 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py +++ b/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 # -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 diff --git a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py index 642d669335..ee5a95d438 100644 --- a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py +++ b/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: diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 0692f11ec1..c4134cff00 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/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 # -# 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))