forked from Archive/PX4-Autopilot
Added ground truth tests to sitl gazebo CI.
This commit is contained in:
parent
d150b4b084
commit
8fdd392700
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
}
|
|
@ -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
|
||||
|
|
|
@ -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 : -->
|
||||
|
|
|
@ -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 : -->
|
||||
|
|
Loading…
Reference in New Issue