Skip to content

Commit e4fb17f

Browse files
committed
test: replace px4tools with pyulog for mission estimator analysis
px4tools is abandoned (last commit 2022) and its DataFrame handling triggers a pandas ValueError where 'timestamp' is both an index level and column label. This broke the MAVROS Mission post-flight analysis. Replace the three px4tools calls (read_ulog, compute_data, estimator_analysis) with a single analyze_estimator_attitude() function that uses pyulog directly with vectorized numpy quaternion-to-euler conversion. The six attitude error assertions are unchanged. Also switch the CI pip install from ad-hoc package list to Tools/setup/requirements.txt for consistency with the rest of PX4.
1 parent c447f85 commit e4fb17f

File tree

2 files changed

+47
-5
lines changed

2 files changed

+47
-5
lines changed

.github/workflows/ci-orchestrator.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1075,7 +1075,7 @@ jobs:
10751075
run: git config --system --add safe.directory '*'
10761076

10771077
- name: Setup - Install Python Test Dependencies
1078-
run: pip3 install pymavlink px4tools "pandas<2"
1078+
run: pip3 install -r Tools/setup/requirements.txt
10791079

10801080
- name: Cache - Restore ccache
10811081
uses: actions/cache/restore@v4

integrationtests/python_src/px4_it/mavros/mission_test.py

Lines changed: 46 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,8 @@
4343
import json
4444
import math
4545
import os
46-
from px4tools import ulog
46+
import numpy as np
47+
from pyulog import ULog
4748
import sys
4849
from mavros import mavlink
4950
from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached
@@ -65,6 +66,49 @@ def get_last_log():
6566
return last_log
6667

6768

69+
def analyze_estimator_attitude(log_file):
70+
"""Compute attitude estimator error metrics from a ULog file."""
71+
ulog = ULog(log_file)
72+
73+
att = ulog.get_dataset('vehicle_attitude').data
74+
att_gt = ulog.get_dataset('vehicle_attitude_groundtruth').data
75+
76+
def quat_to_euler(q0, q1, q2, q3):
77+
"""Quaternion (w,x,y,z) to (roll, pitch, yaw) via ZYX Tait-Bryan."""
78+
roll = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
79+
sinp = 2 * (q0 * q2 - q3 * q1)
80+
pitch = np.where(np.abs(sinp) >= 1,
81+
np.copysign(np.pi / 2, sinp), np.arcsin(sinp))
82+
yaw = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
83+
return roll, pitch, yaw
84+
85+
roll, pitch, yaw = quat_to_euler(
86+
att['q[0]'], att['q[1]'], att['q[2]'], att['q[3]'])
87+
roll_gt, pitch_gt, yaw_gt = quat_to_euler(
88+
att_gt['q[0]'], att_gt['q[1]'], att_gt['q[2]'], att_gt['q[3]'])
89+
90+
# interpolate groundtruth onto attitude timestamps
91+
ts = att['timestamp']
92+
ts_gt = att_gt['timestamp']
93+
roll_gt = np.interp(ts, ts_gt, roll_gt)
94+
pitch_gt = np.interp(ts, ts_gt, pitch_gt)
95+
yaw_gt = np.interp(ts, ts_gt, yaw_gt)
96+
97+
wrap = lambda x: np.arcsin(np.sin(x))
98+
e_roll = wrap(roll - roll_gt)
99+
e_pitch = wrap(pitch - pitch_gt)
100+
e_yaw = wrap(yaw - yaw_gt)
101+
102+
return {
103+
'roll_error_mean': np.rad2deg(np.mean(e_roll)),
104+
'pitch_error_mean': np.rad2deg(np.mean(e_pitch)),
105+
'yaw_error_mean': np.rad2deg(np.mean(e_yaw)),
106+
'roll_error_std': np.rad2deg(np.std(e_roll)),
107+
'pitch_error_std': np.rad2deg(np.std(e_pitch)),
108+
'yaw_error_std': np.rad2deg(np.std(e_yaw)),
109+
}
110+
111+
68112
def read_mission(mission_filename):
69113
wps = []
70114
with open(mission_filename, 'r') as f:
@@ -290,9 +334,7 @@ def test_mission(self):
290334
rospy.loginfo("mission done, calculating performance metrics")
291335
last_log = get_last_log()
292336
rospy.loginfo("log file {0}".format(last_log))
293-
data = ulog.read_ulog(last_log).concat(dt=0.1)
294-
data = ulog.compute_data(data)
295-
res = ulog.estimator_analysis(data, False)
337+
res = analyze_estimator_attitude(last_log)
296338

297339
# enforce performance
298340
self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res))

0 commit comments

Comments
 (0)