diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 8d5ef8e38c2eced8372ab7d2e8576d472ab3d897..a0ba0dbc4be14610be492394d99f30b586d3eb15 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/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
diff --git a/integrationtests/python_src/px4_it/mavros/multirotor_box.mission b/integrationtests/python_src/px4_it/mavros/multirotor_box.mission
new file mode 100644
index 0000000000000000000000000000000000000000..5fdf7cdb5b2e062fd6fd729bb59b78880a6d5474
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/multirotor_box.mission
@@ -0,0 +1,121 @@
+{
+ "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"
+}
diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index a74fbd1716c7e6e9ff1b281644a68d8309870ca5..76793f7fd77fee0737c0cc69aa24d4c88ed15f24 100755
--- a/integrationtests/run_tests.bash
+++ b/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
diff --git a/launch/mavros_posix_tests_iris.launch b/launch/mavros_posix_tests_iris.launch
index d482859927f8c2873fdfec3ff36da6f5cab63092..04a79f814eedd7aadecf6da535ddcde9a5b78973 100644
--- a/launch/mavros_posix_tests_iris.launch
+++ b/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 : -->
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
index 278bead65f548a3267088857ec728de617a3e96b..4ce051cb40b1edb1cfe370ab77b387a0172550d9 100644
--- a/launch/mavros_posix_tests_standard_vtol.launch
+++ b/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 : -->