Added ground truth tests to sitl gazebo CI.

This commit is contained in:
James Goppert 2016-12-23 22:18:01 -05:00 committed by Lorenz Meier
parent d150b4b084
commit 8fdd392700
5 changed files with 192 additions and 6 deletions

View File

@ -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

View File

@ -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"
}

View File

@ -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

View File

@ -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 : -->

View File

@ -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 : -->