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 sys
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
|
import glob
|
||||||
|
import json
|
||||||
|
|
||||||
import mavros
|
import mavros
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
from mavros import mavlink
|
from mavros import mavlink
|
||||||
|
|
||||||
|
import px4tools
|
||||||
|
|
||||||
from geometry_msgs.msg import PoseStamped
|
from geometry_msgs.msg import PoseStamped
|
||||||
from mavros_msgs.srv import CommandLong, WaypointPush
|
from mavros_msgs.srv import CommandLong, WaypointPush
|
||||||
from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
|
from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
|
||||||
|
@ -60,6 +64,35 @@ from sensor_msgs.msg import NavSatFix
|
||||||
from mavros.mission import QGroundControlWP
|
from mavros.mission import QGroundControlWP
|
||||||
#from px4_test_helper import PX4TestHelper
|
#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):
|
class MavrosMissionTest(unittest.TestCase):
|
||||||
"""
|
"""
|
||||||
Run a mission
|
Run a mission
|
||||||
|
@ -276,11 +309,23 @@ class MavrosMissionTest(unittest.TestCase):
|
||||||
mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
|
mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
|
||||||
|
|
||||||
rospy.loginfo("reading mission %s", mission_file)
|
rospy.loginfo("reading mission %s", mission_file)
|
||||||
mission = QGroundControlWP()
|
|
||||||
wps = []
|
wps = []
|
||||||
for waypoint in mission.read(open(mission_file, 'r')):
|
|
||||||
wps.append(waypoint)
|
with open(mission_file, 'r') as f:
|
||||||
rospy.logdebug(waypoint)
|
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")
|
rospy.loginfo("wait until ready")
|
||||||
self.wait_until_ready()
|
self.wait_until_ready()
|
||||||
|
@ -321,6 +366,19 @@ class MavrosMissionTest(unittest.TestCase):
|
||||||
|
|
||||||
index += 1
|
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__':
|
if __name__ == '__main__':
|
||||||
import rostest
|
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
|
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
|
||||||
set -e
|
set -e
|
||||||
|
|
||||||
|
# TODO move to docker image
|
||||||
|
pip install px4tools pymavlink -q
|
||||||
|
|
||||||
# handle cleaning command
|
# handle cleaning command
|
||||||
do_clean=true
|
do_clean=true
|
||||||
if [ "$1" = "-o" ]
|
if [ "$1" = "-o" ]
|
||||||
|
@ -39,8 +42,7 @@ fi
|
||||||
export ROS_HOME=$JOB_DIR/.ros
|
export ROS_HOME=$JOB_DIR/.ros
|
||||||
export ROS_LOG_DIR=$ROS_HOME/log
|
export ROS_LOG_DIR=$ROS_HOME/log
|
||||||
export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
|
export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
|
||||||
|
export PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
|
||||||
PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
|
|
||||||
TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
|
TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
|
||||||
|
|
||||||
# TODO
|
# TODO
|
||||||
|
|
|
@ -15,7 +15,10 @@
|
||||||
</include>
|
</include>
|
||||||
|
|
||||||
<group ns="$(arg ns)">
|
<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_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" />
|
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
|
||||||
</group>
|
</group>
|
||||||
</launch>
|
</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" />
|
<test test-name="mission_test_old_3" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_3.txt" />
|
||||||
</group>
|
</group>
|
||||||
</launch>
|
</launch>
|
||||||
|
|
||||||
|
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
|
||||||
|
|
Loading…
Reference in New Issue