mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-01 04:01:37 +08:00
refactor ecl ekf analysis (#11412)
* refactor ekf analysis part 1: move plotting to functions * add plot_check_flags plot function * put plots in seperate file * use object-oriented programming for plotting * move functions for post processing and pdf report creation to new files * add in_air_detector and description as a csv file * refactor metrics and checks into separate functions * refactor metrics into seperate file, seperate plotting * ecl-ekf tools: re-structure folder and move results table generation * ecl-ekf-tool: fix imports and test_results_table * ecl-ekf tools: bugfix output observer tracking error plot * ecl-ekf-tools: update batch processing to new api, fix exception handling * ecl-ekf-tools: use correct in_air_detector * ecl-ekf-tools: rename csv file containing the bare test results table * ecl-tools: refactor for improving readability * ecl-ekf tools: small plotting bugfixes * ecl-ekf tools: small bugfixes in_air time, on_ground_trans, filenames * ecl-ekf-tools: fix amber metric bug * ecl-ekf-tools: remove custom function in inairdetector * ecl-ekf-tools: remove import of pandas * ecl-ekf-tools: add python interpreter to the script start * ecl-ekf-tools pdf_report: fix python interpreter line * px4-dev-ros-kinetic: update container tag to 2019-02-13 * ecl-ekf-tools python interpreter line: call python3 bin directly * ecl-ekf-tools: change airtime from namedtuple to class for python 3.5 * ecl-ekf-tools: update docker image px4-dev-ros-kinetic * ecl-ekf-tools: fix memory leak by correctly closing matplotlib figures
This commit is contained in:
JohannesBrand
@@ -1,13 +1,18 @@
|
||||
#! /usr/bin/env python
|
||||
#! /usr/bin/env python3
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import os, sys
|
||||
import os
|
||||
import sys
|
||||
import csv
|
||||
from typing import Dict
|
||||
|
||||
from pyulog import *
|
||||
from pyulog import ULog
|
||||
|
||||
from analyse_logdata_ekf import analyse_ekf
|
||||
from plotting.pdf_report import create_pdf_report
|
||||
from analysis.detectors import PreconditionError
|
||||
|
||||
"""
|
||||
Performs a health assessment on the ecl EKF navigation estimator data contained in a an ULog file
|
||||
@@ -15,95 +20,146 @@ Outputs a health assessment summary in a csv file named <inputfilename>.mdat.csv
|
||||
Outputs summary plots in a pdf file named <inputfilename>.pdf
|
||||
"""
|
||||
|
||||
parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data')
|
||||
parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
|
||||
parser.add_argument('--no-plots', action='store_true',
|
||||
help='Whether to only analyse and not plot the summaries for developers.')
|
||||
parser.add_argument('--check-level-thresholds', type=str, default=None,
|
||||
help='The csv file of fail and warning test thresholds for analysis.')
|
||||
parser.add_argument('--no-sensor-safety-margin', action='store_true',
|
||||
help='Whether to not cut-off 5s after take-off and 5s before landing '
|
||||
'(for certain sensors that might be influence by proximity to ground).')
|
||||
def get_arguments():
|
||||
parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data')
|
||||
parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
|
||||
parser.add_argument('--no-plots', action='store_true',
|
||||
help='Whether to only analyse and not plot the summaries for developers.')
|
||||
parser.add_argument('--check-level-thresholds', type=str, default=None,
|
||||
help='The csv file of fail and warning test thresholds for analysis.')
|
||||
parser.add_argument('--check-table', type=str, default=None,
|
||||
help='The csv file with descriptions of the checks.')
|
||||
parser.add_argument('--no-sensor-safety-margin', action='store_true',
|
||||
help='Whether to not cut-off 5s after take-off and 5s before landing '
|
||||
'(for certain sensors that might be influence by proximity to ground).')
|
||||
return parser.parse_args()
|
||||
|
||||
def is_valid_directory(parser, arg):
|
||||
if os.path.isdir(arg):
|
||||
# Directory exists so return the directory
|
||||
return arg
|
||||
|
||||
def create_results_table(
|
||||
check_table_filename: str, master_status: str, check_status: Dict[str, str],
|
||||
metrics: Dict[str, float], airtime_info: Dict[str, float]) -> Dict[str, list]:
|
||||
"""
|
||||
creates the output results table
|
||||
:param check_table_filename:
|
||||
:param master_status:
|
||||
:param check_status:
|
||||
:param metrics:
|
||||
:param airtime_info:
|
||||
:return:
|
||||
"""
|
||||
|
||||
try:
|
||||
with open(check_table_filename, 'r') as file:
|
||||
reader = csv.DictReader(file)
|
||||
test_results_table = {
|
||||
row['check_id']: [float('NaN'), row['check_description']] for row in reader}
|
||||
print('Using test description loaded from {:s}'.format(check_table_filename))
|
||||
except:
|
||||
raise PreconditionError('could not find {:s}'.format(check_table_filename))
|
||||
|
||||
# store metrics
|
||||
for key, value in metrics.items():
|
||||
test_results_table[key][0] = value
|
||||
|
||||
# store check results
|
||||
for key, value in check_status.items():
|
||||
test_results_table[key][0] = value
|
||||
|
||||
# store check results
|
||||
for key, value in test_results_table.items():
|
||||
if key.endswith('_status'):
|
||||
test_results_table[key][0] = str(value[0])
|
||||
|
||||
# store master status
|
||||
test_results_table['master_status'][0] = master_status
|
||||
|
||||
# store take_off and landing information
|
||||
test_results_table['in_air_transition_time'][0] = airtime_info['in_air_transition_time']
|
||||
test_results_table['on_ground_transition_time'][0] = airtime_info['on_ground_transition_time']
|
||||
|
||||
return test_results_table
|
||||
|
||||
|
||||
def process_logdata_ekf(
|
||||
filename: str, check_level_dict_filename: str, check_table_filename: str,
|
||||
plot: bool = True, sensor_safety_margins: bool = True):
|
||||
|
||||
## load the log and extract the necessary data for the analyses
|
||||
try:
|
||||
ulog = ULog(filename)
|
||||
except:
|
||||
raise PreconditionError('could not open {:s}'.format(filename))
|
||||
|
||||
try:
|
||||
# get the dictionary of fail and warning test thresholds from a csv file
|
||||
with open(check_level_dict_filename, 'r') as file:
|
||||
reader = csv.DictReader(file)
|
||||
check_levels = {row['check_id']: float(row['threshold']) for row in reader}
|
||||
print('Using test criteria loaded from {:s}'.format(check_level_dict_filename))
|
||||
except:
|
||||
raise PreconditionError('could not find {:s}'.format(check_level_dict_filename))
|
||||
|
||||
in_air_margin = 5.0 if sensor_safety_margins else 0.0
|
||||
# perform the ekf analysis
|
||||
master_status, check_status, metrics, airtime_info = analyse_ekf(
|
||||
ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0,
|
||||
in_air_margin_seconds=in_air_margin)
|
||||
|
||||
test_results = create_results_table(
|
||||
check_table_filename, master_status, check_status, metrics, airtime_info)
|
||||
|
||||
# write metadata to a .csv file
|
||||
with open('{:s}.mdat.csv'.format(filename), "w") as file:
|
||||
|
||||
file.write("name,value,description\n")
|
||||
|
||||
# loop through the test results dictionary and write each entry on a separate row, with data comma separated
|
||||
# save data in alphabetical order
|
||||
key_list = list(test_results.keys())
|
||||
key_list.sort()
|
||||
for key in key_list:
|
||||
file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n")
|
||||
print('Test results written to {:s}.mdat.csv'.format(filename))
|
||||
|
||||
if plot:
|
||||
create_pdf_report(ulog, '{:s}.pdf'.format(filename))
|
||||
print('Plots saved to {:s}.pdf'.format(filename))
|
||||
|
||||
return test_results
|
||||
|
||||
|
||||
def main() -> None:
|
||||
|
||||
args = get_arguments()
|
||||
|
||||
if args.check_level_thresholds is not None:
|
||||
check_level_dict_filename = args.check_level_thresholds
|
||||
else:
|
||||
parser.error('The directory {} does not exist'.format(arg))
|
||||
file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__)))
|
||||
check_level_dict_filename = os.path.join(file_dir, "check_level_dict.csv")
|
||||
|
||||
args = parser.parse_args()
|
||||
if args.check_table is not None:
|
||||
check_table_filename = args.check_table
|
||||
else:
|
||||
file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__)))
|
||||
check_table_filename = os.path.join(file_dir, "check_table.csv")
|
||||
|
||||
## load the log and extract the necessary data for the analyses
|
||||
ulog = ULog(args.filename, None)
|
||||
data = ulog.data_list
|
||||
try:
|
||||
test_results = process_logdata_ekf(
|
||||
args.filename, check_level_dict_filename, check_table_filename,
|
||||
plot=not args.no_plots, sensor_safety_margins=not args.no_sensor_safety_margin)
|
||||
except Exception as e:
|
||||
print(str(e))
|
||||
sys.exit(-1)
|
||||
|
||||
# extract data from EKF status message
|
||||
estimator_status_data = {}
|
||||
try:
|
||||
estimator_status_data = ulog.get_dataset('estimator_status').data;
|
||||
except (KeyError, IndexError, ValueError) as error:
|
||||
print(type(error), "(estimator_status):", error)
|
||||
# print master test status to console
|
||||
if (test_results['master_status'][0] == 'Pass'):
|
||||
print('No anomalies detected')
|
||||
elif (test_results['master_status'][0] == 'Warning'):
|
||||
print('Minor anomalies detected')
|
||||
elif (test_results['master_status'][0] == 'Fail'):
|
||||
print('Major anomalies detected')
|
||||
sys.exit(-1)
|
||||
|
||||
# extract data from EKF innovations message
|
||||
ekf2_innovations_data = {}
|
||||
try:
|
||||
ekf2_innovations_data = ulog.get_dataset('ekf2_innovations').data;
|
||||
except (KeyError, IndexError, ValueError) as error:
|
||||
print(type(error), "(ekf2_innovations):", error)
|
||||
|
||||
# extract data from sensor preflight check message
|
||||
sensor_preflight_data = {}
|
||||
try:
|
||||
sensor_preflight_data = ulog.get_dataset('sensor_preflight').data;
|
||||
except (KeyError, IndexError, ValueError) as error:
|
||||
print(type(error), "(sensor_preflight):", error)
|
||||
|
||||
if args.check_level_thresholds:
|
||||
check_level_dict_filename = args.check_level_thresholds
|
||||
else:
|
||||
file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__)))
|
||||
check_level_dict_filename = os.path.join(file_dir, "check_level_dict.csv")
|
||||
|
||||
# get the dictionary of fail and warning test thresholds from a csv file
|
||||
with open(check_level_dict_filename, 'r') as file:
|
||||
check_levels = {}
|
||||
for line in file:
|
||||
x = line.split(",")
|
||||
a = x[0]
|
||||
b = x[1]
|
||||
check_levels[a] = float(b)
|
||||
|
||||
print('Using test criteria loaded from {:s}'.format(check_level_dict_filename))
|
||||
|
||||
# perform the ekf analysis
|
||||
test_results = analyse_ekf(
|
||||
estimator_status_data, ekf2_innovations_data, sensor_preflight_data,
|
||||
check_levels, plot=not args.no_plots, output_plot_filename=args.filename + ".pdf",
|
||||
late_start_early_ending=not args.no_sensor_safety_margin)
|
||||
|
||||
# write metadata to a .csv file
|
||||
with open(args.filename + ".mdat.csv", "w") as file:
|
||||
|
||||
file.write("name,value,description\n")
|
||||
|
||||
# loop through the test results dictionary and write each entry on a separate row, with data comma separated
|
||||
# save data in alphabetical order
|
||||
key_list = list(test_results.keys())
|
||||
key_list.sort()
|
||||
for key in key_list:
|
||||
file.write(key+","+str(test_results[key][0])+","+test_results[key][1]+"\n")
|
||||
|
||||
print('Test results written to {:s}.mdat.csv'.format(args.filename))
|
||||
|
||||
if not args.no_plots:
|
||||
print('Plots saved to {:s}.pdf'.format(args.filename))
|
||||
|
||||
# print master test status to console
|
||||
if (test_results['master_status'][0] == 'Pass'):
|
||||
print('No anomalies detected')
|
||||
elif (test_results['master_status'][0] == 'Warning'):
|
||||
print('Minor anomalies detected')
|
||||
elif (test_results['master_status'][0] == 'Fail'):
|
||||
print('Major anomalies detected')
|
||||
sys.exit(-1)
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user