diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 8d5ef8e38c..a0ba0dbc4b 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 0000000000..5fdf7cdb5b --- /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 a74fbd1716..76793f7fd7 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 d482859927..04a79f814e 100644 --- a/launch/mavros_posix_tests_iris.launch +++ b/launch/mavros_posix_tests_iris.launch @@ -15,7 +15,10 @@ + + + diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch index 278bead65f..4ce051cb40 100644 --- a/launch/mavros_posix_tests_standard_vtol.launch +++ b/launch/mavros_posix_tests_standard_vtol.launch @@ -22,3 +22,5 @@ + +