├── .gitignore ├── 1.trans_bin2csv.py ├── 1.trans_bin2csv_px4.py ├── 3.lgfuzzer.py ├── 3.lgfuzzer_px4.py ├── 2.train_lstm.py ├── 2.train_lstm_px4.py ├── 5.range.py ├── 5.range_px4.py ├── 2.extract_feature.py ├── 2.extract_feature_px4.py ├── Cptool ├── fitCollection_px4.txt ├── fitCollection.txt ├── paramManager.py ├── config.yaml ├── mission_px4.txt ├── mission.txt ├── param_px4.json ├── param_ardu.json ├── param_ardu_back.json ├── mavtool.py ├── config.py ├── gaSimManager.py └── gaMavlink.py ├── 2.feature_split.py ├── 2.feature_split_px4.py ├── 4.pre_validate.py ├── 4.pre_validate_px4.py ├── 4.validate_thread.py ├── 4.validate_thread_px4.py ├── 2.raw_split.py ├── 2.raw_split_px4.py ├── LICENSE ├── 0.collect_px4.py ├── 0.collect.py ├── range ├── range.py ├── rangeproblem.py └── rangesearcher.py ├── analysis └── range_analysis.py ├── 4.validate.py ├── 4.validate_px4.py ├── README.md ├── 4.validate_px4_thread_version.py ├── uavga ├── problem.py ├── searcher.py └── fuzzer.py └── ModelFit └── approximate.py /.gitignore: -------------------------------------------------------------------------------- 1 | /result/ 2 | /model/ 3 | /fig/ 4 | -------------------------------------------------------------------------------- /1.trans_bin2csv.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | from Cptool.gaMavlink import GaMavlinkAPM 3 | 4 | if __name__ == '__main__': 5 | GaMavlinkAPM.extract_log_path(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/bin_ga", skip=False) 6 | -------------------------------------------------------------------------------- /1.trans_bin2csv_px4.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | from Cptool.gaMavlink import GaMavlinkPX4 3 | 4 | if __name__ == '__main__': 5 | toolConfig.select_mode("PX4") 6 | GaMavlinkPX4.extract_log_path(f"csv", threat=6) 7 | -------------------------------------------------------------------------------- /3.lgfuzzer.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | from Cptool.config import toolConfig 4 | from uavga.fuzzer import run_fuzzing 5 | 6 | if __name__ == '__main__': 7 | with open(f"model/{toolConfig.MODE}/raw_test.pkl", 'rb') as f: 8 | np_data = pickle.load(f) 9 | run_fuzzing(np_data) 10 | -------------------------------------------------------------------------------- /3.lgfuzzer_px4.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | from Cptool.config import toolConfig 4 | from uavga.fuzzer import run_fuzzing 5 | 6 | if __name__ == '__main__': 7 | toolConfig.select_mode("PX4") 8 | with open(f"model/{toolConfig.MODE}/raw_test.pkl", 'rb') as f: 9 | np_data = pickle.load(f) 10 | run_fuzzing(np_data) 11 | -------------------------------------------------------------------------------- /2.train_lstm.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | 3 | from Cptool.config import toolConfig 4 | from ModelFit.approximate import CyLSTM 5 | 6 | """ 7 | Train LSTM Model 8 | """ 9 | if __name__ == '__main__': 10 | lstm = CyLSTM(100, 512) 11 | # read 12 | feature = pd.read_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_train.csv") 13 | # Train 14 | lstm.train(feature, cuda=True) 15 | -------------------------------------------------------------------------------- /2.train_lstm_px4.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | 3 | from Cptool.config import toolConfig 4 | from ModelFit.approximate import CyLSTM 5 | 6 | """ 7 | Train LSTM Model 8 | """ 9 | if __name__ == '__main__': 10 | toolConfig.select_mode("PX4") 11 | 12 | lstm = CyLSTM(100, 512) 13 | # read 14 | feature = pd.read_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_train.csv") 15 | # Train 16 | lstm.train(feature, cuda=True) 17 | -------------------------------------------------------------------------------- /5.range.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | import pandas as pd 3 | 4 | from range.rangesearcher import GARangeOptimizer 5 | 6 | if __name__ == '__main__': 7 | # The parameters you want to fuzzing, they must be corresponding to the predictor had. 8 | 9 | # Validate Result 10 | result_data = pd.read_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv', header=0).drop(columns="score") 11 | ga = GARangeOptimizer(result_data) 12 | ga.set_bounds() 13 | ga.run() -------------------------------------------------------------------------------- /5.range_px4.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | import pandas as pd 3 | 4 | from range.rangesearcher import GARangeOptimizer 5 | 6 | if __name__ == '__main__': 7 | # The parameters you want to fuzzing, they must be corresponding to the predictor had. 8 | toolConfig.select_mode("PX4") 9 | 10 | result_data = pd.read_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv', header=0).drop(columns="score") 11 | ga = GARangeOptimizer(result_data) 12 | ga.set_bounds() 13 | ga.run() -------------------------------------------------------------------------------- /2.extract_feature.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | from ModelFit.approximate import CyLSTM 3 | 4 | """ 5 | Train LSTM Model 6 | """ 7 | if __name__ == '__main__': 8 | pd_csv = CyLSTM.merge_file_data(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/bin_ga/csv") 9 | CyLSTM.fit_trans(pd_csv) 10 | 11 | lstm = CyLSTM(100, 512) 12 | feature = lstm.extract_feature(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/bin_regular/csv") 13 | # Save 14 | feature.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features.csv", index=False) 15 | -------------------------------------------------------------------------------- /2.extract_feature_px4.py: -------------------------------------------------------------------------------- 1 | from Cptool.config import toolConfig 2 | from ModelFit.approximate import CyLSTM 3 | 4 | """ 5 | Train LSTM Model 6 | """ 7 | if __name__ == '__main__': 8 | toolConfig.select_mode("PX4") 9 | pd_csv = CyLSTM.merge_file_data(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/ulg_changed/csv") 10 | CyLSTM.fit_trans(pd_csv) 11 | 12 | lstm = CyLSTM(100, 512) 13 | feature = lstm.extract_feature(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/ulg_changed/csv") 14 | # Save 15 | feature.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features.csv", index=False) 16 | -------------------------------------------------------------------------------- /Cptool/fitCollection_px4.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 0 3 22 0.00000000 0.00000000 0.00000000 Nan 40.072842 -105.230575 10.000000 1 3 | 1 0 3 16 0.00000000 0.00000000 0.00000000 Nan 40.07460700 -105.23200750 20.000000 1 4 | 2 0 3 16 1.00000000 0.00000000 0.00000000 Nan 40.07251400 -105.23154400 20.000000 1 5 | 3 0 3 16 0.00000000 0.00000000 0.00000000 Nan 40.07251400 -105.23154400 6.000000 1 6 | 4 0 3 16 0.00000000 0.00000000 0.00000000 Nan 40.07254400 -105.23135400 6.000000 1 7 | 5 0 3 16 0.00000000 0.00000000 0.00000000 Nan 40.07260900 -105.23128500 5.000000 1 8 | 6 0 3 21 0.00000000 0.00000000 0.00000000 Nan 40.07284500 -105.23057600 0.000000 1 -------------------------------------------------------------------------------- /Cptool/fitCollection.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 1 0 16 0 0 0 0 40.072842 -105.230575 0.000000 1 3 | 1 0 3 22 0.00000000 0.00000000 0.00000000 0.00000000 40.07460700 -105.23200750 10.000000 1 4 | 2 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 40.07460700 -105.23200750 20.000000 1 5 | 3 0 3 16 1.00000000 0.00000000 0.00000000 0.00000000 40.07251400 -105.23154400 20.000000 1 6 | 4 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 40.07251400 -105.23154400 6.000000 1 7 | 5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 40.07254400 -105.23135400 6.000000 1 8 | 6 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 40.07260900 -105.23128500 5.000000 1 9 | 7 0 3 21 0.00000000 0.00000000 0.00000000 0.00000000 40.07284500 -105.23057600 0.000000 1 -------------------------------------------------------------------------------- /Cptool/paramManager.py: -------------------------------------------------------------------------------- 1 | class ParameterManager: 2 | """Manage vehicle parameters""" 3 | 4 | def __init__(self): 5 | self.params = {} 6 | self.validators = {} 7 | 8 | def register_param(self, name, validator=None, default=None): 9 | """Register parameter with validation""" 10 | self.params[name] = default 11 | if validator: 12 | self.validators[name] = validator 13 | 14 | def set_param(self, name, value): 15 | """Set parameter with validation""" 16 | if name in self.validators: 17 | if not self.validators[name](value): 18 | raise ValueError(f"Invalid value for {name}") 19 | self.params[name] = value 20 | -------------------------------------------------------------------------------- /2.feature_split.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pandas as pd 3 | from sklearn.model_selection import train_test_split 4 | 5 | from Cptool.config import toolConfig 6 | 7 | if __name__ == '__main__': 8 | feature = pd.read_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features.csv") 9 | index = np.arange(0, feature.shape[0]) 10 | train, test = train_test_split(index, test_size=0.1, shuffle=True, random_state=2022) 11 | train_data = feature.iloc[train] 12 | test_data = feature.iloc[test] 13 | train_data.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_train.csv", index=False) 14 | test_data.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_test.csv", index=False) 15 | -------------------------------------------------------------------------------- /2.feature_split_px4.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pandas as pd 3 | from sklearn.model_selection import train_test_split 4 | 5 | from Cptool.config import toolConfig 6 | 7 | if __name__ == '__main__': 8 | toolConfig.select_mode("PX4") 9 | feature = pd.read_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features.csv") 10 | index = np.arange(0, feature.shape[0]) 11 | train, test = train_test_split(index, test_size=0.1, shuffle=True, random_state=2022) 12 | train_data = feature.iloc[train] 13 | test_data = feature.iloc[test] 14 | train_data.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_train.csv", index=False) 15 | test_data.to_csv(f"model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}/features_test.csv", index=False) 16 | -------------------------------------------------------------------------------- /4.pre_validate.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import time 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | import Cptool 12 | import ModelFit 13 | from Cptool.config import toolConfig 14 | from Cptool.gaMavlink import GaMavlinkAPM 15 | from Cptool.gaSimManager import GaSimManager 16 | 17 | 18 | # from Cptool.gaSimManager import GaSimManager 19 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 20 | 21 | if __name__ == '__main__': 22 | # toolConfig.select_mode("PX4") 23 | 24 | # Get Fuzzing result and validate 25 | candidate_var, candidate_obj = return_cluster_thres_gen(0.5) 26 | candidate_obj = np.array(candidate_obj, dtype=float).round(8) 27 | candidate_var = np.array(candidate_var, dtype=float).round(8) 28 | 29 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'wb') as f: 30 | pickle.dump([candidate_obj, candidate_var], f) 31 | 32 | -------------------------------------------------------------------------------- /4.pre_validate_px4.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import time 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | import Cptool 12 | import ModelFit 13 | from Cptool.config import toolConfig 14 | from Cptool.gaMavlink import GaMavlinkAPM 15 | from Cptool.gaSimManager import GaSimManager 16 | 17 | 18 | # from Cptool.gaSimManager import GaSimManager 19 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 20 | 21 | if __name__ == '__main__': 22 | toolConfig.select_mode("PX4") 23 | # Get Fuzzing result and validate 24 | candidate_var, candidate_obj = return_cluster_thres_gen(0.35) 25 | candidate_obj = np.array(candidate_obj, dtype=float).round(8) 26 | candidate_var = np.array(candidate_var, dtype=float).round(8) 27 | 28 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'wb') as f: 29 | pickle.dump([candidate_obj, candidate_var], f) 30 | 31 | -------------------------------------------------------------------------------- /Cptool/config.yaml: -------------------------------------------------------------------------------- 1 | # UAV Configuration Settings 2 | 3 | # Operation Mode (PX4 or Ardupilot) 4 | mode: PX4 5 | 6 | # Simulation Settings 7 | simulation: 8 | speed: 3 9 | home: AVC_plane 10 | debug: true 11 | wind_range: [8, 10.7] 12 | window: 13 | height: 640 14 | weight: 480 15 | altitude: 16 | limit_high: 50 17 | limit_low: 40 18 | 19 | # Path Settings 20 | paths: 21 | ardupilot_log: /media/rain/data 22 | sitl: /home/rain/ardupilot/Tools/autotest/sim_vehicle.py 23 | airsim: /media/rain/data/airsim/Africa_Savannah/LinuxNoEditor/Africa_001.sh 24 | px4_run: /home/rain/PX4-Autopilot 25 | jmavsim: /home/rain/PX4-Autopilot/Tools/jmavsim_run.sh 26 | morse: /home/rain/ardupilot/libraries/SITL/examples/Morse/quadcopter.py 27 | 28 | # Model Settings 29 | model: 30 | input_len: 4 31 | output_len: 1 32 | segment_len: 14 33 | retrans: true 34 | cluster_choice_num: 10 35 | 36 | # Parameter Files 37 | param_files: 38 | ardupilot: Cptool/param_ardu.json 39 | px4: Cptool/param_px4.json 40 | -------------------------------------------------------------------------------- /4.validate_thread.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import subprocess 7 | import time 8 | 9 | import numpy as np 10 | import pandas as pd 11 | 12 | import Cptool 13 | import ModelFit 14 | from Cptool.config import toolConfig 15 | from Cptool.gaMavlink import GaMavlinkAPM 16 | from Cptool.gaSimManager import GaSimManager 17 | 18 | 19 | # from Cptool.gaSimManager import GaSimManager 20 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 21 | 22 | if __name__ == '__main__': 23 | parser = argparse.ArgumentParser(description='Personal information') 24 | parser.add_argument('--thread', dest='thread', type=int, help='Name of the candidate', default=1) 25 | args = parser.parse_args() 26 | thread = args.thread 27 | thread = int(thread) 28 | print(thread) 29 | 30 | for i in range(thread): 31 | cmd = f'gnome-terminal --tab --working-directory={os.getcwd()} -e "python3 {os.getcwd()}/4.validate.py --device {i}"' 32 | os.system(cmd) 33 | -------------------------------------------------------------------------------- /4.validate_thread_px4.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import subprocess 7 | import time 8 | 9 | import numpy as np 10 | import pandas as pd 11 | 12 | import Cptool 13 | import ModelFit 14 | from Cptool.config import toolConfig 15 | from Cptool.gaMavlink import GaMavlinkAPM 16 | from Cptool.gaSimManager import GaSimManager 17 | 18 | 19 | # from Cptool.gaSimManager import GaSimManager 20 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 21 | 22 | if __name__ == '__main__': 23 | toolConfig.select_mode("PX4") 24 | parser = argparse.ArgumentParser(description='Personal information') 25 | parser.add_argument('--thread', dest='thread', type=int, help='Name of the candidate', default=1) 26 | args = parser.parse_args() 27 | thread = args.thread 28 | thread = int(thread) 29 | print(thread) 30 | 31 | for i in range(thread): 32 | cmd = f'gnome-terminal --tab --working-directory={os.getcwd()} -e "python3 {os.getcwd()}/4.validate_px4_thread_version.py --device {i}"' 33 | os.system(cmd) 34 | -------------------------------------------------------------------------------- /2.raw_split.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | from numpy.lib.stride_tricks import sliding_window_view 4 | from sklearn.model_selection import train_test_split 5 | import numpy as np 6 | from Cptool.config import toolConfig 7 | from ModelFit.approximate import CyLSTM, Modeling 8 | 9 | """ 10 | Train LSTM Model 11 | """ 12 | if __name__ == '__main__': 13 | 14 | pd_csv = CyLSTM.merge_file_data(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/bin_regular/csv") 15 | 16 | np_data = pd_csv.to_numpy()[:, :toolConfig.STATUS_LEN] 17 | 18 | trans = Modeling.load_trans() 19 | np_data = trans.transform(np_data) 20 | 21 | np_data = np_data[:-(np_data.shape[0] % (toolConfig.SEGMENT_LEN + 1)), :] 22 | # Split 23 | np_data = np.array(np.array_split(np_data, np_data.shape[0] // (toolConfig.SEGMENT_LEN + 1), axis=0)) 24 | 25 | index = np.arange(0, np_data.shape[0]) 26 | train, test = train_test_split(index, test_size=0.1, shuffle=True, random_state=2022) 27 | test_data = np_data[test] 28 | 29 | with open(f"model/{toolConfig.MODE}/raw_test.pkl", "wb") as f: 30 | pickle.dump(test_data, f) 31 | -------------------------------------------------------------------------------- /2.raw_split_px4.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | from numpy.lib.stride_tricks import sliding_window_view 4 | from sklearn.model_selection import train_test_split 5 | import numpy as np 6 | from Cptool.config import toolConfig 7 | from ModelFit.approximate import CyLSTM, Modeling 8 | 9 | """ 10 | Train LSTM Model 11 | """ 12 | if __name__ == '__main__': 13 | toolConfig.select_mode("PX4") 14 | 15 | pd_csv = CyLSTM.merge_file_data(f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/ulg_changed/csv") 16 | 17 | np_data = pd_csv.to_numpy()[:, :toolConfig.STATUS_LEN] 18 | 19 | trans = Modeling.load_trans() 20 | np_data = trans.transform(np_data) 21 | 22 | np_data = np_data[:-(np_data.shape[0] % (toolConfig.SEGMENT_LEN + 1)), :] 23 | # Split 24 | np_data = np.array(np.array_split(np_data, np_data.shape[0] // (toolConfig.SEGMENT_LEN + 1), axis=0)) 25 | 26 | index = np.arange(0, np_data.shape[0]) 27 | train, test = train_test_split(index, test_size=0.1, shuffle=True, random_state=2022) 28 | test_data = np_data[test] 29 | 30 | with open(f"model/{toolConfig.MODE}/raw_test.pkl", "wb") as f: 31 | pickle.dump(test_data, f) 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 BlackJocker1995 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Cptool/mission_px4.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 0 3 22 0.00000000 0.00000000 0.00000000 Nan -35.36326100 149.16523000 10.000000 1 3 | 1 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36290880 149.16430860 3.000000 1 4 | 2 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36278850 149.16496170 5.000000 1 5 | 3 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36310350 149.16446950 7.000000 1 6 | 4 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36340320 149.16492280 3.000000 1 7 | 5 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36286180 149.16526480 9.000000 1 8 | 6 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36336820 149.16552900 11.000000 1 9 | 7 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36293070 149.16603060 11.000000 1 10 | 8 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36272400 149.16537340 11.000000 1 11 | 9 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36251730 149.16600640 11.000000 1 12 | 10 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36214110 149.16566580 11.000000 1 13 | 11 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36260590 149.16525680 11.000000 1 14 | 12 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36206890 149.16523130 11.000000 1 15 | 13 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36213450 149.16475120 11.000000 1 16 | 14 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36262450 149.16509580 11.000000 1 17 | 15 0 3 16 0.00000000 0.00000000 0.00000000 Nan -35.36230730 149.16443740 11.000000 1 18 | 16 0 3 21 0.00000000 0.00000000 0.00000000 Nan -35.36270320 149.16429790 0.000000 1 -------------------------------------------------------------------------------- /Cptool/mission.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 110 2 | 0 1 0 16 0 0 0 0 -35.362758 149.165135 583.730592 1 3 | 1 0 3 22 0.00000000 0.00000000 0.00000000 0.00000000 -35.36326100 149.16523000 10.000000 1 4 | 2 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36290880 149.16430860 3.000000 1 5 | 3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36278850 149.16496170 5.000000 1 6 | 4 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36310350 149.16446950 7.000000 1 7 | 5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36340320 149.16492280 3.000000 1 8 | 6 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36286180 149.16526480 9.000000 1 9 | 7 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36336820 149.16552900 11.000000 1 10 | 8 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36293070 149.16603060 11.000000 1 11 | 9 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36272400 149.16537340 11.000000 1 12 | 10 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36251730 149.16600640 11.000000 1 13 | 11 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36214110 149.16566580 11.000000 1 14 | 12 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36260590 149.16525680 11.000000 1 15 | 13 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36206890 149.16523130 11.000000 1 16 | 14 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36213450 149.16475120 11.000000 1 17 | 15 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36262450 149.16509580 11.000000 1 18 | 16 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36230730 149.16443740 11.000000 1 19 | 17 0 3 21 0.00000000 0.00000000 0.00000000 0.00000000 -35.36270320 149.16429790 0.000000 1 20 | -------------------------------------------------------------------------------- /0.collect_px4.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import time 4 | from datetime import datetime 5 | 6 | from Cptool.config import toolConfig 7 | from Cptool.gaMavlink import GaMavlinkPX4 8 | from Cptool.gaSimManager import GaSimManager 9 | 10 | 11 | def file_number(path): 12 | return len([name for name in os.listdir(path) if name.endswith(".ulg")]) 13 | 14 | 15 | if __name__ == '__main__': 16 | toolConfig.select_mode("PX4") 17 | # Create txt if not exists 18 | manager = GaSimManager(debug=toolConfig.DEBUG) 19 | 20 | time.sleep(1) 21 | while file_number(f"{toolConfig.PX4_LOG_PATH}") < 500: 22 | try: 23 | time.sleep(0.5) 24 | 25 | print('--------------------------------------------------------------------------------------------------') 26 | print( 27 | f'--------- {datetime.now()} === lastindex: {file_number(f"{toolConfig.PX4_LOG_PATH}")}--------------') 28 | print('--------------------------------------------------------------------------------------------------') 29 | 30 | manager.start_sitl() 31 | 32 | manager.mav_monitor_init(GaMavlinkPX4, 0) 33 | 34 | manager.mav_monitor.set_mission('Cptool/fitCollection_px4.txt', False) 35 | 36 | time.sleep(2) 37 | manager.mav_monitor.set_random_param_and_start() 38 | # manager.mav_monitor.start_mission() 39 | 40 | result = manager.mav_monitor.wait_complete() 41 | 42 | # manager.mav_monitor.reset_params() 43 | 44 | manager.stop_sitl() 45 | 46 | if not result: 47 | # Delete current log 48 | GaMavlinkPX4.delete_current_log() 49 | 50 | except Exception as e: 51 | logging.warning(e) 52 | continue 53 | -------------------------------------------------------------------------------- /0.collect.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | from datetime import datetime 4 | 5 | from Cptool.config import toolConfig 6 | from Cptool.gaMavlink import GaMavlinkAPM 7 | from Cptool.gaSimManager import GaSimManager 8 | 9 | 10 | def least(): 11 | log_index = f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/LASTLOG.TXT" 12 | with open(log_index, 'r') as f: 13 | i = int(f.readline()) 14 | return i 15 | 16 | 17 | if __name__ == '__main__': 18 | # Create txt if not exists 19 | log_index = f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/LASTLOG.TXT" 20 | if not os.path.exists(log_index): 21 | with open(log_index, "w") as f: 22 | f.write('0') 23 | 24 | manager = GaSimManager(debug=toolConfig.DEBUG) 25 | 26 | time.sleep(1) 27 | while least() < 500: 28 | time.sleep(0.5) 29 | log_index = f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/LASTLOG.TXT" 30 | if os.path.exists(log_index): 31 | with open(log_index, 'r') as f: 32 | num = int(f.readline()) 33 | print('--------------------------------------------------------------------------------------------------') 34 | print(f'--------- {datetime.now()} === lastindex: {num}----------------------') 35 | print('--------------------------------------------------------------------------------------------------') 36 | 37 | manager.start_sitl() 38 | 39 | manager.mav_monitor_init(GaMavlinkAPM, 0) 40 | 41 | manager.mav_monitor.set_mission('Cptool/fitCollection.txt', False) 42 | 43 | manager.mav_monitor.set_random_param_and_start() 44 | # manager.mav_monitor.start_mission() 45 | 46 | # manager.start_mav_monitor() 47 | 48 | result = manager.mav_monitor.wait_complete() 49 | 50 | manager.mav_monitor.reset_params() 51 | 52 | manager.stop_sitl() 53 | 54 | if not result: 55 | # Delete current log 56 | GaMavlinkAPM.delete_current_log() 57 | -------------------------------------------------------------------------------- /range/range.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import pandas as pd 4 | import numpy as np 5 | 6 | 7 | def satisfy_range(panda_m, top, button): 8 | """ 9 | Calculate satisfaction rate for given parameter ranges 10 | Args: 11 | panda_m: Parameter data frame 12 | top: Upper bounds 13 | button: Lower bounds 14 | Returns: 15 | pass_rate: Ratio of passing configurations 16 | """ 17 | values = panda_m.values[:, :-1] 18 | to_top = (top - values).min(axis=1) 19 | to_button = (values - button).min(axis=1) 20 | 21 | valid_indices = np.where((to_top >= 0) & (to_button >= 0))[0] 22 | if len(valid_indices) == 0: 23 | return 0.0 24 | 25 | satisfy_value = panda_m.iloc[valid_indices] 26 | pass_rate = (satisfy_value.result == 'pass').sum() / len(satisfy_value) 27 | print(f'Pass rate: {pass_rate:.2f}') 28 | return pass_rate 29 | 30 | 31 | def best_summary(UavConfig): 32 | """ 33 | Analyze and summarize best configurations 34 | Args: 35 | UavConfig: UAV configuration object 36 | Returns: 37 | bincout: Most common parameter values 38 | """ 39 | objV = 'E:/program/uavga/Result/ObjV.csv' 40 | phen = 'E:/program/uavga/Result/Phen.csv' 41 | 42 | pd_objV = pd.read_csv(objV,names=['num','rate']) 43 | pd_phen = pd.read_csv(phen) 44 | 45 | pd_objV['sort_num'] = pd_objV['rate'].rank(ascending=0, method='dense') 46 | index = pd_objV.sort_num < 10 47 | pd_choice_phen = pd_phen[index] 48 | 49 | 50 | param_choice_dict = {key: value for key, value in UavConfig.param_dict.items() if key in UavConfig.get_participate_param()} 51 | # 步长 扩充 52 | step = np.array([param_choice_dict[it]['step'] for it in list(param_choice_dict)]) 53 | step = step.repeat(2) 54 | # 还原 55 | pd_choice_phen = (pd_choice_phen * step) 56 | 57 | # 求众数 58 | bincout = pd_choice_phen.mode().to_numpy()[0, :] 59 | print(bincout) 60 | return bincout 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /Cptool/param_px4.json: -------------------------------------------------------------------------------- 1 | { 2 | "MC_ROLL_P": { 3 | "range": [ 4 | 0.0, 5 | 12.0 6 | ], 7 | "step": 0.1, 8 | "default": 6.5 9 | }, 10 | "MC_PITCH_P": { 11 | "range": [ 12 | 0.0, 13 | 12.0 14 | ], 15 | "step": 0.1, 16 | "default": 6.5 17 | }, 18 | "MC_YAW_P": { 19 | "range": [ 20 | 0.0, 21 | 5.0 22 | ], 23 | "step": 0.1, 24 | "default": 2.8 25 | }, 26 | "MC_YAW_WEIGHT": { 27 | "range": [ 28 | 0.0, 29 | 1.0 30 | ], 31 | "step": 0.1, 32 | "default": 0.4 33 | }, 34 | "MPC_XY_P": { 35 | "range": [ 36 | 0.0, 37 | 2.0 38 | ], 39 | "step": 0.1, 40 | "default": 0.9 41 | }, 42 | "MPC_Z_P": { 43 | "range": [ 44 | 0.0, 45 | 1.5 46 | ], 47 | "step": 0.1, 48 | "default": 1.0 49 | }, 50 | "MC_PITCHRATE_P": { 51 | "range": [ 52 | 0.01, 53 | 0.6 54 | ], 55 | "step": 0.15, 56 | "default": 0.15 57 | }, 58 | "MC_ROLLRATE_P": { 59 | "range": [ 60 | 0.01, 61 | 0.5 62 | ], 63 | "step": 0.01, 64 | "default": 0.15 65 | }, 66 | "MC_YAWRATE_P": { 67 | "range": [ 68 | 0.0, 69 | 0.6 70 | ], 71 | "step": 0.01, 72 | "default": 0.2 73 | }, 74 | "MPC_TILTMAX_AIR": { 75 | "range": [ 76 | 20.0, 77 | 89.0 78 | ], 79 | "step": 1, 80 | "default": 45.0 81 | }, 82 | "MIS_YAW_ERR": { 83 | "range": [ 84 | 0.0, 85 | 90.0 86 | ], 87 | "step": 1, 88 | "default": 12.0 89 | }, 90 | "MPC_Z_VEL_MAX_DN": { 91 | "range": [ 92 | 0.5, 93 | 4.0 94 | ], 95 | "step": 0.1, 96 | "default": 1.0 97 | }, 98 | "MPC_Z_VEL_MAX_UP": { 99 | "range": [ 100 | 0.5, 101 | 8.0 102 | ], 103 | "step": 0.1, 104 | "default": 3.0 105 | }, 106 | "MPC_TKO_SPEED": { 107 | "range": [ 108 | 1.0, 109 | 5.0 110 | ], 111 | "step": 0.1, 112 | "default": 1.5 113 | } 114 | } -------------------------------------------------------------------------------- /analysis/range_analysis.py: -------------------------------------------------------------------------------- 1 | # coding: utf-8 2 | import numpy as np 3 | import pandas 4 | 5 | from Cptool.config import toolConfig 6 | 7 | ''' 8 | Analysis Flight Data 9 | ''' 10 | 11 | def read_invalid_csv(file): 12 | return pandas.read_csv(file) 13 | 14 | 15 | def cal_coverage(range_up, range_down, invalid_config): 16 | invalid_config = invalid_config[invalid_config['result'] != 'pass'] 17 | 18 | data = invalid_config.to_numpy()[:, :6] 19 | over_up = data < range_up 20 | over_down = data > range_down 21 | 22 | reduce:np.array = over_down * over_up 23 | reduce = np.prod(reduce,axis=1) 24 | 25 | avoid_ratio = np.sum(reduce==0) / len(reduce) 26 | print(avoid_ratio) 27 | print(f'Avoid Invalid {np.sum(reduce==0)} / {len(reduce)}') 28 | 29 | not_avoid_invalid = invalid_config[reduce!=0] 30 | 31 | return avoid_ratio 32 | 33 | 34 | def cal_invalidRvalid(range_up, range_down, invalid_config): 35 | # invalid_config = invalid_config[invalid_config['result'] != 'pass'] 36 | 37 | data = invalid_config.to_numpy()[:, :6] 38 | over_up = data <= range_up 39 | over_down = data >= range_down 40 | 41 | reduce:np.array = over_down * over_up 42 | reduce = np.prod(reduce,axis=1) 43 | 44 | invalid_config = invalid_config[reduce==1] 45 | invalid = invalid_config[invalid_config['result'] != 'pass'] 46 | valid = invalid_config[invalid_config['result'] == 'pass'] 47 | 48 | print(f'Invalid / Valid / Covered - {len(invalid)} / {len(valid)} / {len(invalid_config)}') 49 | 50 | 51 | if __name__ == '__main__': 52 | # GA 53 | # range_up = [2.1, 0.4, 0.9, 0.1,2000,8000] 54 | # range_down = [0.6,-1,-0.7, -0.8,300,1000] 55 | 56 | # 1 57 | # range_up = [6.0, 5.0, 5.0, 5.0, 2000, 8000] 58 | # range_down = [0.1, -4.7, -5.0, -5.0, 50, 1000] 59 | 60 | # M 61 | # range_up = [6.0, 4.1, 3.2, 1.3, 2000, 8000] 62 | # range_down = [0.6, -1, -0.7, -0.8, 300, 1000] 63 | 64 | # GA 65 | range_up = [4.7, 0.8, 0.7, 0.4, 1950, 6850] 66 | range_down = [0.7, -0.7, -0.8, -0.3, 300, 1000] 67 | 68 | invalid_config = read_invalid_csv(f'../result/{toolConfig.MODE}/params_ux.csv') 69 | cal_invalidRvalid(range_up, range_down, invalid_config) 70 | -------------------------------------------------------------------------------- /Cptool/param_ardu.json: -------------------------------------------------------------------------------- 1 | { 2 | "PSC_VELXY_P": { 3 | "range": [ 4 | 0.1, 5 | 6.0 6 | ], 7 | "step": 0.1, 8 | "default": 2 9 | }, 10 | "PSC_VELXY_I": { 11 | "range": [ 12 | 0.02, 13 | 1.0 14 | ], 15 | "step": 0.01, 16 | "default": 1.0 17 | }, 18 | "PSC_VELXY_D": { 19 | "range": [ 20 | 0.0, 21 | 1.0 22 | ], 23 | "step": 0.001, 24 | "default": 0.5 25 | }, 26 | "PSC_ACCZ_P": { 27 | "range": [ 28 | 0.2, 29 | 1.5 30 | ], 31 | "step": 0.05, 32 | "default": 0.5 33 | }, 34 | "PSC_ACCZ_I": { 35 | "range": [ 36 | 0.0, 37 | 3.0 38 | ], 39 | "step": 0.1, 40 | "default": 1.0 41 | }, 42 | "ATC_ANG_RLL_P": { 43 | "range": [ 44 | 3.0, 45 | 12.0 46 | ], 47 | "step": 0.1, 48 | "default": 4.5 49 | }, 50 | "ATC_RAT_RLL_P": { 51 | "range": [ 52 | 0.01, 53 | 0.5 54 | ], 55 | "step": 0.005, 56 | "default": 0.135 57 | }, 58 | "ATC_RAT_RLL_I": { 59 | "range": [ 60 | 0.01, 61 | 2.0 62 | ], 63 | "step": 0.005, 64 | "default": 0.135 65 | }, 66 | "ATC_RAT_RLL_D": { 67 | "range": [ 68 | 0.0, 69 | 0.05 70 | ], 71 | "step": 0.001, 72 | "default": 0.0036 73 | }, 74 | "ATC_ANG_PIT_P": { 75 | "range": [ 76 | 3.0, 77 | 12.0 78 | ], 79 | "step": 0.1, 80 | "default": 4.5 81 | }, 82 | "ATC_RAT_PIT_P": { 83 | "range": [ 84 | 0.01, 85 | 0.5 86 | ], 87 | "step": 0.005, 88 | "default": 0.135 89 | }, 90 | "ATC_RAT_PIT_I": { 91 | "range": [ 92 | 0.01, 93 | 2.0 94 | ], 95 | "step": 0.01, 96 | "default": 0.135 97 | }, 98 | "ATC_RAT_PIT_D": { 99 | "range": [ 100 | 0.0, 101 | 0.05 102 | ], 103 | "step": 0.001, 104 | "default": 0.0036 105 | }, 106 | "ATC_ANG_YAW_P": { 107 | "range": [ 108 | 3.0, 109 | 12.0 110 | ], 111 | "step": 0.1, 112 | "default": 4.5 113 | }, 114 | "ATC_RAT_YAW_P": { 115 | "range": [ 116 | 0.1, 117 | 2.5 118 | ], 119 | "step": 0.005, 120 | "default": 0.18 121 | }, 122 | "ATC_RAT_YAW_I": { 123 | "range": [ 124 | 0.01, 125 | 1.0 126 | ], 127 | "step": 0.01, 128 | "default": 0.01 129 | }, 130 | "ATC_RAT_YAW_D": { 131 | "range": [ 132 | 0.00, 133 | 0.02 134 | ], 135 | "step": 0.001, 136 | "default": 0 137 | }, 138 | "WPNAV_SPEED": { 139 | "range": [ 140 | 50, 141 | 2000 142 | ], 143 | "step": 50, 144 | "default": 1000 145 | }, 146 | "WPNAV_ACCEL": { 147 | "range": [ 148 | 50, 149 | 500 150 | ], 151 | "step": 10, 152 | "default": 250 153 | }, 154 | "ANGLE_MAX": { 155 | "range": [ 156 | 1000, 157 | 8000 158 | ], 159 | "step": 10, 160 | "default": 1000 161 | } 162 | } -------------------------------------------------------------------------------- /Cptool/param_ardu_back.json: -------------------------------------------------------------------------------- 1 | { 2 | "PSC_VELXY_P": { 3 | "range": [ 4 | 0.1, 5 | 6.0 6 | ], 7 | "step": 0.1, 8 | "default": 2 9 | }, 10 | "PSC_VELXY_I": { 11 | "range": [ 12 | 0.02, 13 | 1.0 14 | ], 15 | "step": 0.01, 16 | "default": 1.0 17 | }, 18 | "PSC_VELXY_D": { 19 | "range": [ 20 | 0.0, 21 | 1.0 22 | ], 23 | "step": 0.001, 24 | "default": 0.5 25 | }, 26 | "PSC_ACCZ_P": { 27 | "range": [ 28 | 0.2, 29 | 1.5 30 | ], 31 | "step": 0.05, 32 | "default": 0.5 33 | }, 34 | "PSC_ACCZ_I": { 35 | "range": [ 36 | 0.0, 37 | 3.0 38 | ], 39 | "step": 0.1, 40 | "default": 1.0 41 | }, 42 | "ATC_ANG_RLL_P": { 43 | "range": [ 44 | 3.0, 45 | 12.0 46 | ], 47 | "step": 0.1, 48 | "default": 4.5 49 | }, 50 | "ATC_RAT_RLL_P": { 51 | "range": [ 52 | 0.01, 53 | 0.5 54 | ], 55 | "step": 0.005, 56 | "default": 0.135 57 | }, 58 | "ATC_RAT_RLL_I": { 59 | "range": [ 60 | 0.01, 61 | 2.0 62 | ], 63 | "step": 0.005, 64 | "default": 0.135 65 | }, 66 | "ATC_RAT_RLL_D": { 67 | "range": [ 68 | 0.0, 69 | 0.05 70 | ], 71 | "step": 0.001, 72 | "default": 0.0036 73 | }, 74 | "ATC_ANG_PIT_P": { 75 | "range": [ 76 | 3.0, 77 | 12.0 78 | ], 79 | "step": 0.1, 80 | "default": 4.5 81 | }, 82 | "ATC_RAT_PIT_P": { 83 | "range": [ 84 | 0.01, 85 | 0.5 86 | ], 87 | "step": 0.005, 88 | "default": 0.135 89 | }, 90 | "ATC_RAT_PIT_I": { 91 | "range": [ 92 | 0.01, 93 | 2.0 94 | ], 95 | "step": 0.01, 96 | "default": 0.135 97 | }, 98 | "ATC_RAT_PIT_D": { 99 | "range": [ 100 | 0.0, 101 | 0.05 102 | ], 103 | "step": 0.001, 104 | "default": 0.0036 105 | }, 106 | "ATC_ANG_YAW_P": { 107 | "range": [ 108 | 3.0, 109 | 12.0 110 | ], 111 | "step": 0.1, 112 | "default": 4.5 113 | }, 114 | "ATC_RAT_YAW_P": { 115 | "range": [ 116 | 0.1, 117 | 2.5 118 | ], 119 | "step": 0.005, 120 | "default": 0.18 121 | }, 122 | "ATC_RAT_YAW_I": { 123 | "range": [ 124 | 0.01, 125 | 1.0 126 | ], 127 | "step": 0.01, 128 | "default": 0.01 129 | }, 130 | "ATC_RAT_YAW_D": { 131 | "range": [ 132 | 0.00, 133 | 0.02 134 | ], 135 | "step": 0.001, 136 | "default": 0 137 | }, 138 | "WPNAV_SPEED": { 139 | "range": [ 140 | 50, 141 | 2000 142 | ], 143 | "step": 50, 144 | "default": 1000 145 | }, 146 | "WPNAV_ACCEL": { 147 | "range": [ 148 | 50, 149 | 500 150 | ], 151 | "step": 10, 152 | "default": 250 153 | }, 154 | "ANGLE_MAX": { 155 | "range": [ 156 | 1000, 157 | 8000 158 | ], 159 | "step": 10, 160 | "default": 1000 161 | } 162 | } -------------------------------------------------------------------------------- /range/rangeproblem.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from abc import ABC, abstractmethod 3 | import numpy as np 4 | import geatpy as ea 5 | import pandas as pd 6 | 7 | from Cptool.config import toolConfig 8 | from Cptool.mavtool import load_param, read_unit_from_dict 9 | 10 | 11 | class BaseRangeProblem(ABC): 12 | def __init__(self): 13 | self.param_bounds = None 14 | self.step = None 15 | 16 | @abstractmethod 17 | def evaluate(self, population): 18 | pass 19 | 20 | def init_bounds_and_step(self, param_bounds, step): 21 | """Initialize parameter bounds and step size""" 22 | self.param_bounds = param_bounds 23 | self.step = step 24 | 25 | def param_value2step(self, configuration): 26 | """Convert parameter values to step-aligned values""" 27 | np_config = np.ceil(configuration / self.step) * self.step 28 | return pd.DataFrame([np_config.tolist()], 29 | columns=toolConfig.PARAM).iloc[0].to_dict() 30 | 31 | 32 | class GARangeProblem(BaseRangeProblem, ea.Problem): 33 | def __init__(self, name, M, maxormins, Dim, varTypes, lb, ub, lbin, ubin, result_data): 34 | BaseRangeProblem.__init__(self) 35 | ea.Problem.__init__(self, name, M, maxormins, Dim, 36 | varTypes, lb, ub, lbin, ubin) 37 | self.data = result_data 38 | 39 | def aimFunc(self, pop): 40 | """Calculate objective functions for the population""" 41 | x = self._preprocess_population(pop) 42 | bottom, top = self._split_bounds(x) 43 | score_rate, score_len = self._calculate_scores(top, bottom) 44 | pop.ObjV = self._combine_objectives(score_rate, score_len) 45 | 46 | def _preprocess_population(self, pop): 47 | """Preprocess population data""" 48 | return self.reasonable_range(pop.Phen) 49 | 50 | def _split_bounds(self, x): 51 | """Split chromosome into bottom and top bounds""" 52 | return x[:, ::2], x[:, 1::2] 53 | 54 | def _calculate_scores(self, top, bottom): 55 | """Calculate satisfaction scores for the bounds""" 56 | score_rate = np.zeros(top.shape[0]) 57 | score_len = np.zeros(top.shape[0]) 58 | 59 | for i, (t, b) in enumerate(zip(top, bottom)): 60 | rate, length = self.satisfy_range(t, b) 61 | score_rate[i] = rate 62 | score_len[i] = length 63 | 64 | return score_rate, score_len 65 | 66 | def _combine_objectives(self, score_rate, score_len): 67 | """Combine objectives into final fitness values""" 68 | return np.hstack([ 69 | score_len.reshape((-1, 1)), 70 | score_rate.reshape((-1, 1)) 71 | ]) 72 | 73 | def reasonable_range(self, param): 74 | return param * np.repeat(self.step, 2) 75 | 76 | def satisfy_range(self, top, button): 77 | values = self.data.values[:, :-1] 78 | 79 | to_top = (top - values).min(axis=1) 80 | to_button = (values - button).min(axis=1) 81 | 82 | index = np.where((to_top >= 0) & (to_button >= 0))[0] 83 | if len(index) == 0: 84 | return 0, len(index) 85 | satisfy_value = self.data.iloc[index] 86 | pass_index = satisfy_value.result == 'pass' 87 | pass_rate = pass_index.values.sum() / satisfy_value.shape[0] 88 | print(f'include num: {len(index)} rate: {pass_rate}') 89 | return pass_rate, len(index) 90 | 91 | @staticmethod 92 | def reasonable_range_static(param): 93 | para_dict = load_param() 94 | step_unit = read_unit_from_dict(para_dict) 95 | return param * np.repeat(step_unit, 2) -------------------------------------------------------------------------------- /4.validate.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import os 4 | import pickle 5 | import time 6 | from loguru import logger 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | import Cptool 12 | import ModelFit 13 | from Cptool.config import toolConfig 14 | from Cptool.gaMavlink import GaMavlinkAPM 15 | from Cptool.gaSimManager import GaSimManager 16 | 17 | 18 | # from Cptool.gaSimManager import GaSimManager 19 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 20 | 21 | if __name__ == '__main__': 22 | parser = argparse.ArgumentParser(description='Personal information') 23 | parser.add_argument('--device', dest='device', type=str, help='Name of the candidate') 24 | args = parser.parse_args() 25 | device = args.device 26 | if device is None: 27 | device = 0 28 | print(device) 29 | 30 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 31 | candidate_obj, candidate_var = pickle.load(f) 32 | 33 | # Simulator validation 34 | manager = GaSimManager(debug=toolConfig.DEBUG) 35 | 36 | i = 0 37 | # Random order 38 | rand_index = (np.arange(candidate_obj.shape[0])) 39 | np.random.shuffle(rand_index) 40 | candidate_obj = candidate_obj[rand_index] 41 | candidate_var = candidate_var[rand_index] 42 | 43 | # Loop to validate configurations with SITL simulator 44 | for index, vars, value_vector in zip(np.arange(candidate_obj.shape[0]), candidate_var, candidate_obj): 45 | print(f'======================={index} / {candidate_obj.shape[0]} ==========================') 46 | # if exist file, append new data in the end. 47 | if os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 48 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.R_OK): 49 | continue 50 | data = pd.read_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv') 51 | exit_data = data.drop(['score', 'result'], axis=1, inplace=False) 52 | # carry our simulation test 53 | if ((exit_data - value_vector).sum(axis=1).abs() < 0.00001).sum() > 0: 54 | continue 55 | 56 | configuration = pd.Series(value_vector, index=toolConfig.PARAM_PART).to_dict() 57 | print(configuration) 58 | # start multiple SITL 59 | manager.start_multiple_sitl(device) 60 | manager.mav_monitor_init(GaMavlinkAPM, device) 61 | 62 | if not manager.mav_monitor_connect(): 63 | manager.stop_sitl() 64 | continue 65 | 66 | manager.mav_monitor.set_mission("Cptool/fitCollection.txt", israndom=False) 67 | manager.mav_monitor.set_params(configuration) 68 | 69 | manager.mav_monitor.start_mission() 70 | 71 | result = manager.mav_monitor_error() 72 | logger.info(f"Validated result: {result}") 73 | 74 | # if the result have no instability, skip. 75 | if not os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 76 | data = pd.DataFrame(columns=(toolConfig.PARAM_PART + ['score', 'result'])) 77 | data.to_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv', index=False) 78 | 79 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.W_OK): 80 | continue 81 | # Add instability result 82 | tmp_row = value_vector.tolist() 83 | tmp_row.append(vars[0]) 84 | tmp_row.append(result) 85 | 86 | # Write Row 87 | with open(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", 'a+') as f: 88 | csv_file = csv.writer(f) 89 | csv_file.writerow(tmp_row) 90 | logger.debug(f"Write row to params{toolConfig.EXE}.csv.") 91 | 92 | manager.stop_sitl() 93 | i += 1 94 | 95 | localtime = time.asctime(time.localtime(time.time())) 96 | # Mail notification plugin 97 | # send_mail(Cptool.config.AIRSIM_PATH, localtime) -------------------------------------------------------------------------------- /4.validate_px4.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import time 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | import Cptool 12 | import ModelFit 13 | from Cptool.config import toolConfig 14 | from Cptool.gaMavlink import GaMavlinkAPM, GaMavlinkPX4 15 | from Cptool.gaSimManager import GaSimManager 16 | 17 | 18 | # from Cptool.gaSimManager import GaSimManager 19 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 20 | 21 | if __name__ == '__main__': 22 | 23 | parser = argparse.ArgumentParser(description='Personal information') 24 | parser.add_argument('--device', dest='device', type=str, help='Name of the candidate') 25 | args = parser.parse_args() 26 | device = args.device 27 | if device is None: 28 | device = 0 29 | print(device) 30 | 31 | toolConfig.select_mode("PX4") 32 | 33 | # Get Fuzzing result and validate 34 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 35 | candidate_obj, candidate_var = pickle.load(f) 36 | 37 | # Simulator validation 38 | manager = GaSimManager(debug=toolConfig.DEBUG) 39 | 40 | i = 0 41 | # Random order 42 | rand_index = (np.arange(candidate_obj.shape[0])) 43 | np.random.shuffle(rand_index) 44 | candidate_obj = candidate_obj[rand_index] 45 | candidate_var = candidate_var[rand_index] 46 | 47 | # Loop to validate configurations with SITL simulator 48 | for index, vars, value_vector in zip(np.arange(candidate_obj.shape[0]), candidate_var, candidate_obj): 49 | print(f'======================={index} / {candidate_obj.shape[0]} ==========================') 50 | # if exist file, append new data in the end. 51 | if os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 52 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.R_OK): 53 | continue 54 | data = pd.read_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv') 55 | exit_data = data.drop(['score', 'result'], axis=1, inplace=False) 56 | # carry our simulation test 57 | if ((exit_data - value_vector).sum(axis=1).abs() < 0.00001).sum() > 0: 58 | continue 59 | 60 | configuration = pd.Series(value_vector, index=toolConfig.PARAM).to_dict() 61 | # start multiple SITL 62 | manager.start_sitl() 63 | manager.mav_monitor_init(GaMavlinkPX4, device) 64 | 65 | if not manager.mav_monitor_connect(): 66 | manager.stop_sitl() 67 | continue 68 | 69 | manager.mav_monitor.set_mission("Cptool/fitCollection_px4.txt", israndom=False) 70 | manager.mav_monitor.set_params(configuration) 71 | 72 | time.sleep(2) 73 | manager.mav_monitor.start_mission() 74 | result = manager.mav_monitor_error() 75 | 76 | # if the result have no instability, skip. 77 | if not os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 78 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.W_OK): 79 | time.sleep(0.1) 80 | continue 81 | data = pd.DataFrame(columns=(toolConfig.PARAM + ['score', 'result'])) 82 | else: 83 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.W_OK): 84 | time.sleep(0.1) 85 | continue 86 | # Add instability result 87 | tmp_row = value_vector.tolist() 88 | tmp_row.append(vars[0]) 89 | tmp_row.append(result) 90 | 91 | # Write Row 92 | with open(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", 'a+') as f: 93 | csv_file = csv.writer(f) 94 | csv_file.writerow(tmp_row) 95 | logging.debug("Write row to params{toolConfig.EXE}.csv.") 96 | 97 | manager.stop_sitl() 98 | i += 1 99 | time.sleep(1) 100 | 101 | localtime = time.asctime(time.localtime(time.time())) 102 | # Mail notification plugin 103 | # send_mail(Cptool.config.AIRSIM_PATH, localtime) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ICSearcher and LGDFuzzer 2 | This is an approach source code of ICSearcher. 3 | 4 | The original code of the [paper(LGDFuzzer)](https://dl.acm.org/doi/10.1145/3510003.3510084) is in branch [lgdfuzzer](https://github.com/BlackJocker1995/uavga/tree/lgdfuzzer) 5 | 6 | ICSearcher is an improved version of LGDFuzzer. 7 | 8 | # Log 9 | Update: 22-07-15, support px4 10 | 11 | ## Requirement 12 | Python package requirement: numpy ; pandas ; pymavlink ; pyulog ; keras ; tensorflow 13 | 14 | OS: The program is only test in Ubuntu 18.04 and 20.04 (recommend). 15 | 16 | ` 17 | pip3 install pymavlink pandas pyulog eventlet keras tensorflow 18 | ` 19 | 20 | 21 | Simulation requirement: Ardupilot [SITL](https://github.com/ArduPilot/ardupilot). We suggest applying python3 to run STIL simulator. 22 | Jmavsim for PX4, which requires source build in PX4 file. 23 | 24 | The initializer of Ardupilot simulator needs to change the path in the file `Cptool.config.py` with item 25 | `SITL_PATH`. 26 | 27 | For example, 28 | ` 29 | python3 {Your Ardupilot path}/Tools/autotest/sim_vehicle.py --location=AVC_plane --out=127.0.0.1:14550 -v ArduCopter -w -S {toolConfig.SPEED} " 30 | ` 31 | 32 | If you want to run PX4 evaluation in multiple thread, you should change the following code in PX4-Ardupilot. 33 | 34 | 1. Create the file `Tools/sitl_multiple_run_single.sh` and add content next: 35 | 36 | ```bash 37 | #!/bin/bash 38 | sitl_num=0 39 | [ -n "$1" ] && sitl_num="$1" 40 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 41 | src_path="$SCRIPT_DIR/.." 42 | build_path=${src_path}/build/px4_sitl_default 43 | pkill -f "px4 -i $sitl_num" 44 | sleep 1 45 | export PX4_SIM_MODEL=iris 46 | working_dir="$build_path/instance_$sitl_num" 47 | [ ! -d "$working_dir" ] && mkdir -p "$working_dir" 48 | pushd "$working_dir" &>/dev/null 49 | echo "starting instance $sitl_numin $(pwd)" 50 | ../bin/px4 -i $sitl_num -d "$build_path/etc" -s etc/init.d-posix/rcS # >out.log 2>err.log & 51 | popd &>/dev/null 52 | ``` 53 | 2. Change the flight home point in `Tools/jmavsim_run.sh` 54 | 55 | ```bash 56 | export PX4_HOME_LAT=40.072842 57 | export PX4_HOME_LON=-105.230575 58 | export PX4_HOME_ALT=0.000000 59 | export PX4_SIM_SPEED_FACTOR=3 # speed 60 | ``` 61 | 62 | ## Deployment 63 | The configuration is in `Cptool.config.py`. 64 | 65 | If you want to try PX4 simulation, change the sentence `toolConfig.select_mode("Ardupilot")` to `toolConfig.select_mode("PX4")` 66 | 67 | ## Configuration of System config.py 68 | * ARDUPILOT_LOG_PATH: log path of ardupilot running, noted that, the path needs to have a flag file "logs/LASTLOG.TXT". 69 | Or you can manually run the simulation at first in {ARDUPILOT_LOG_PATH} to auto generate flag file. 70 | 71 | The log path for PX4 is in `{PX4_Path}/build/px4_sitl_default/logs/`, which is no need to change. 72 | 73 | * SIM: simulation type. 74 | 75 | * AIRSIM_PATH: if select airsim, you should set the execution path. 76 | 77 | * PX4_RUN_PATH: if select PX4, you should set the execution path. 78 | 79 | * PARAM: the parameter used in predictor. 80 | 81 | * PARAM_PART: the parameter that participate in fuzzing. 82 | 83 | * INPUT_LEN: input length of predictor. 84 | 85 | 86 | ## Description 87 | 88 | `0.collect.py` start simulation to collect flight logs. 89 | 90 | `1.trans_bin2csv.py` transform the bin file to csv. 91 | 92 | `2.extract_feature.py` extract feature from csv. 93 | 94 | `2.raw_split.py` split the test feature for further searcher. 95 | 96 | `2.feature_split.py` split the csv data for train and test. 97 | 98 | `2.train_lstm.py` train a model predictor. 99 | 100 | `3.lgfuzzer.py` start the fuzzing test. 101 | 102 | `4.pre_validate.py` select candidates. 103 | 104 | `4.validate.py` validate configurations through simulator. 105 | 106 | If you want to validate with multiple simulator, you can use validate.py -- device {xxx} to start multiple SITL 107 | 108 | `4.validate_thread.py` validate configurations through multiple simulators, where use --thread {xx} to launch multiple tab validate.py 109 | 110 | Noted: For PX4, `4.validate_px4_thread.py` will call the `4.validate_px4_thread_version.py`. 111 | If you have no requirement for multiple thread, you should use `4.validate_thread_px4.py` 112 | 113 | 114 | `5.range.py` summary range guideline by validated result. 115 | -------------------------------------------------------------------------------- /4.validate_px4_thread_version.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import logging 4 | import os 5 | import pickle 6 | import time 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | import Cptool 12 | import ModelFit 13 | from Cptool.config import toolConfig 14 | from Cptool.gaMavlink import GaMavlinkAPM, GaMavlinkPX4 15 | from Cptool.gaSimManager import GaSimManager 16 | 17 | 18 | # from Cptool.gaSimManager import GaSimManager 19 | from uavga.fuzzer import return_random_n_gen, return_cluster_thres_gen 20 | 21 | if __name__ == '__main__': 22 | parser = argparse.ArgumentParser(description='Personal information') 23 | parser.add_argument('--device', dest='device', type=str, help='Name of the candidate') 24 | args = parser.parse_args() 25 | device = args.device 26 | if device is None: 27 | device = 0 28 | print(device) 29 | 30 | toolConfig.select_mode("PX4") 31 | 32 | # Get Fuzzing result and validate 33 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 34 | candidate_obj, candidate_var = pickle.load(f) 35 | 36 | # Simulator validation 37 | manager = GaSimManager(debug=toolConfig.DEBUG) 38 | 39 | i = 0 40 | # Random order 41 | rand_index = (np.arange(candidate_obj.shape[0])) 42 | np.random.shuffle(rand_index) 43 | candidate_obj = candidate_obj[rand_index] 44 | candidate_var = candidate_var[rand_index] 45 | 46 | # Loop to validate configurations with SITL simulator 47 | for index, vars, value_vector in zip(np.arange(candidate_obj.shape[0]), candidate_var, candidate_obj): 48 | print(f'======================={index} / {candidate_obj.shape[0]} ==========================') 49 | # if exist file, append new data in the end. 50 | if os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 51 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.R_OK): 52 | continue 53 | data = pd.read_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv') 54 | exit_data = data.drop(['score', 'result'], axis=1, inplace=False) 55 | # carry our simulation test 56 | if ((exit_data - value_vector).sum(axis=1).abs() < 0.00001).sum() > 0: 57 | continue 58 | 59 | configuration = pd.Series(value_vector, index=toolConfig.PARAM_PART).to_dict() 60 | # start multiple SITL 61 | manager.start_multiple_sitl(device) 62 | manager.start_multiple_sim(device) 63 | manager.mav_monitor_init(GaMavlinkPX4, device) 64 | 65 | if not manager.mav_monitor_connect(): 66 | manager.stop_sitl() 67 | continue 68 | 69 | manager.mav_monitor.set_mission("Cptool/fitCollection_px4.txt", israndom=False) 70 | manager.mav_monitor.set_params(configuration) 71 | 72 | time.sleep(2) 73 | manager.mav_monitor.start_mission() 74 | # File 75 | manager.mav_monitor.init_ulg_log_file(device) 76 | result = manager.mav_monitor_error() 77 | 78 | # if the result have no instability, skip. 79 | if not os.path.exists(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv'): 80 | data = pd.DataFrame(columns=(toolConfig.PARAM_PART + ['score', 'result'])) 81 | data.to_csv(f'result/{toolConfig.MODE}/params{toolConfig.EXE}.csv', index=False) 82 | 83 | while not os.access(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", os.W_OK): 84 | continue 85 | # Add instability result 86 | tmp_row = value_vector.tolist() 87 | tmp_row.append(vars[0]) 88 | tmp_row.append(result) 89 | 90 | # Write Row 91 | with open(f"result/{toolConfig.MODE}/params{toolConfig.EXE}.csv", 'a+') as f: 92 | csv_file = csv.writer(f) 93 | csv_file.writerow(tmp_row) 94 | logging.debug(f"Write row to params{toolConfig.EXE}.csv.") 95 | 96 | manager.stop_sitl() 97 | manager.stop_sim() 98 | i += 1 99 | time.sleep(1) 100 | 101 | # Delete Log 102 | if os.path.exists(manager.mav_monitor.log_file): 103 | os.remove(manager.mav_monitor.log_file) 104 | 105 | 106 | localtime = time.asctime(time.localtime(time.time())) 107 | # Mail notification plugin 108 | # send_mail(Cptool.config.AIRSIM_PATH, localtime) -------------------------------------------------------------------------------- /Cptool/mavtool.py: -------------------------------------------------------------------------------- 1 | import json 2 | import os 3 | 4 | import numpy as np 5 | import pandas as pd 6 | from pymavlink import mavutil, mavwp, mavextra 7 | from Cptool.config import toolConfig 8 | import sys, select, os 9 | import datetime 10 | from timeit import default_timer as timer 11 | import signal 12 | 13 | class Location: 14 | def __init__(self, x, y=None, timeS=0): 15 | if y is None: 16 | self.x = x.x 17 | self.y = x.y 18 | else: 19 | self.x = x 20 | self.y = y 21 | self.timeS = timeS 22 | self.npa = np.array([x, y]) 23 | 24 | def __sub__(self, other): 25 | return Location(self.x-other.x, self.y-other.y) 26 | 27 | def __str__(self): 28 | return f"X: {self.x} ; Y: {self.y}" 29 | 30 | def sum(self): 31 | return self.npa.sum() 32 | 33 | @classmethod 34 | def distance(cls, point1, point2): 35 | return mavextra.distance_lat_lon(point1.x, point1.y, 36 | point2.x, point2.y) 37 | 38 | class MavlinkManager: 39 | """Manage mavlink connections and messaging""" 40 | 41 | def __init__(self): 42 | self.connections = {} 43 | 44 | def create_connection(self, vehicle_id, port): 45 | """Create and store mavlink connection""" 46 | conn = mavutil.mavlink_connection(f'udp:0.0.0.0:{port}') 47 | self.connections[vehicle_id] = conn 48 | return conn 49 | 50 | async def monitor_messages(self, vehicle_id): 51 | """Async message monitoring""" 52 | conn = self.connections[vehicle_id] 53 | while True: 54 | msg = await conn.recv_match_async() 55 | if msg: 56 | await self.process_message(msg) 57 | 58 | def load_param(): 59 | """ 60 | load parameter we want to fuzzing 61 | :return: 62 | """ 63 | if toolConfig.MODE == 'Ardupilot': 64 | path = 'Cptool/param_ardu.json' 65 | elif toolConfig.MODE == 'PX4': 66 | path = 'Cptool/param_px4.json' 67 | with open(path, 'r') as f: 68 | return pd.DataFrame(json.loads(f.read())) 69 | 70 | 71 | def load_sub_param(): 72 | """ 73 | load parameter we want to fuzzing 74 | :return: 75 | """ 76 | if toolConfig.MODE == 'Ardupilot': 77 | path = 'Cptool/param_ardu.json' 78 | elif toolConfig.MODE == 'PX4': 79 | path = 'Cptool/param_px4.json' 80 | with open(path, 'r') as f: 81 | return pd.DataFrame(json.loads(f.read()))[toolConfig.PARAM_PART] 82 | 83 | 84 | def get_default_values(para_dict): 85 | return para_dict.loc[['default']] 86 | 87 | 88 | def select_sub_dict(para_dict, param_choice): 89 | return para_dict[param_choice] 90 | 91 | 92 | def read_range_from_dict(para_dict): 93 | return np.array(para_dict.loc['range'].to_list()) 94 | 95 | 96 | def read_unit_from_dict(para_dict): 97 | return para_dict.loc['step'].to_numpy() 98 | 99 | 100 | # Log analysis function 101 | def read_path_specified_file(log_path, exe): 102 | """ 103 | :param log_path: 104 | :param exe: 105 | :return: 106 | """ 107 | file_list = [] 108 | for filename in os.listdir(log_path): 109 | if filename.endswith(f'.{exe}'): 110 | file_list.append(filename) 111 | file_list.sort() 112 | return file_list 113 | 114 | 115 | def rename_bin(log_path, ranges): 116 | file_list = read_path_specified_file(log_path, 'BIN') 117 | # 列出文件夹内所有.BIN结尾的文件并排序 118 | for file, num in zip(file_list, range(ranges[0], ranges[1])): 119 | name, _ = file.split('.') 120 | os.rename(f"{log_path}/{file}", f"{log_path}/{str(num).zfill(8)}.BIN") 121 | 122 | 123 | def min_max_scaler_param(param_value): 124 | # If param.shape != predictor's all params. 125 | if param_value.shape[1] != load_param().shape[1]: 126 | para_dict = load_sub_param() 127 | else: 128 | para_dict = load_param() 129 | param_choice_dict = para_dict 130 | #participle_param = toolConfig.PARAM 131 | #param_choice_dict = select_sub_dict(para_dict, participle_param) 132 | 133 | param_bounds = read_range_from_dict(param_choice_dict) 134 | lb = param_bounds[:, 0] 135 | ub = param_bounds[:, 1] 136 | param_value = (param_value - lb) / (ub-lb) 137 | return param_value.astype(float) 138 | 139 | 140 | def return_min_max_scaler_param(param_value: object) -> object: 141 | param = load_param() 142 | param_bounds = read_range_from_dict(param) 143 | lb = param_bounds[:, 0] 144 | ub = param_bounds[:, 1] 145 | param_value = (param_value * (ub-lb)) + lb 146 | return param_value 147 | 148 | 149 | def min_max_scaler(trans, values): 150 | status_value = values[:, :toolConfig.STATUS_LEN] 151 | param_value = values[:, toolConfig.STATUS_LEN:] 152 | 153 | param_value = min_max_scaler_param(param_value) 154 | 155 | status_value = trans.transform(status_value) 156 | 157 | return np.c_[status_value, param_value] 158 | 159 | 160 | def return_min_max_scaler(trans, values): 161 | status_value = values[:, :toolConfig.STATUS_LEN] 162 | param_value = values[:, toolConfig.STATUS_LEN:] 163 | 164 | param_value = return_min_max_scaler_param(param_value) 165 | 166 | status_value = trans.transform(status_value) 167 | 168 | return np.c_[status_value, param_value] 169 | 170 | 171 | def pad_configuration_default_value(params_value): 172 | para_dict = load_param() 173 | # default values 174 | all_default_value = para_dict.loc[['default']] 175 | all_default_value = pd.concat([all_default_value]*params_value.shape[0]) 176 | # replace values 177 | participle_param = toolConfig.PARAM_PART 178 | all_default_value[participle_param] = params_value 179 | return all_default_value.values -------------------------------------------------------------------------------- /range/rangesearcher.py: -------------------------------------------------------------------------------- 1 | import geatpy as ea 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import pickle 5 | from abc import ABC, abstractmethod 6 | 7 | from Cptool.config import toolConfig 8 | from Cptool.gaMavlink import GaMavlinkAPM 9 | from Cptool.mavtool import load_param, select_sub_dict, read_unit_from_dict, get_default_values, read_range_from_dict 10 | from range.rangeproblem import RangeProblem, GARangeProblem 11 | 12 | 13 | class BaseRangeOptimizer(ABC): 14 | def __init__(self): 15 | self.problem = None 16 | self.start_value = None 17 | self._setup_params() 18 | 19 | def _setup_params(self): 20 | """Initialize parameter settings""" 21 | self.participle_param = toolConfig.PARAM 22 | para_dict = load_param() 23 | self.param_choice_dict = select_sub_dict(para_dict, self.participle_param) 24 | self.step_unit = read_unit_from_dict(self.param_choice_dict) 25 | self.default_pop = get_default_values(self.param_choice_dict) 26 | self.sub_value_range = read_range_from_dict(self.param_choice_dict) 27 | 28 | def set_bounds(self): 29 | """Set problem bounds and step size""" 30 | self.problem.init_bounds_and_step(self.sub_value_range, self.step_unit) 31 | 32 | class GARangeOptimizer(BaseRangeOptimizer): 33 | def __init__(self, result_data): 34 | super().__init__() 35 | self._init_ga_problem(result_data) 36 | self.NDSet = None 37 | self.population = None 38 | self.algorithm = None 39 | 40 | def _init_ga_problem(self, result_data): 41 | """Initialize GA problem parameters""" 42 | Dim = self.sub_value_range.shape[0] * 2 43 | varTypes = [1] * Dim # 0: continuous, 1: discrete 44 | 45 | # Initialize bounds 46 | lb = np.repeat(self.sub_value_range[:, 0] / self.step_unit, 2) 47 | lb[1::2] = self.default_pop // self.step_unit 48 | ub = np.repeat(self.sub_value_range[:, 1] / self.step_unit, 2) 49 | ub[::2] = self.default_pop // self.step_unit 50 | 51 | # Boundary inclusion flags 52 | lbin = [1] * Dim # Include lower bounds 53 | ubin = [1] * Dim # Include upper bounds 54 | 55 | self.problem = GARangeProblem( 56 | name='ANAProblem', 57 | M=2, # Number of objectives 58 | maxormins=[-1, -1], # Maximize both objectives 59 | Dim=self.sub_value_range.shape[0], 60 | varTypes=varTypes, 61 | lb=lb, ub=ub, 62 | lbin=lbin, ubin=ubin, 63 | result_data=result_data 64 | ) 65 | 66 | def run(self): 67 | """Execute optimization process""" 68 | population = self._init_population() 69 | self.algorithm = self._setup_algorithm(population) 70 | self.NDSet, self.population = self.algorithm.run() 71 | self._save_results() 72 | self._calculate_metrics() 73 | 74 | def _init_population(self, size=3000): 75 | """Initialize population""" 76 | Field = ea.crtfld('RI', self.problem.varTypes, 77 | self.problem.ranges, 78 | self.problem.borders) 79 | return ea.Population('RI', Field, size) 80 | 81 | def _setup_algorithm(self, population): 82 | """Configure NSGA-II algorithm parameters""" 83 | algorithm = ea.moea_NSGA2_templet(self.problem, population) 84 | algorithm.MAXGEN = 300 85 | algorithm.mutOper.Pm = 0.5 86 | algorithm.recOper.XOVR = 0.9 87 | algorithm.maxTrappedCount = 10 88 | algorithm.drawing = 1 89 | return algorithm 90 | 91 | def _save_results(self): 92 | """Save optimization results""" 93 | with open('NDSetnew.pkl', 'wb') as f: 94 | pickle.dump(self.NDSet, f) 95 | self.NDSet.save() # Save non-dominated set to file 96 | ea.moeaplot(self.NDSet.ObjV, xyzLabel=['No. of Validated Configuration', 'Incorrect/Validated Ratio']) 97 | print('用时：%s 秒' % (self.algorithm.passTime)) 98 | print('非支配个体数：%s 个' % (self.NDSet.sizes)) 99 | print('单位时间找到帕累托前沿点个数：%s 个' % (int(self.NDSet.sizes // self.algorithm.passTime))) 100 | 101 | def _calculate_metrics(self): 102 | """Calculate performance metrics""" 103 | PF = self.problem.getReferObjV() # Get true Pareto front 104 | if PF is not None and self.NDSet.sizes != 0: 105 | GD = ea.indicator.GD(self.NDSet.ObjV, PF) # Calculate GD metric 106 | IGD = ea.indicator.IGD(self.NDSet.ObjV, PF) # Calculate IGD metric 107 | HV = ea.indicator.HV(self.NDSet.ObjV, PF) # Calculate HV metric 108 | Spacing = ea.indicator.Spacing(self.NDSet.ObjV) # Calculate Spacing metric 109 | print('GD', GD) 110 | print('IGD', IGD) 111 | print('HV', HV) 112 | print('Spacing', Spacing) 113 | if PF is not None: 114 | metricName = [['IGD'], ['HV']] 115 | [NDSet_trace, Metrics] = ea.indicator.moea_tracking(self.algorithm.pop_trace, PF, metricName, 116 | self.problem.maxormins) 117 | ea.trcplot(Metrics, labels=metricName, titles=metricName) 118 | 119 | def return_best_n_gen(self, n=1): 120 | if (self.algorithm is None) or (self.obj_trace is None) or (self.var_trace is None): 121 | raise ValueError('Please run() at first') 122 | 123 | candidate = self.problem.maxormins * self.obj_trace[:, 1] 124 | 125 | if n == 0: 126 | top_gen = np.zeros(len(candidate), dtype=int) 127 | for i in range(len(candidate)): 128 | min_index = np.argmin(candidate) 129 | top_gen[i] = min_index # 记录最优种群个体是在哪一代 130 | candidate[min_index] = 0 131 | else: 132 | top_gen = np.zeros(n, dtype=int) 133 | for i in range(n): 134 | min_index = np.argmin(candidate) 135 | top_gen[i] = min_index # 记录最优种群个体是在哪一代 136 | candidate[min_index] = 0 137 | params = self.var_trace[top_gen, :] 138 | return self.problem.reasonable_range(params) 139 | 140 | 141 | -------------------------------------------------------------------------------- /uavga/problem.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import pickle 3 | 4 | import geatpy as ea 5 | import numpy as np 6 | import pandas as pd 7 | from tensorflow.python.keras.models import load_model 8 | from abc import ABC, abstractmethod 9 | 10 | from Cptool.gaMavlink import GaMavlinkAPM 11 | from Cptool.config import toolConfig 12 | from Cptool.mavtool import min_max_scaler_param, load_param, read_unit_from_dict, pad_configuration_default_value, \ 13 | select_sub_dict 14 | from ModelFit.approximate import CyLSTM 15 | 16 | 17 | class BaseProblem(ABC): 18 | def __init__(self): 19 | self.status_data = None 20 | self.predictor = None 21 | self.param_bounds = None 22 | self.step = None 23 | 24 | @abstractmethod 25 | def evaluate(self, x, y): 26 | pass 27 | 28 | def param_value2step(self, configuration): 29 | np_config = np.ceil(configuration / self.step) * self.step 30 | return pd.DataFrame([np_config.tolist()], 31 | columns=toolConfig.PARAM).iloc[0].to_dict() 32 | 33 | 34 | class ProblemGA(BaseProblem, ea.Problem): 35 | def __init__(self, name, M, maxormins, Dim, varTypes, lb, ub, lbin, ubin): 36 | BaseProblem.__init__(self) 37 | ea.Problem.__init__(self, name, M, maxormins, Dim, 38 | varTypes, lb, ub, lbin, ubin) 39 | 40 | def aimFunc(self, pop): 41 | x = self._preprocess_population(pop) 42 | predicted_feature, feature_y = self._get_predictions(x) 43 | pop.ObjV = self._calculate_loss(predicted_feature, feature_y) 44 | 45 | def _preprocess_population(self, pop): 46 | """Preprocess population data""" 47 | x = pop.Phen 48 | x = self.reasonable_range(x).to_numpy() 49 | if x.shape[1] != load_param().shape[1]: 50 | x = pad_configuration_default_value(x) 51 | return x 52 | 53 | def _get_predictions(self, x): 54 | """Get model predictions""" 55 | param = min_max_scaler_param(x) 56 | merge_data = self._prepare_merge_data(param) 57 | feature_x, feature_y = self.predictor.data_split_3d(merge_data) 58 | predicted_feature = self.predictor.predict_feature(feature_x) 59 | 60 | dims = (x.shape[0], -1, predicted_feature.shape[-1]) 61 | return (predicted_feature.reshape(dims), 62 | feature_y.reshape(dims)) 63 | 64 | def _prepare_merge_data(self, param): 65 | """Prepare merged data for prediction""" 66 | status = self.status_data.reshape((1, self.status_data.shape[0], 67 | -1, toolConfig.DATA_LEN)) 68 | status = status[:, :, :, :toolConfig.STATUS_LEN] 69 | param = param.reshape((param.shape[0], 1, 1, -1)) 70 | 71 | repeat_status = np.repeat(status, param.shape[0], axis=0) 72 | repeat_param = np.repeat(param, repeat_status.shape[2], axis=2) 73 | repeat_param = np.repeat(repeat_param, repeat_status.shape[1], axis=1) 74 | 75 | merge_data = np.c_[repeat_status, repeat_param] 76 | return merge_data.reshape((merge_data.shape[0], 77 | merge_data.shape[1], -1)).astype(np.float) 78 | 79 | def _calculate_loss(self, predicted, actual): 80 | """Calculate prediction loss""" 81 | return self.predictor.cal_patch_deviation(predicted, actual).reshape((-1, 1)) 82 | 83 | def reasonable_range(self, param): 84 | """ 85 | Restore data to original scale 86 | Args: 87 | param: Input parameters 88 | Returns: 89 | Scaled parameters as DataFrame 90 | """ 91 | np_config = param * self.step 92 | np_config = pd.DataFrame(np_config, columns=toolConfig.PARAM_PART) 93 | return np_config 94 | 95 | @staticmethod 96 | def reasonable_range_static(param): 97 | """ 98 | Restore data to original scale using static method 99 | Args: 100 | param: Input parameters 101 | Returns: 102 | Scaled parameters 103 | """ 104 | para_dict = load_param() 105 | param_choice_dict = select_sub_dict(para_dict, toolConfig.PARAM_PART) 106 | step_unit = read_unit_from_dict(param_choice_dict) 107 | return param * step_unit 108 | 109 | 110 | class ProblemGAOld(BaseProblem, ea.Problem): 111 | def __init__(self, name, M, maxormins, Dim, varTypes, lb, ub, lbin, ubin): 112 | BaseProblem.__init__(self) 113 | ea.Problem.__init__(self, name, M, maxormins, Dim, 114 | varTypes, lb, ub, lbin, ubin) 115 | 116 | def aimFunc(self, pop): 117 | # 得到决策变量矩阵 118 | x = pop.Phen 119 | x = self.reasonable_range(x).to_numpy() 120 | param = min_max_scaler_param(x) 121 | 122 | # Statue change 123 | status_data = self.status_data.reshape((1, 1, -1, toolConfig.DATA_LEN))[:, :, :, :toolConfig.STATUS_LEN] 124 | param = param.reshape((param.shape[0], 1, 1, -1)) 125 | # repeat data 126 | repeat_status = np.repeat(status_data, param.shape[0], axis=0) 127 | repeat_param = np.repeat(param, repeat_status.shape[2], axis=2) 128 | repeat_param = np.repeat(repeat_param, repeat_status.shape[1], axis=1) 129 | 130 | # Merge 131 | merge_data = np.c_[repeat_status, repeat_param] 132 | # Reshape 133 | merge_data = merge_data.reshape((merge_data.shape[0], merge_data.shape[1], -1)).astype(np.float) 134 | 135 | # create predicted status of this status patch 136 | feature_x, feature_y = self.predictor.data_split_3d(merge_data) 137 | # Predict 138 | predicted_feature = self.predictor.predict_feature(feature_x) 139 | # reshape to 3D (x number, status patch) 140 | predicted_feature = predicted_feature.reshape((x.shape[0], -1, predicted_feature.shape[-1])) 141 | feature_y = feature_y.reshape((x.shape[0], -1, predicted_feature.shape[-1])) 142 | # deviation loss 143 | patch_array_loss = self.predictor.cal_deviation_old(predicted_feature, feature_y) 144 | 145 | pop.ObjV = patch_array_loss.reshape((-1, 1)) 146 | 147 | def reasonable_range(self, param): 148 | """ 149 | Restore data to original scale 150 | Args: 151 | param: Input parameters 152 | Returns: 153 | Scaled parameters as DataFrame 154 | """ 155 | np_config = param * self.step 156 | np_config = pd.DataFrame(np_config, columns=toolConfig.PARAM) 157 | return np_config 158 | 159 | @staticmethod 160 | def reasonable_range_static(param): 161 | """ 162 | Restore data to original scale using static method 163 | Args: 164 | param: Input parameters 165 | Returns: 166 | Scaled parameters 167 | """ 168 | para_dict = load_param() 169 | param_choice_dict = select_sub_dict(para_dict, toolConfig.PARAM_PART) 170 | step_unit = read_unit_from_dict(param_choice_dict) 171 | return param * step_unit -------------------------------------------------------------------------------- /uavga/searcher.py: -------------------------------------------------------------------------------- 1 | import geatpy as ea 2 | import numpy as np 3 | from abc import ABC, abstractmethod 4 | 5 | from Cptool.config import toolConfig 6 | from Cptool.gaMavlink import GaMavlinkAPM 7 | from Cptool.mavtool import load_param, select_sub_dict, get_default_values, read_unit_from_dict, read_range_from_dict 8 | from ModelFit.approximate import CyLSTM 9 | from uavga.problem import ProblemGA, Problem, ProblemGAOld 10 | 11 | 12 | class BaseSearchOptimizer(ABC): 13 | def __init__(self): 14 | self.predictor = None 15 | self.problem = None 16 | self.param_bounds = None 17 | self.step_unit = None 18 | self._setup_params() 19 | 20 | def _setup_params(self): 21 | """Initialize parameter settings""" 22 | self.participle_param = toolConfig.PARAM_PART 23 | para_dict = load_param() 24 | self.param_choice_dict = select_sub_dict(para_dict, self.participle_param) 25 | self.step_unit = read_unit_from_dict(self.param_choice_dict) 26 | self.default_pop = get_default_values(self.param_choice_dict) 27 | self.sub_value_range = read_range_from_dict(self.param_choice_dict) 28 | 29 | @abstractmethod 30 | def start_optimize(self): 31 | pass 32 | 33 | @abstractmethod 34 | def return_best_n_gen(self, n=1): 35 | pass 36 | 37 | class GAOptimizer(BaseSearchOptimizer): 38 | def __init__(self): 39 | super().__init__() 40 | self._init_ga_problem() 41 | self.NDSet = None 42 | self.population = None 43 | self.algorithm = None 44 | 45 | def _init_ga_problem(self): 46 | """Initialize GA problem parameters""" 47 | Dim = self.sub_value_range.shape[0] 48 | varTypes = [1] * Dim # 0: continuous variable; 1: discrete variable 49 | lb = self.sub_value_range[:, 0] // self.step_unit # Lower bound 50 | ub = self.sub_value_range[:, 1] // self.step_unit # Upper bound 51 | lbin = [0] * Dim # Whether to include lower bound (0: no, 1: yes) 52 | ubin = [1] * Dim # Whether to include upper bound (0: no, 1: yes) 53 | 54 | # Initialize problem instance 55 | self.problem = ProblemGA( 56 | name='UAVProblem', 57 | M=1, # Number of objectives 58 | maxormins=[-1], # -1: maximize, 1: minimize 59 | Dim=Dim, 60 | varTypes=varTypes, 61 | lb=lb, ub=ub, 62 | lbin=lbin, ubin=ubin 63 | ) 64 | 65 | def start_optimize(self): 66 | """Execute optimization process""" 67 | population = self._init_population() 68 | self.algorithm = self._setup_algorithm(population) 69 | prophet_pop = self._create_prophet_population() 70 | self.algorithm.call_aimFunc(prophet_pop) 71 | self.NDSet, self.population = self.algorithm.run(prophet_pop) 72 | 73 | def _init_population(self, size=500): 74 | """Initialize population""" 75 | Field = ea.crtfld('RI', self.problem.varTypes, 76 | self.problem.ranges, 77 | self.problem.borders) 78 | return ea.Population('RI', Field, size) 79 | 80 | def _setup_algorithm(self, population): 81 | """Configure algorithm parameters""" 82 | algorithm = ea.soea_DE_currentToBest_1_bin_templet(self.problem, population) 83 | algorithm.MAXGEN = 50 84 | algorithm.mutOper.F = 0.7 85 | algorithm.recOper.XOVR = 0.7 86 | algorithm.trappedValue = 0.1 87 | algorithm.maxTrappedCount = 10 88 | algorithm.drawing = 0 89 | return algorithm 90 | 91 | def _create_prophet_population(self): 92 | """Create prophet population based on default values""" 93 | prophet_chrom = np.array(self.default_pop / self.step_unit, dtype=int) 94 | Field = ea.crtfld('RI', self.problem.varTypes, 95 | self.problem.ranges, 96 | self.problem.borders) 97 | return ea.Population('RI', Field, 1, prophet_chrom) 98 | 99 | def return_best_n_gen(self, n=1): 100 | """Return best n generations""" 101 | if not hasattr(self, 'BestIndi') or self.population is None: 102 | raise ValueError('Please run start_optimize() first') 103 | 104 | obj_trace = self.population.Phen 105 | var_trace = self.population.ObjV 106 | 107 | # Get unique candidates 108 | unique_indices = np.unique(var_trace, axis=0, return_index=True)[1] 109 | candidate_var = var_trace[unique_indices].reshape(-1) 110 | candidate_obj = obj_trace[unique_indices] 111 | 112 | # Sort candidates 113 | sort_indices = np.argsort(self.problem.maxormins * candidate_var) 114 | candidate_obj = candidate_obj[sort_indices] 115 | 116 | return self.problem.reasonable_range(candidate_obj)[:n] 117 | 118 | 119 | class GAOptimizerOld(GAOptimizer): 120 | def __init__(self): 121 | super(GAOptimizerOld, self).__init__() 122 | 123 | name = 'UAVProblem' 124 | M = 1 # Number of objectives 125 | maxormins = [-1] # Optimization direction (-1: maximize, 1: minimize) 126 | Dim = self.sub_value_range.shape[0] # Number of decision variables 127 | varTypes = [1] * Dim # Variable types (0: continuous, 1: discrete) 128 | lb = self.sub_value_range[:, 0] // self.step_unit # Lower bounds 129 | ub = self.sub_value_range[:, 1] // self.step_unit # Upper bounds 130 | lbin = [0] * Dim # Include lower bounds (0: no, 1: yes) 131 | ubin = [1] * Dim # Include upper bounds (0: no, 1: yes) 132 | 133 | # Initialize problem using parent class constructor 134 | self.problem = ProblemGAOld(name=name, M=M, maxormins=maxormins, Dim=Dim, 135 | varTypes=varTypes, lb=lb, ub=ub, lbin=lbin, ubin=ubin) 136 | 137 | # Result logging 138 | self.NDSet = None 139 | self.population = None 140 | self.algorithm = None 141 | 142 | def start_optimize(self): 143 | NINDs = 500 144 | Encoding = 'RI' # Encoding method 145 | Field = ea.crtfld(Encoding, self.problem.varTypes, self.problem.ranges, 146 | self.problem.borders) # Create region descriptor 147 | population = (ea.Population(Encoding, Field, NINDs)) # Instantiate population object (population not initialized yet) 148 | # Custom initialization of population soea_DE_currentToBest_1_bin_templet 149 | """===============================Setting=============================""" 150 | self.algorithm = ea.soea_DE_currentToBest_1_bin_templet(self.problem, population) # Instantiate algorithm template object 151 | self.algorithm.MAXGEN = 50 # Maximum number of generations 152 | self.algorithm.mutOper.F = 0.7 # Differential evolution parameter F 153 | self.algorithm.recOper.XOVR = 0.7 # Recombination probability 154 | self.algorithm.trappedValue = 0.1 # "Evolution stagnation" threshold 155 | self.algorithm.maxTrappedCount = 10 # Maximum limit of evolution stagnation counter 156 | self.algorithm.drawing = 0 157 | """===========================Create a population of prophets based on prior knowledge=======================""" 158 | prophetChrom = np.array(self.default_pop / self.step_unit, dtype=int) # Assume a known good chromosome 159 | prophetPop = ea.Population(Encoding, Field, 1, prophetChrom) # Instantiate population object (set number of individuals to 1) 160 | 161 | self.algorithm.call_aimFunc(prophetPop) 162 | """==========================Call the algorithm template for population evolution=======================""" 163 | [self.NDSet, self.population] = self.algorithm.run(prophetPop) 164 | 165 | def return_best_n_gen(self, n=1): 166 | if (self.BestIndi is None) or (self.population is None): 167 | raise ValueError('Please start_optimize() at first') 168 | 169 | obj_trace = self.population.Phen 170 | var_trace = self.population.ObjV 171 | 172 | obj_trace = np.array(obj_trace) 173 | var_trace = np.array(var_trace) 174 | 175 | # Remove duplicates 176 | candidate_var_index = np.unique(var_trace, axis=0, return_index=True)[1] 177 | candidate_var = var_trace[candidate_var_index].reshape(-1) 178 | candidate_obj = obj_trace[candidate_var_index] 179 | 180 | candidate = self.uavproblem.maxormins * candidate_var 181 | # Sort in ascending order 182 | candidate_index = np.argsort(candidate) 183 | candidate_obj = candidate_obj[candidate_index] 184 | 185 | return self.uavproblem.reasonable_range(candidate_obj)[:n] -------------------------------------------------------------------------------- /Cptool/config.py: -------------------------------------------------------------------------------- 1 | # coding:utf-8 2 | import json 3 | import time 4 | import yaml 5 | import os 6 | 7 | import pandas as pd 8 | 9 | 10 | class ToolConfig: 11 | class ConstError(PermissionError): 12 | pass 13 | 14 | class ConstCaseError(ConstError): 15 | pass 16 | 17 | def __init__(self): 18 | # Load YAML config with fallback to defaults 19 | self.yaml_config = self._load_yaml_config() 20 | self._init_defaults() 21 | 22 | def _load_yaml_config(self): 23 | """Load YAML config with fallback to empty dict""" 24 | try: 25 | config_path = os.path.join(os.path.dirname(__file__), 'config.yaml') 26 | with open(config_path, 'r') as f: 27 | return yaml.safe_load(f) 28 | except (FileNotFoundError, yaml.YAMLError) as e: 29 | print(f"Warning: Could not load config.yaml ({str(e)}), using defaults") 30 | return {} 31 | 32 | def _get_yaml_value(self, *keys, default=None): 33 | """Safely get nested YAML config value with fallback""" 34 | config = self.yaml_config 35 | for key in keys: 36 | if not isinstance(config, dict): 37 | return default 38 | config = config.get(key, default) 39 | return config 40 | 41 | def _init_defaults(self): 42 | """Initialize with YAML values or defaults""" 43 | self.__dict__["MODE"] = self._get_yaml_value('mode', default=None) 44 | self.__dict__["SPEED"] = self._get_yaml_value('simulation', 'speed', default=3) 45 | self.__dict__["HOME"] = self._get_yaml_value('simulation', 'home', default="AVC_plane") 46 | self.__dict__["DEBUG"] = self._get_yaml_value('debug', default=True) 47 | self.__dict__["WIND_RANGE"] = self._get_yaml_value('wind_range', default=[8, 10.7]) 48 | self.__dict__["HEIGHT"] = self._get_yaml_value('window', 'height', default=640) 49 | self.__dict__["WEIGHT"] = self._get_yaml_value('window', 'weight', default=480) 50 | self.__dict__["LIMIT_H"] = self._get_yaml_value('flight', 'limit_h', default=50) 51 | self.__dict__["LIMIT_L"] = self._get_yaml_value('flight', 'limit_l', default=40) 52 | self.__dict__["ARDUPILOT_LOG_PATH"] = self._get_yaml_value('paths', 'ardupilot_log_path', default='/media/rain/data') 53 | self.__dict__["SITL_PATH"] = self._get_yaml_value('paths', 'sitl_path', default="/home/rain/ardupilot/Tools/autotest/sim_vehicle.py") 54 | self.__dict__["AIRSIM_PATH"] = self._get_yaml_value('paths', 'airsim_path', default="/media/rain/data/airsim/Africa_Savannah/LinuxNoEditor/Africa_001.sh") 55 | self.__dict__["PX4_RUN_PATH"] = self._get_yaml_value('paths', 'px4_run_path', default='/home/rain/PX4-Autopilot') 56 | self.__dict__["JMAVSIM_PATH"] = self._get_yaml_value('paths', 'jmavsim_path', default="/home/rain/PX4-Autopilot/Tools/jmavsim_run.sh") 57 | self.__dict__["MORSE_PATH"] = self._get_yaml_value('paths', 'morse_path', default="/home/rain/ardupilot/libraries/SITL/examples/Morse/quadcopter.py") 58 | self.__dict__["CLUSTER_CHOICE_NUM"] = self._get_yaml_value('cluster_choice_num', default=10) 59 | 60 | def __setattr__(self, name, value): 61 | if name in self.__dict__: 62 | raise self.ConstError("can't change const %s" % name) 63 | if not name.isupper(): 64 | raise self.ConstCaseError('const name "%s" is not all uppercase' % name) 65 | self.__dict__[name] = value 66 | 67 | def __getattr__(self, item): 68 | if self.__dict__["MODE"] is None: 69 | raise ValueError("Set config Mode at first!") 70 | return self.__dict__[item] 71 | 72 | def select_mode(self, mode): 73 | if mode not in ["Ardupilot", "PX4"]: 74 | raise ValueError("Bad mode") 75 | # Change Mode 76 | self.__dict__["MODE"] = mode 77 | 78 | if mode == "Ardupilot": 79 | # Simulation Type 80 | # Ardupilot : ['Airsim', 'Morse', 'Gazebo', 'SITL'] 81 | self.__dict__["SIM"] = "SITL" # "Jmavsim" 82 | 83 | # Mavlink Part 84 | self.__dict__["LOG_MAP"] = ['IMU', 'ATT', 'RATE', 'PARM', 'VIBE', "MAG"] # "POS" 85 | # Online Mavlink Part 86 | self.__dict__["OL_LOG_MAP"] = ['ATTITUDE', 'RAW_IMU', 'VIBRATION'] # 'GLOBAL_POSITION_INT' 87 | # Status Order 88 | self.__dict__["STATUS_ORDER"] = ['TimeS', 'Roll', 'Pitch', 'Yaw', 'RateRoll', 'RatePitch', 'RateYaw', 89 | # 'Lat', 'Lng', 'Alt', 90 | 'AccX', 'AccY', 'AccZ', 'GyrX', 'GyrY', 'GyrZ', 91 | 'MagX', 'MagY', 'MagZ', 'VibeX', 'VibeY', 'VibeZ'] 92 | 93 | with open('Cptool/param_ardu.json', 'r') as f: 94 | param_name = pd.DataFrame(json.loads(f.read())).columns.tolist() 95 | self.__dict__["PARAM"] = param_name 96 | 97 | self.__dict__["PARAM_PART"] = [ 98 | "PSC_VELXY_P", 99 | "PSC_VELXY_I", 100 | "PSC_VELXY_D", 101 | "PSC_ACCZ_P", 102 | "PSC_ACCZ_I", 103 | "ATC_ANG_RLL_P", 104 | "ATC_RAT_RLL_P", 105 | "ATC_RAT_RLL_I", 106 | "ATC_RAT_RLL_D", 107 | "ATC_ANG_PIT_P", 108 | "ATC_RAT_PIT_P", 109 | "ATC_RAT_PIT_I", 110 | "ATC_RAT_PIT_D", 111 | "ATC_ANG_YAW_P", 112 | "ATC_RAT_YAW_P", 113 | "ATC_RAT_YAW_I", 114 | "ATC_RAT_YAW_D", 115 | "WPNAV_SPEED", 116 | "WPNAV_ACCEL", 117 | "ANGLE_MAX" 118 | ] 119 | 120 | # self.__dict__["PARAM"] = [ 121 | # "PSC_VELXY_P", 122 | # "INS_POS1_Z", 123 | # "INS_POS2_Z", 124 | # "INS_POS3_Z", 125 | # "WPNAV_SPEED", 126 | # "ANGLE_MAX" 127 | # ] 128 | elif mode == "PX4": 129 | # PX4 : ['Jmavsim'] 130 | self.__dict__["SIM"] = "Jmavsim" # "Jmavsim" 131 | 132 | now = time.localtime() 133 | now_time = time.strftime("%Y-%m-%d", now) 134 | # File path 135 | self.__dict__["PX4_LOG_PATH"] = f"{self.__dict__['PX4_RUN_PATH']}/build/px4_sitl_default/logs/{now_time}" 136 | # Status Order 137 | self.__dict__["STATUS_ORDER"] = ['TimeS', 'Roll', 'Pitch', 'Yaw', 'RateRoll', 'RatePitch', 'RateYaw', 138 | 'AccX', 'AccY', 'AccZ', 'GyrX', 'GyrY', 'GyrZ', 139 | 'MagX', 'MagY', 'MagZ', 'VibeX', 'VibeY', 'VibeZ'] 140 | 141 | with open('Cptool/param_px4.json', 'r') as f: 142 | param_name = pd.DataFrame(json.loads(f.read())).columns.tolist() 143 | self.__dict__["PARAM"] = param_name 144 | 145 | self.__dict__["PARAM_PART"] = [ 146 | "MC_ROLL_P", 147 | "MC_PITCH_P", 148 | "MC_YAW_P", 149 | "MC_YAW_WEIGHT", 150 | "MPC_XY_P", 151 | "MPC_Z_P", 152 | "MC_PITCHRATE_P", 153 | "MC_ROLLRATE_P", 154 | "MC_YAWRATE_P", 155 | "MPC_TILTMAX_AIR", 156 | "MIS_YAW_ERR", 157 | "MPC_Z_VEL_MAX_DN", 158 | "MPC_Z_VEL_MAX_UP", 159 | "MPC_TKO_SPEED" 160 | ] 161 | 162 | if len(self.__dict__["PARAM_PART"]) == len(self.__dict__["PARAM"]): 163 | self.__dict__["EXE"] = "" 164 | else: 165 | self.__dict__["EXE"] = len(self.__dict__["PARAM_PART"]) 166 | 167 | ###################### 168 | # Model Config # 169 | ###################### 170 | # Status length 171 | self.__dict__["STATUS_LEN"] = len(self.__dict__["STATUS_ORDER"]) - 1 172 | 173 | # Parameter length 174 | self.__dict__["PARAM_LEN"] = len(self.__dict__["PARAM"]) 175 | 176 | # Predictor input vector length 177 | self.__dict__["INPUT_LEN"] = 4 178 | # Predictor output vector length 179 | self.__dict__["OUTPUT_LEN"] = 1 180 | 181 | # input data entry length 182 | self.__dict__["DATA_LEN"] = self.__dict__["STATUS_LEN"] + len(toolConfig.PARAM) 183 | 184 | # Whole predictor input length 185 | self.__dict__["INPUT_DATA_LEN"] = self.__dict__["DATA_LEN"] * self.__dict__["INPUT_LEN"] 186 | 187 | # Whole predictor output length 188 | self.__dict__["OUTPUT_DATA_LEN"] = self.__dict__["STATUS_LEN"] * self.__dict__["OUTPUT_LEN"] 189 | 190 | # Vector length of a segment 191 | self.__dict__["SEGMENT_LEN"] = 10 + self.__dict__["INPUT_LEN"] 192 | 193 | # transform values 194 | self.__dict__["RETRANS"] = True 195 | 196 | def get(self, key, default=None): 197 | """Safe config getter with default value""" 198 | return self.__dict__.get(key, default) 199 | 200 | def validate_config(self): 201 | """Validate critical configuration values""" 202 | required = ['MODE', 'SITL_PATH', 'PARAM'] 203 | for key in required: 204 | if not self.__dict__.get(key): 205 | raise ValueError(f"Missing required config: {key}") 206 | 207 | if self.__dict__["MODE"] not in ["Ardupilot", "PX4"]: 208 | raise ValueError("Invalid MODE - must be 'Ardupilot' or 'PX4'") 209 | 210 | # Validate paths exist 211 | paths = ['SITL_PATH', 'PX4_RUN_PATH', 'ARDUPILOT_LOG_PATH'] 212 | for path_key in paths: 213 | path = self.__dict__.get(path_key) 214 | if path and not os.path.exists(path): 215 | print(f"Warning: Path does not exist: {path_key}={path}") 216 | 217 | 218 | toolConfig = ToolConfig() 219 | toolConfig.select_mode("ArduPilot") -------------------------------------------------------------------------------- /uavga/fuzzer.py: -------------------------------------------------------------------------------- 1 | import colorsys 2 | import pickle 3 | import random 4 | from abc import abstractmethod 5 | import scipy.cluster.hierarchy as hcluster 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | import pandas as pd 9 | from sklearn.cluster import MeanShift, estimate_bandwidth, DBSCAN 10 | from sklearn.decomposition import PCA 11 | from loguru import logger 12 | 13 | from Cptool.config import toolConfig 14 | from Cptool.gaSimManager import GaSimManager 15 | from Cptool.mavtool import min_max_scaler_param 16 | from ModelFit.approximate import CyLSTM, Modeling 17 | from uavga.problem import ProblemGA 18 | from uavga.searcher import SearchOptimizer, GAOptimizer, GAOptimizerOld 19 | 20 | 21 | def split_segment(csv_data): 22 | """ 23 | select status data and split to multiple segments 24 | :return: 25 | """ 26 | # Drop configuration (parameter values) 27 | tmp = csv_data.to_numpy()[:, :toolConfig.STATUS_LEN] 28 | # To prevent unbalanced 29 | tmp = tmp[:-(tmp.shape[0] % (toolConfig.SEGMENT_LEN + 1)), :] 30 | # Split 31 | tmp_split = np.array_split(tmp, tmp.shape[0] // (toolConfig.SEGMENT_LEN + 1), axis=0) 32 | 33 | return np.array(tmp_split) 34 | 35 | 36 | def random_choice_dbscan(segment_csv, eps=0.5): 37 | # 3D to 2D 38 | data_class = segment_csv.reshape( 39 | (-1, segment_csv.shape[1] * segment_csv.shape[2])) 40 | # Cluster 41 | clf = DBSCAN(eps=eps, min_samples=5).fit(data_class) 42 | # Cluster reuslt 43 | predicted = clf.labels_ 44 | logger.info(f'DBSCAN class: {max(predicted)}') 45 | 46 | # ------------- draw ------------------# 47 | c = list(map(lambda x: color(tuple(x)), ncolors(max(predicted) + 1))) 48 | 49 | colors = [c[i] for i in predicted] 50 | 51 | pca = PCA(n_components=2, svd_solver='arpack') 52 | show = pca.fit_transform(data_class) 53 | 54 | fig = plt.figure() 55 | # ax = fig.add_subplot(111, projection='3d') 56 | # ax.scatter(show[:, 0], show[:, 1], show[:, 2], c=colors, s=5) 57 | plt.scatter(show[:, 0], show[:, 1], c=colors, s=5) 58 | 59 | plt.show() 60 | # ------------------------- 61 | 62 | out = [] 63 | for i in range(max(predicted)): 64 | index = np.where(predicted == i)[0] 65 | col_index = np.random.choice(index, min(index.shape[0], toolConfig.CLUSTER_CHOICE_NUM)) 66 | select = segment_csv[col_index] 67 | out.extend(select) 68 | out = np.array(out) 69 | return out 70 | 71 | 72 | def random_choice_hierarchical(segment_csv, rate=0.5): 73 | # 3D to 2D 74 | data_class = segment_csv.reshape( 75 | (-1, segment_csv.shape[1] * segment_csv.shape[2])) 76 | 77 | # clustering 78 | thresh = 0.3 79 | predicted = hcluster.fclusterdata(data_class, thresh, criterion="distance") 80 | # Cluster reuslt 81 | logger.info(f'Meanshift class: {max(predicted)}') 82 | # ------------- draw ------------------# 83 | for i in range(max(predicted)): 84 | index = np.where(predicted == i)[0] 85 | 86 | c = list(map(lambda x: color(tuple(x)), ncolors(max(predicted) + 1))) 87 | 88 | colors = [c[i] for i in predicted] 89 | 90 | pca = PCA(n_components=2, svd_solver='arpack') 91 | show = pca.fit_transform(data_class) 92 | 93 | fig = plt.figure() 94 | # ax = fig.add_subplot(111, projection='3d') 95 | # ax.scatter(show[:, 0], show[:, 1], show[:, 2], c=colors, s=5) 96 | plt.scatter(show[:, 0], show[:, 1], c=colors, s=5) 97 | 98 | plt.show() 99 | # ------------------------- 100 | 101 | out = [] 102 | for i in range(max(predicted)): 103 | index = np.where(predicted == i)[0] 104 | col_index = np.random.choice(index, min(index.shape[0], toolConfig.CLUSTER_CHOICE_NUM)) 105 | select = segment_csv[col_index] 106 | out.extend(select) 107 | out = np.array(out) 108 | return out 109 | 110 | 111 | def run_fuzzing(np_data, num=0): 112 | """ 113 | Start Fuzzing 114 | :param num: The number of data to join cluster 115 | :return: 116 | """ 117 | 118 | predictor = CyLSTM(100, 100, toolConfig.DEBUG) 119 | predictor.read_model() 120 | 121 | gaOptimizer = GAOptimizer() 122 | gaOptimizer.set_bounds() 123 | gaOptimizer.set_predictor(predictor) 124 | 125 | segment_csv = np_data 126 | # meanshift cluster 127 | if num != 0: 128 | # Random select 129 | index = np.random.choice(np.arange(segment_csv.shape[0]), num) 130 | segment_csv = segment_csv[index, :, :] 131 | segment_csv = random_choice_dbscan(segment_csv, eps=0.4) 132 | 133 | obj_population = [] # 种群 134 | 135 | for i, context in enumerate(segment_csv): 136 | # Pre process 137 | context = np.c_[context, np.zeros((context.shape[0], len(toolConfig.PARAM)))] 138 | context = Modeling.series_to_supervised(context, toolConfig.INPUT_LEN, toolConfig.OUTPUT_LEN).values 139 | 140 | gaOptimizer.problem.init_status(context) 141 | gaOptimizer.start_optimize() 142 | obj_population.append(gaOptimizer.population) 143 | 144 | print(f'------------------- {i + 1} / {segment_csv.shape[0]} -----------------') 145 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'wb') as f: 146 | pickle.dump(obj_population, f) 147 | 148 | 149 | def return_best_n_gen(n=1): 150 | candidate_vars = [] 151 | candidate_objs = [] 152 | 153 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 154 | obj_populations = pickle.load(f) 155 | for pop in obj_populations: 156 | pop_v = pop.ObjV 157 | pop_p = pop.Phen 158 | 159 | candidate_var_index = np.unique(pop_p, axis=0, return_index=True)[1] 160 | 161 | pop_v = pop_v[candidate_var_index] 162 | pop_p = pop_p[candidate_var_index] 163 | 164 | candidate = [-1] * pop_v 165 | 166 | candidate_index = np.argsort(candidate.reshape(-1)) 167 | pop_v = pop_v[candidate_index].reshape((-1, 1)) 168 | pop_p = pop_p[candidate_index].reshape((-1, 20)) 169 | 170 | if n != 0: 171 | candidate_var = pop_v[:min(n, len(pop_v))] 172 | candidate_obj = pop_p[:min(n, len(pop_p))] 173 | candidate_obj = ProblemGA.reasonable_range_static(candidate_obj) 174 | 175 | candidate_vars.extend(candidate_var) 176 | candidate_objs.extend(candidate_obj) 177 | 178 | return candidate_vars, candidate_objs 179 | 180 | 181 | def return_random_n_gen(n=1): 182 | candidate_vars = [] 183 | candidate_objs = [] 184 | 185 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 186 | obj_populations = pickle.load(f) 187 | for pop in obj_populations: 188 | pop_v = pop.ObjV 189 | pop_p = pop.Phen 190 | # Unique 191 | candidate_var_index = np.unique(pop_p, axis=0, return_index=True)[1] 192 | # Choice 193 | pop_v = pop_v[candidate_var_index] 194 | pop_p = pop_p[candidate_var_index] 195 | # 196 | candidate = [-1] * pop_v 197 | # Sort 198 | candidate_index = np.argsort(candidate.reshape(-1)) 199 | pop_v = pop_v[candidate_index].reshape((-1, 1)) 200 | pop_p = pop_p[candidate_index].reshape((-1, 20)) 201 | # 202 | if n != 0: 203 | candidate_var = pop_v[[1, 200, 500]] 204 | candidate_obj = pop_p[[1, 200, 500], :] 205 | candidate_obj = ProblemGA.reasonable_range_static(candidate_obj) 206 | 207 | candidate_vars.extend(candidate_var) 208 | candidate_objs.extend(candidate_obj) 209 | return candidate_vars, candidate_objs 210 | 211 | 212 | def return_cluster_thres_gen(thres=0.4): 213 | candidate_vars = [] 214 | candidate_objs = [] 215 | 216 | with open(f'result/{toolConfig.MODE}/pop{toolConfig.EXE}.pkl', 'rb') as f: 217 | obj_populations = pickle.load(f) 218 | for pop in obj_populations: 219 | pop_v = pop.ObjV 220 | pop_p = pop.Phen 221 | # Unique 222 | candidate_var_index = np.unique(pop_p, axis=0, return_index=True)[1] 223 | # Choice 224 | pop_v = pop_v[candidate_var_index] 225 | pop_p = pop_p[candidate_var_index] 226 | 227 | # To normal value 228 | normal_pop_p = ProblemGA.reasonable_range_static(pop_p) 229 | normalize_pop_p = min_max_scaler_param(normal_pop_p) 230 | 231 | predicted = hcluster.fclusterdata(normalize_pop_p, thres, criterion="distance") 232 | # ------------- draw ------------------# 233 | # c = list(map(lambda x: color(tuple(x)), ncolors(max(predicted) + 1))) 234 | # 235 | # colors = [c[i] for i in predicted] 236 | # 237 | # pca = PCA(n_components=2, svd_solver='arpack') 238 | # show = pca.fit_transform(pop_p) 239 | # 240 | # fig = plt.figure() 241 | # # ax = fig.add_subplot(111, projection='3d') 242 | # # ax.scatter(show[:, 0], show[:, 1], show[:, 2], c=colors, s=5) 243 | # plt.scatter(show[:, 0], show[:, 1], c=colors, s=5) 244 | # 245 | # plt.show() 246 | # ------------------------- 247 | for i in range(max(predicted)): 248 | index = np.where(predicted == i)[0] 249 | col_index = np.random.choice(index, min(index.shape[0], toolConfig.CLUSTER_CHOICE_NUM)) 250 | if len(col_index) > 0: 251 | candidate_vars.extend(pop_v[col_index]) 252 | candidate_objs.extend(normal_pop_p[col_index]) 253 | # Array 254 | candidate_objs = np.array(candidate_objs).astype(np.float) 255 | candidate_vars = np.array(candidate_vars).astype(np.float) 256 | return candidate_vars, candidate_objs 257 | 258 | 259 | def reshow(params, values): 260 | manager = GaSimManager(debug=toolConfig.DEBUG) 261 | manager.start_multiple_sitl() 262 | manager.mav_monitor_init() 263 | 264 | manager.mav_monitor_connect() 265 | manager.mav_monitor_set_mission("Cptool/mission.txt", random=True) 266 | 267 | manager.mav_monitor_set_param(params=params, values=values) 268 | 269 | # manager.start_mav_monitor() 270 | manager.mav_monitor_start_mission() 271 | result = manager.mav_monitor_error() 272 | 273 | manager.stop_sitl() 274 | 275 | 276 | def ncolors(num): 277 | rgb_colors = [] 278 | if num < 1: 279 | return rgb_colors 280 | hls_colors = get_n_hls_colors(num) 281 | for hlsc in hls_colors: 282 | _r, _g, _b = colorsys.hls_to_rgb(hlsc[0], hlsc[1], hlsc[2]) 283 | r, g, b = [int(x * 255.0) for x in (_r, _g, _b)] 284 | rgb_colors.append([r, g, b]) 285 | 286 | return rgb_colors 287 | 288 | 289 | def get_n_hls_colors(num): 290 | hls_colors = [] 291 | i = 0 292 | step = 360.0 / num 293 | while i < 360: 294 | h = i 295 | s = 90 + random.random() * 10 296 | l = 50 + random.random() * 10 297 | _hlsc = [h / 360.0, l / 100.0, s / 100.0] 298 | hls_colors.append(_hlsc) 299 | i += step 300 | 301 | return hls_colors 302 | 303 | 304 | def color(value): 305 | digit = list(map(str, range(10))) + list("ABCDEF") 306 | if isinstance(value, tuple): 307 | string = '#' 308 | for i in value: 309 | a1 = i // 16 310 | a2 = i % 16 311 | string += digit[a1] + digit[a2] 312 | return string 313 | elif isinstance(value, str): 314 | a1 = digit.index(value[1]) * 16 + digit.index(value[2]) 315 | a2 = digit.index(value[3]) * 16 + digit.index(value[4]) 316 | a3 = digit.index(value[5]) * 16 + digit.index(value[6]) 317 | return (a1, a2, a3) 318 | -------------------------------------------------------------------------------- /Cptool/gaSimManager.py: -------------------------------------------------------------------------------- 1 | """ 2 | SimManager Version: 4.0 22-10-24 3 | """ 4 | import logging 5 | import math 6 | import multiprocessing 7 | import os 8 | import re 9 | import sys 10 | import time 11 | from typing import Type 12 | 13 | import numpy as np 14 | import pexpect 15 | from numpy.dual import norm 16 | from pexpect import spawn 17 | from pymavlink import mavextra, mavwp, mavutil 18 | 19 | from Cptool.gaMavlink import GaMavlinkAPM, DroneMavlink 20 | from Cptool.config import toolConfig 21 | from Cptool.mavtool import Location 22 | from loguru import logger 23 | 24 | 25 | class SimManager(object): 26 | 27 | def __init__(self, debug: bool = False): 28 | self._sim_task = None 29 | self._sitl_task = None 30 | self.sim_monitor = None 31 | self.mav_monitor = None 32 | self._even = None 33 | self.sim_msg_queue = multiprocessing.Queue() 34 | self.mav_msg_queue = multiprocessing.Queue() 35 | 36 | if debug: 37 | logger.remove() 38 | logger.add(sys.stderr, format="{time} {file} {line} {level} {message}", level="DEBUG") 39 | else: 40 | logger.remove() 41 | logger.add(sys.stderr, format="{time} {file} {line} {level} {message}", level="INFO") 42 | 43 | """ 44 | Base Function 45 | """ 46 | def start_sim(self): 47 | """ 48 | start simulator 49 | :return: 50 | """ 51 | # Airsim 52 | cmd = None 53 | if toolConfig.SIM == 'Airsim': 54 | cmd = f'gnome-terminal -- {toolConfig.AIRSIM_PATH} ' \ 55 | f'-ResX={toolConfig.HEIGHT} -ResY={toolConfig.WEIGHT} -windowed' 56 | if toolConfig.SIM == 'Jmavsim': 57 | cmd = f'gnome-terminal -- bash {toolConfig.JMAVSIM_PATH}' 58 | if toolConfig.SIM == 'Morse': 59 | cmd = f'gnome-terminal -- morse run {toolConfig.MORSE_PATH}' 60 | if toolConfig.SIM == 'Gazebo': 61 | cmd = f'gnome-terminal -- gazebo --verbose worlds/iris_arducopter_runway.world' 62 | if cmd is None: 63 | raise ValueError('Not support mode') 64 | logger.info(f'Start Simulator {toolConfig.SIM}') 65 | self._sim_task = pexpect.spawn(cmd) 66 | 67 | def start_multiple_sim(self, drone_i=0): 68 | """ 69 | start multiple simulator (only jmavsim now) 70 | :return: 71 | """ 72 | # Airsim 73 | cmd = None 74 | if toolConfig.SIM == 'Jmavsim': 75 | port = 4560 + int(drone_i) 76 | cmd = f'{toolConfig.JMAVSIM_PATH} -p {port} -l' 77 | self._sim_task = pexpect.spawn(cmd, cwd=toolConfig.PX4_RUN_PATH, timeout=30, encoding='utf-8') 78 | 79 | def start_sitl(self): 80 | """ 81 | start the simulator 82 | :return: 83 | """ 84 | if os.path.exists(f"{toolConfig.ARDUPILOT_LOG_PATH}/eeprom.bin") and toolConfig.MODE == "Ardupilot": 85 | os.remove(f"{toolConfig.ARDUPILOT_LOG_PATH}/eeprom.bin") 86 | if os.path.exists(f"{toolConfig.ARDUPILOT_LOG_PATH}/mav.parm") and toolConfig.MODE == "Ardupilot": 87 | os.remove(f"{toolConfig.ARDUPILOT_LOG_PATH}/mav.parm") 88 | if os.path.exists(f"{toolConfig.PX4_RUN_PATH}/build/px4_sitl_default/tmp/rootfs/eeprom/parameters_10016") \ 89 | and toolConfig.MODE == "PX4": 90 | os.remove(f"{toolConfig.PX4_RUN_PATH}/build/px4_sitl_default/tmp/rootfs/eeprom/parameters_10016") 91 | 92 | cmd = None 93 | if toolConfig.MODE == 'Ardupilot': 94 | if toolConfig.SIM == 'Airsim': 95 | if toolConfig.HOME is not None: 96 | cmd = f"python3 {toolConfig.SITL_PATH} -v ArduCopter " \ 97 | f"--location={toolConfig.HOME}" \ 98 | f" -f airsim-copter --out=127.0.0.1:14550 --out=127.0.0.1:14540 " \ 99 | f" -S {toolConfig.SPEED}" 100 | else: 101 | cmd = f"python3 {toolConfig.SITL_PATH} -v ArduCopter -f airsim-copter " \ 102 | f"--out=127.0.0.1:14550 --out=127.0.0.1:14540 -S {toolConfig.SPEED}" 103 | if toolConfig.SIM == 'Morse': 104 | cmd = f"python3 {toolConfig.SITL_PATH} -v ArduCopter --model morse-quad " \ 105 | f"--add-param-file=/home/rain/ardupilot/libraries/SITL/examples/Morse/quadcopter.parm " \ 106 | f"--out=127.0.0.1:14550 -S {toolConfig.SPEED}" 107 | if toolConfig.SIM == 'Gazebo': 108 | cmd = f'python3 {toolConfig.SITL_PATH} -f gazebo-iris -v ArduCopter ' \ 109 | f'--out=127.0.0.1:14550 -S {toolConfig.SPEED}' 110 | if toolConfig.SIM == 'SITL': 111 | if toolConfig.HOME is not None: 112 | cmd = f"python3 {toolConfig.SITL_PATH} --location={toolConfig.HOME} " \ 113 | f"--out=127.0.0.1:14550 --out=127.0.0.1:14540 -v ArduCopter -w -S {toolConfig.SPEED} " 114 | else: 115 | cmd = f"python3 {toolConfig.SITL_PATH} " \ 116 | f"--out=127.0.0.1:14550 --out=127.0.0.1:14540 -v ArduCopter -w -S {toolConfig.SPEED} " 117 | self._sitl_task = pexpect.spawn(cmd, cwd=toolConfig.ARDUPILOT_LOG_PATH, timeout=30, encoding='utf-8') 118 | 119 | if toolConfig.MODE == 'PX4': 120 | if toolConfig.HOME is None: 121 | pre_argv = f"HEADLESS=1 " \ 122 | f"PX4_HOME_LAT=-35.362758 " \ 123 | f"PX4_HOME_LON=149.165135 " \ 124 | f"PX4_HOME_ALT=583.730592 " \ 125 | f"PX4_SIM_SPEED_FACTOR={toolConfig.SPEED}" 126 | else: 127 | pre_argv = f"HEADLESS=1 " \ 128 | f"PX4_HOME_LAT=40.072842 " \ 129 | f"PX4_HOME_LON=-105.230575 " \ 130 | f"PX4_HOME_ALT=0.000000 " \ 131 | f"PX4_SIM_SPEED_FACTOR={toolConfig.SPEED}" 132 | 133 | if toolConfig.SIM == 'Airsim': 134 | cmd = f'make {pre_argv} px4_sitl none_iris' 135 | if toolConfig.SIM == 'Jmavsim': 136 | cmd = f'make {pre_argv} px4_sitl jmavsim' 137 | 138 | self._sitl_task = pexpect.spawn(cmd, cwd=toolConfig.PX4_RUN_PATH, timeout=30, encoding='utf-8') 139 | logger.info(f"Start {toolConfig.MODE} --> [{toolConfig.SIM}]") 140 | if cmd is None: 141 | raise ValueError('Not support mode or simulator') 142 | 143 | def start_multiple_sitl(self, drone_i=0): 144 | """ 145 | start multiple simulators (not support PX4 now) 146 | :param drone_i: 147 | :return: 148 | """ 149 | if toolConfig.MODE == 'Ardupilot': 150 | if os.path.exists(f"{toolConfig.ARDUPILOT_LOG_PATH}/eeprom.bin"): 151 | os.remove(f"{toolConfig.ARDUPILOT_LOG_PATH}/eeprom.bin") 152 | if os.path.exists(f"{toolConfig.ARDUPILOT_LOG_PATH}/mav.parm"): 153 | os.remove(f"{toolConfig.ARDUPILOT_LOG_PATH}/mav.parm") 154 | 155 | if toolConfig.HOME is not None: 156 | cmd = f"python3 {toolConfig.SITL_PATH} --location={toolConfig.HOME} " \ 157 | f"--out=127.0.0.1:1455{drone_i} --out=127.0.0.1:1454{drone_i} " \ 158 | f"-v ArduCopter -w -S {toolConfig.SPEED} --instance {drone_i}" 159 | else: 160 | cmd = f"python3 {toolConfig.SITL_PATH} " \ 161 | f"--out=127.0.0.1:1455{drone_i} --out=127.0.0.1:1454{drone_i} " \ 162 | f"-v ArduCopter -w -S {toolConfig.SPEED} --instance {drone_i}" 163 | 164 | self._sitl_task = (pexpect.spawn(cmd, cwd=toolConfig.ARDUPILOT_LOG_PATH, timeout=30, encoding='utf-8')) 165 | 166 | if toolConfig.MODE == toolConfig.MODE == 'PX4': 167 | 168 | if os.path.exists(f"{toolConfig.PX4_RUN_PATH}/build/px4_sitl_default/instance_{drone_i}/eeprom/parameters_10016") \ 169 | and toolConfig.MODE == "PX4": 170 | os.remove(f"{toolConfig.PX4_RUN_PATH}/build/px4_sitl_default/instance_{drone_i}/eeprom/parameters_10016") 171 | 172 | if toolConfig.SIM == 'Jmavsim': 173 | cmd = f"{toolConfig.PX4_RUN_PATH}/Tools/sitl_multiple_run_single.sh {drone_i}" 174 | os.environ['PX4_SIM_SPEED_FACTOR'] = f"{toolConfig.SPEED}" 175 | if toolConfig.HOME is None: 176 | os.environ['PX4_HOME_LAT'] = "-35.363261" 177 | os.environ['PX4_HOME_LON'] = "149.165230" 178 | os.environ['PX4_HOME_ALT'] = "583.730592" 179 | else: 180 | os.environ['PX4_HOME_LAT'] = "40.072842" 181 | os.environ['PX4_HOME_LON'] = "-105.230575" 182 | os.environ['PX4_HOME_ALT'] = "0.000000" 183 | 184 | self._sitl_task = pexpect.spawn(cmd, cwd=toolConfig.PX4_RUN_PATH, timeout=30, encoding='utf-8') 185 | 186 | logger.info(f"Start {toolConfig.MODE} --> [{toolConfig.SIM} - {drone_i}]") 187 | 188 | def mav_monitor_init(self, mavlink_class: Type[DroneMavlink] = DroneMavlink, drone_i=0): 189 | """ 190 | initial SITL monitor 191 | :return: 192 | """ 193 | self.mav_monitor = mavlink_class(14540 + int(drone_i), 194 | recv_msg_queue=self.sim_msg_queue, 195 | send_msg_queue=self.mav_msg_queue) 196 | self.mav_monitor.connect() 197 | if toolConfig.MODE == 'Ardupilot': 198 | if self.mav_monitor.ready2fly(): 199 | return True 200 | elif toolConfig.MODE == 'PX4': 201 | while True: 202 | line = self._sitl_task.readline() 203 | if 'notify' in line: 204 | # Disable the fail warning and return 205 | self._sitl_task.send("param set NAV_RCL_ACT 0 \n") 206 | time.sleep(0.1) 207 | self._sitl_task.send("param set NAV_DLL_ACT 0 \n") 208 | time.sleep(0.1) 209 | # Enable detector 210 | self._sitl_task.send("param set CBRK_FLIGHTTERM 0 \n") 211 | return True 212 | 213 | def sim_monitor_init(self, simulator_class): 214 | """ 215 | init airsim monitor 216 | :return: 217 | """ 218 | self.sim_monitor = simulator_class(recv_msg_queue=self.mav_msg_queue, send_msg_queue=self.sim_msg_queue) 219 | time.sleep(3) 220 | 221 | def start_mav_monitor(self): 222 | """ 223 | start monitor 224 | :return: 225 | """ 226 | self.mav_monitor.start() 227 | 228 | def start_sim_monitor(self): 229 | """ 230 | Start Simulator monitor process 231 | :return: 232 | """ 233 | self.sim_monitor.start() 234 | 235 | """ 236 | Mavlink Operation 237 | """ 238 | 239 | def mav_monitor_connect(self): 240 | """ 241 | mavlink connect 242 | :return: 243 | """ 244 | return self.mav_monitor.connect() 245 | 246 | def mav_monitor_set_mission(self, mission_file, random: bool = False): 247 | """ 248 | set mission 249 | :param mission_file: file path 250 | :param random: 251 | :return: 252 | """ 253 | return self.mav_monitor.set_mission(mission_file, random) 254 | 255 | def mav_monitor_set_param(self, params, values): 256 | """ 257 | set drone configuration 258 | :return: 259 | """ 260 | for param, value in zip(params, values): 261 | self.mav_monitor.set_param(param, value) 262 | 263 | def mav_monitor_get_param(self, param): 264 | """ 265 | get drone configuration 266 | :return: 267 | """ 268 | return self.mav_monitor.get_param(param) 269 | 270 | def mav_monitor_start_mission(self): 271 | """ 272 | start mission 273 | :return: 274 | """ 275 | self.mav_monitor.start_mission() 276 | 277 | def stop_sitl(self): 278 | """ 279 | stop the simulator 280 | :return: 281 | """ 282 | self._sitl_task.sendcontrol('c') 283 | while True: 284 | line = self._sitl_task.readline() 285 | if not line: 286 | break 287 | self._sitl_task.close(force=True) 288 | logger.info('Stop SITL task.') 289 | logging.debug('Send mavclosed to Airsim.') 290 | 291 | def stop_sim(self): 292 | self._sim_task.sendcontrol('c') 293 | self._sim_task.close(force=True) 294 | logger.info('Stop Sim task.') 295 | 296 | """ 297 | Other get/set 298 | """ 299 | def get_mav_monitor(self): 300 | return self.mav_monitor 301 | 302 | def sitl_task(self) -> spawn: 303 | return self._sitl_task 304 | 305 | 306 | class GaSimManager(SimManager): 307 | def __init__(self, debug: bool = False): 308 | super(GaSimManager, self).__init__(debug) 309 | 310 | """ 311 | Advanced Function 312 | """ 313 | def mav_monitor_error(self): 314 | """ 315 | monitor error during the flight 316 | :return: 317 | """ 318 | logger.info(f'Start error monitor.') 319 | # Setting 320 | mission_time_out_th = 200 321 | result = 'pass' 322 | # Waypoint 323 | loader = mavwp.MAVWPLoader() 324 | if toolConfig.MODE == "PX4": 325 | loader.load('Cptool/fitCollection_px4.txt') 326 | else: 327 | loader.load('Cptool/fitCollection.txt') 328 | # 329 | lpoint1 = Location(loader.wpoints[0]) 330 | lpoint2 = Location(loader.wpoints[1]) 331 | pre_location = Location(loader.wpoints[0]) 332 | # logger 333 | small_move_num = 0 334 | deviation_num = 0 335 | low_lat_num = 0 336 | # Flag 337 | start_check = False 338 | current_mission = 0 339 | pre_alt = 0 340 | last_time = 0 341 | 342 | start_time = time.time() 343 | while True: 344 | if toolConfig.MODE == "PX4": 345 | # time.sleep(0.1) 346 | self.mav_monitor.gcs_msg_request() 347 | status_message = self.mav_monitor.get_msg(["STATUSTEXT"]) 348 | position_msg = self.mav_monitor.get_msg(["GLOBAL_POSITION_INT", "MISSION_CURRENT"]) 349 | 350 | # System status message 351 | if status_message is not None and status_message.get_type() == "STATUSTEXT": 352 | line = status_message.text 353 | logging.debug(f"Status message: {status_message}") 354 | # print(status_message) 355 | if status_message.severity == 6: 356 | if "Disarming" in line or "landed" in line or "Landing" in line or "Land" in line: 357 | # if successful landed, break the loop and return true 358 | logger.info("Successful break the loop.") 359 | break 360 | if "preflight disarming" in line: 361 | result = 'PreArm Failed' 362 | break 363 | # if "SIM Hit ground" in line: 364 | # result = 'crash' 365 | # break 366 | elif status_message.severity == 2 or status_message.severity == 0: 367 | # Appear error, break loop and return false 368 | if "SIM Hit ground" in line \ 369 | or "Crash" in line \ 370 | or "Failsafe enabled: no global position" in line \ 371 | or "failure detected" in line: 372 | result = 'crash' 373 | break 374 | elif "Potential Thrust Loss" in line: 375 | result = 'Thrust Loss' 376 | break 377 | elif "PreArm" in line: # or "speed has been constrained by max speed" in line: 378 | result = 'PreArm Failed' 379 | break 380 | 381 | if position_msg is not None and position_msg.get_type() == "MISSION_CURRENT": 382 | # print(position_msg) 383 | if int(position_msg.seq) > current_mission and int(position_msg.seq) != 6: 384 | logging.debug(f"Mission change {current_mission} -> {position_msg.seq}") 385 | lpoint1 = Location(loader.wpoints[current_mission]) 386 | lpoint2 = Location(loader.wpoints[position_msg.seq]) 387 | 388 | # Start Check 389 | if int(position_msg.seq) == 2: 390 | start_check = True 391 | if int(position_msg.seq) == 1 and toolConfig.MODE == "PX4": 392 | start_check = True 393 | 394 | current_mission = int(position_msg.seq) 395 | if toolConfig.MODE == "PX4" and int(position_msg.seq) == 5: 396 | start_check = False 397 | elif position_msg is not None and position_msg.get_type() == "GLOBAL_POSITION_INT": 398 | # print(position_msg) 399 | # Check deviation 400 | position_lat = position_msg.lat * 1.0e-7 401 | position_lon = position_msg.lon * 1.0e-7 402 | alt = position_msg.relative_alt / 1000 403 | time_usec = position_msg.time_boot_ms * 1e-6 404 | position = Location(position_lat, position_lon, time_usec) 405 | 406 | # Calculate distance 407 | moving_dis = Location.distance(pre_location, position) 408 | # time 409 | time_step = position.timeS - pre_location.timeS 410 | # altitude change 411 | alt_change = abs(pre_alt - alt) 412 | # Update position 413 | pre_location.x = position_lat 414 | pre_location.y = position_lon 415 | pre_alt = alt 416 | 417 | if start_check: 418 | velocity = moving_dis / time_step 419 | # print(f"Velocity {velocity}.") 420 | # Is small move? velocity smaller than 1 and altitude change smaller than 0.1 421 | if velocity < 1 and alt_change < 0.1: 422 | small_move_num += 1 423 | logger.debug(f"Small moving {small_move_num}, num++, num now - {small_move_num}.") 424 | else: 425 | small_move_num = 0 426 | 427 | # Point2line distance 428 | a = Location.distance(position, lpoint1) 429 | b = Location.distance(position, lpoint2) 430 | c = Location.distance(lpoint1, lpoint2) 431 | 432 | if c != 0: 433 | p = (a + b + c) / 2 434 | deviation_dis = 2 * math.sqrt(p * (p - a) * (p - b) * (p - c) + 0.01) / c 435 | else: 436 | deviation_dis = 0 437 | # Is deviation ? 438 | # logger.debug(f"Point2line distance {deviation_dis}.") 439 | if deviation_dis > 10: 440 | if deviation_dis % 5 == 0: 441 | logger.debug(f"Deviation {round(deviation_dis, 4)}, " 442 | f"num++, num now - {deviation_num}.") 443 | deviation_num += 1 444 | else: 445 | deviation_num = 0 446 | 447 | # # Threshold; deviation judgement 448 | if deviation_num > 15: 449 | result = 'deviation' 450 | break 451 | 452 | # Timeout 453 | if small_move_num > 10: 454 | result = 'timeout' 455 | break 456 | # ============================ # 457 | 458 | # Timeout Check if stack at one point 459 | mid_point_time = time.time() 460 | if (mid_point_time - start_time) > mission_time_out_th: 461 | result = 'timeout' 462 | break 463 | return result 464 | -------------------------------------------------------------------------------- /ModelFit/approximate.py: -------------------------------------------------------------------------------- 1 | # -*- coding:utf-8 -*- 2 | from loguru import logger 3 | import os 4 | import pickle 5 | import time 6 | from abc import abstractmethod 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import pandas as pd 11 | from keras.layers import Dense, Dropout, RepeatVector 12 | from keras.layers import LSTM 13 | from keras.models import Sequential 14 | from numpy.lib.stride_tricks import sliding_window_view 15 | from sklearn.model_selection import train_test_split 16 | from sklearn.preprocessing import MinMaxScaler 17 | from tcn import TCN 18 | from tensorflow.python.keras.models import load_model 19 | from tqdm import tqdm 20 | 21 | from Cptool.config import toolConfig 22 | from Cptool.mavtool import min_max_scaler 23 | 24 | 25 | class Modeling(object): 26 | """Base class for time series modeling and prediction""" 27 | 28 | def __init__(self, debug: bool = False): 29 | """ 30 | Initialize the model 31 | Args: 32 | debug: Enable debug logging if True 33 | """ 34 | self._model: Sequential = None 35 | self.in_out = f"{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}" 36 | 37 | @classmethod 38 | def cs_to_sl(cls, values): 39 | """ 40 | Convert continuous series to supervised learning format 41 | Args: 42 | values: Input time series data 43 | Returns: 44 | Formatted data for supervised learning 45 | """ 46 | values = values.astype('float32') 47 | 48 | # Normalize features if retransformation is enabled 49 | if toolConfig.RETRANS: 50 | trans = cls.load_trans() 51 | values = min_max_scaler(trans, values) 52 | 53 | reframed = cls.series_to_supervised(values, toolConfig.INPUT_LEN, 54 | toolConfig.OUTPUT_LEN, True) 55 | 56 | # Ensure model directory exists 57 | model_dir = f'model/{toolConfig.MODE}/{toolConfig.INPUT_LEN}_{toolConfig.OUTPUT_LEN}' 58 | os.makedirs(model_dir, exist_ok=True) 59 | 60 | return reframed 61 | 62 | @staticmethod 63 | def series_to_supervised(data, n_in=1, n_out=1, dropnan=True): 64 | """ 65 | convert series to supervised learning 66 | :param data: 67 | :param n_in: 68 | :param dropnan: 69 | :return: 70 | """ 71 | n_vars = 1 if type(data) is list else data.shape[1] 72 | df = pd.DataFrame(data) 73 | cols, names = list(), list() 74 | # input sequence (t-n, ... t-1) 75 | for i in range(n_in, 0, -1): 76 | cols.append(df.shift(i)) 77 | names += [('var%d(t-%d)' % (j + 1, i)) for j in range(n_vars)] 78 | # forecast sequence (t, t+1, ... t+n) 79 | for i in range(0, n_out): 80 | cols.append(df.shift(-i)) 81 | if i == 0: 82 | names += [('var%d(t)' % (j + 1)) for j in range(n_vars)] 83 | else: 84 | names += [('var%d(t+%d)' % (j + 1, i)) for j in range(n_vars)] 85 | # put it all together 86 | agg = pd.concat(cols, axis=1) 87 | agg.columns = names 88 | # drop rows with NaN values 89 | if dropnan: 90 | agg.dropna(inplace=True) 91 | return agg 92 | 93 | def _train_valid_split(self, values): 94 | # split into train and test sets 95 | X, Y = self.data_split(values) 96 | train_X, valid_X, train_Y, valid_Y = train_test_split(X, Y, test_size=0.2, random_state=2022) 97 | 98 | logger.info(f"Shape: {train_X.shape}, {train_Y.shape}, {valid_X.shape}, {valid_Y.shape}") 99 | 100 | return train_X, train_Y, valid_X, valid_Y 101 | 102 | def _test_split(self, values): 103 | X, Y = self.data_split(values) 104 | logger.info(f"Shape: {X.shape}, {Y.shape}") 105 | return X, Y 106 | 107 | def read_trans(self): 108 | self.trans = self.load_trans() 109 | 110 | def extract_feature(self, dir): 111 | file_list = [] 112 | for filename in os.listdir(dir): 113 | if filename.endswith(".csv"): 114 | file_list.append(filename) 115 | file_list.sort() 116 | 117 | pd_array = None 118 | for index, filename in enumerate(file_list): 119 | # Read file 120 | data = pd.read_csv(f"{dir}/{filename}") 121 | data = data.drop(["TimeS"], axis=1) 122 | # extract patch 123 | values = data.values 124 | values = self.cs_to_sl(values) 125 | # if first 126 | if index == 0: 127 | pd_array = values 128 | else: 129 | pd_array = pd.concat([pd_array, values]) 130 | return pd_array 131 | 132 | def extract_feature_separately(self, dir): 133 | """ 134 | Extract the feature but not merge. 135 | :param dir: 136 | :return: 137 | """ 138 | file_list = [] 139 | # Create folder 140 | if not os.path.exists(dir + "/single"): 141 | os.makedirs(dir + "/single") 142 | # Read all csv 143 | for filename in os.listdir(dir + "/csv"): 144 | if filename.endswith(".csv"): 145 | file_list.append(filename) 146 | file_list.sort() 147 | 148 | for index, filename in enumerate(file_list): 149 | # Read file 150 | data = pd.read_csv(f"{dir}/{filename}") 151 | data = data.drop(["TimeS"], axis=1) 152 | # extract patch 153 | values = data.values 154 | values = self._cs_to_sl(values) 155 | # if first 156 | values.to_csv(f"{dir}/single/{filename}", index=False) 157 | 158 | @abstractmethod 159 | def data_split(self, value): 160 | pass 161 | 162 | @abstractmethod 163 | def _fit_network(self, train_X, train_Y, valid_X, valid_Y, num=None): 164 | return None 165 | 166 | @abstractmethod 167 | def _build_model(self, train_shape: np.shape): 168 | return None 169 | 170 | @abstractmethod 171 | def read_model(self): 172 | pass 173 | 174 | def set_model(self, path): 175 | local = os.getcwd() 176 | self._model = load_model(f"{local}/{path}") 177 | 178 | def train(self, values, cuda: bool = False): 179 | train_X, train_y, valid_X, valid_y = self._train_valid_split(values) 180 | model = self._fit_network(train_X, train_y, valid_X, valid_y) 181 | self._model = model 182 | 183 | def predict(self, values): 184 | if self._model is None: 185 | logger.warning('Model is not trained!') 186 | raise ValueError('Train or load model at first') 187 | 188 | predict_X = self._model.predict(values) 189 | # data retrans 190 | if toolConfig.RETRANS: 191 | trans = self.load_trans() 192 | # trans 193 | predict_X = trans.inverse_transform(predict_X) 194 | 195 | return predict_X 196 | 197 | def status2feature(self, status_data): 198 | # extract patch 199 | if "TimeS" in status_data.columns: 200 | status_data = status_data.drop(["TimeS"], axis=1) 201 | values = status_data.values 202 | values = self.cs_to_sl(values) 203 | return values 204 | 205 | def predict_feature(self, feature_data): 206 | """ 207 | predict feature which has been pre-processed 208 | :param feature_data: 209 | :return: 210 | """ 211 | if self._model is None: 212 | logger.warning('Model is not trained!') 213 | raise ValueError('Train or load model at first') 214 | # predict each status 215 | predict_feature = self._model.predict(feature_data) 216 | 217 | return predict_feature 218 | 219 | def test_cmp_draw(self, test, cmp_name, num=150, exec='pdf'): 220 | """ 221 | Draw comparison plots between predicted and actual values 222 | Args: 223 | test: Test data 224 | cmp_name: Name for comparison plots 225 | num: Number of samples to plot 226 | exec: Output file format 227 | """ 228 | if self._model is None: 229 | raise ValueError('Train or load model first') 230 | 231 | # Create output directory 232 | fig_dir = f'{os.getcwd()}/fig/{toolConfig.MODE}/{self.in_out}/{cmp_name}' 233 | os.makedirs(fig_dir, exist_ok=True) 234 | 235 | if isinstance(test, pd.DataFrame): 236 | test = test.values 237 | 238 | values = self.cs_to_sl(test) 239 | X, Y = self.data_split(values) 240 | 241 | predict_y = self._model.predict(X) 242 | 243 | trans = self.load_trans() 244 | predict_y = trans.inverse_transform(predict_y) 245 | Y = trans.inverse_transform(Y) 246 | 247 | if X.shape[0] > num: 248 | col = self._systematicSampling(X, num) 249 | # col = np.arange(1, 200) 250 | predict_y = predict_y[col, :] 251 | test = Y[col, :] 252 | else: 253 | test = Y 254 | # 'AccX', 'AccY', 'AccZ', 255 | for name, i in zip(['Roll', 'Pitch', 'Yaw', 'RateRoll', 'RatePitch', 'RateYaw'], range(6)): 256 | x = predict_y[:, i] 257 | y = test[:, i] 258 | 259 | fig = plt.figure(figsize=(8, 4)) 260 | ax1 = plt.subplot() 261 | 262 | ax2 = ax1.twinx() 263 | 264 | 265 | if name in ['AccX', 'AccY', 'AccZ']: 266 | ax1.set_ylabel(f'{name} (m/s/s)', fontsize=18) 267 | if name in ['RateRoll', 'RatePitch', 'RateYaw']: 268 | ax1.set_ylabel(f'{name} (deg/s)', fontsize=18) 269 | if name in ['Roll', 'Pitch', 'Yaw']: 270 | ax1.set_ylabel(f'{name} (deg)', fontsize=18) 271 | 272 | ax2.set_ylim([0, 20 * np.max(np.abs(x - y))]) 273 | ax1.set_ylim([-1.2,1.2]) 274 | if name in ['AccX', 'AccY', 'AccZ']: 275 | ax2.set_ylabel('Error (m/s/s)', fontsize=18) 276 | if name in ['RateRoll', 'RatePitch', 'RateYaw']: 277 | ax2.set_ylabel('Error (deg/s)', fontsize=18) 278 | if name in ['Roll', 'Pitch', 'Yaw']: 279 | ax2.set_ylabel('Error (deg)', fontsize=18) 280 | x -=0.05 281 | 282 | ax1.plot(x, '-', label='Predicted', linewidth=2) 283 | ax1.plot(y, '--', label='Real', linewidth=2) 284 | ax1.set_xlabel("Timestamp", fontsize=18) 285 | ax2.bar(np.arange(len(x)), np.abs(x - y), color='tab:cyan', width=1, label='Bias') 286 | 287 | 288 | fig.legend(loc='upper center', ncol=3, fontsize='18') 289 | plt.setp(ax1.get_xticklabels(), fontsize=18) 290 | plt.setp(ax2.get_yticklabels(), fontsize=18) 291 | plt.setp(ax1.get_yticklabels(), fontsize=18) 292 | 293 | plt.margins(0, 0) 294 | plt.gcf().subplots_adjust(bottom=0.174, left=0.145,top=0.843) 295 | plt.savefig(f'{os.getcwd()}/fig/{toolConfig.MODE}/{self.in_out}/{cmp_name}/{name.lower()}.{exec}') 296 | plt.show() 297 | plt.clf() 298 | 299 | def test_feature_draw(self, X, Y, cmp_name, exec='pdf'): 300 | if self._model is None: 301 | logger.warning('Model is not trained!') 302 | raise ValueError('Train or load model at first') 303 | if not os.path.exists(f'{os.getcwd()}/fig/{toolConfig.MODE}/{self.in_out}/{cmp_name}'): 304 | os.makedirs(f'{os.getcwd()}/fig/{toolConfig.MODE}/{self.in_out}/{cmp_name}') 305 | 306 | for name, i in zip(['AccX', 'AccY', 'AccZ', 'Roll', 'Pitch', 'Yaw', 'RateRoll', 'RatePitch', 'RateYaw', ], 307 | range(9)): 308 | x = X[:, i] 309 | y = Y[:, i] 310 | 311 | fig = plt.figure(figsize=(8, 4.8)) 312 | ax1 = plt.subplot() 313 | 314 | ax2 = ax1.twinx() 315 | 316 | ax1.plot(x, '-', label='Predicted', linewidth=2) 317 | ax1.plot(y, '--', label='Real', linewidth=2) 318 | if name in ['AccX', 'AccY', 'AccZ']: 319 | ax1.set_ylabel(f'{name} (m/s/s)', fontsize=18) 320 | if name in ['RateRoll', 'RatePitch', 'RateYaw']: 321 | ax1.set_ylabel(f'{name} (deg/s)', fontsize=18) 322 | if name in ['Roll', 'Pitch', 'Yaw']: 323 | ax1.set_ylabel(f'{name} (deg)', fontsize=18) 324 | ax2.bar(np.arange(len(x)), np.abs(x - y), label='Error') 325 | ax2.set_ylim([0, 10 * np.max(np.abs(x - y))]) 326 | if name in ['AccX', 'AccY', 'AccZ']: 327 | ax2.set_ylabel('Error (m/s/s)', fontsize=18) 328 | if name in ['RateRoll', 'RatePitch', 'RateYaw']: 329 | ax2.set_ylabel('Error (deg/s)', fontsize=18) 330 | if name in ['Roll', 'Pitch', 'Yaw']: 331 | ax2.set_ylabel('Error (deg)', fontsize=18) 332 | 333 | fig.legend(loc='upper center', ncol=3, fontsize='18') 334 | 335 | plt.margins(0, 0) 336 | plt.gcf().subplots_adjust(bottom=0.12) 337 | plt.savefig( 338 | f'{os.getcwd()}/fig/{toolConfig.MODE}/{toolConfig.INPUT_LEN}/{cmp_name}/{name.lower()}.{exec}', 339 | dpi=300) 340 | # plt.show() 341 | plt.clf() 342 | 343 | def run_5flow_test(self, features, cuda: bool = False): 344 | if not cuda: 345 | os.environ["CUDA_VISIBLE_DEVICES"] = "-1" 346 | 347 | # load dataset 348 | 349 | X, Y = self.data_split(features) 350 | 351 | for i in range(5): 352 | _, X_other, _, y_other = train_test_split(X, Y, test_size=0.2, random_state=5 + i, 353 | shuffle=False, stratify=None) 354 | X_valid, X_test, y_valid, y_test = train_test_split(X_other, y_other, test_size=0.2, random_state=5 + i, 355 | shuffle=True, stratify=None) 356 | 357 | self._fit_network(X_valid, y_valid, X_test, y_test, i) 358 | 359 | def feature_deviation(self, test_data, cuda: bool = False): 360 | """ 361 | Calculate deviation between predicted and actual features 362 | Args: 363 | test_data: Test data to evaluate 364 | cuda: Use GPU if True 365 | Returns: 366 | Array of deviation scores 367 | """ 368 | if not self._model: 369 | raise ValueError('Train or load model first') 370 | 371 | if not cuda: 372 | os.environ["CUDA_VISIBLE_DEVICES"] = "-1" 373 | # load dataset 374 | test_X, test_Y = self.data_split(test_data) 375 | 376 | pred_y = self._model.predict(test_X) 377 | 378 | deviation = np.abs(pred_y - test_Y) 379 | 380 | split_deviation = np.split(deviation, 1000) 381 | 382 | split_loss = [it.sum() for it in split_deviation] 383 | # deviation = return_min_max_scaler_param(deviation) 384 | 385 | return np.array(split_loss) 386 | 387 | def feature_deviation_old(self, test_data, cuda: bool = False): 388 | if self._model is None: 389 | logger.warning('Model is not trained!') 390 | raise ValueError('Train or load model at first') 391 | 392 | if not cuda: 393 | os.environ["CUDA_VISIBLE_DEVICES"] = "-1" 394 | # load dataset 395 | test_X, test_Y = self.data_split(test_data) 396 | 397 | pred_y = self._model.predict(test_X) 398 | 399 | deviation = np.abs(pred_y - test_Y) 400 | 401 | split_loss = deviation.sum(axis=1) 402 | # deviation = return_min_max_scaler_param(deviation) 403 | 404 | return np.array(split_loss) 405 | 406 | def test(self, test_data, cuda: bool = False): 407 | if self._model is None: 408 | logger.warning('Model is not trained!') 409 | raise ValueError('Train or load model at first') 410 | 411 | if not cuda: 412 | os.environ["CUDA_VISIBLE_DEVICES"] = "-1" 413 | # load dataset 414 | test_X, test_Y = self.data_split(test_data) 415 | 416 | start = time.time() 417 | self._model.predict(test_X) 418 | end = time.time() 419 | logger.info("time cost:%.4f s" % (end - start)) 420 | 421 | score = self._model.evaluate(test_X, test_Y, batch_size=256, verbose=1) 422 | logger.info(f"{score[1]}") 423 | return score[1] 424 | 425 | def test_kfold(self, model_path, test_data, k, cuda: bool = False): 426 | scores = [] 427 | for i in range(k): 428 | try: 429 | self.set_model(f"{model_path}/lstm.h5") 430 | except Exception as e: 431 | logger.warning('Model is not trained!') 432 | raise ValueError('Train or load model at first') 433 | 434 | score = self.test(test_data, cuda) 435 | scores.append(score) 436 | logger.info(f"test scores: {scores}") 437 | 438 | @staticmethod 439 | def _systematicSampling(dataMat, number): 440 | length = len(dataMat) 441 | k = length // number 442 | out = range(length) 443 | out_index = out[:length:k] 444 | return out_index 445 | 446 | @staticmethod 447 | def fit_trans(pd_csv): 448 | values = pd_csv.values 449 | 450 | status_value = values[:, :toolConfig.STATUS_LEN] 451 | 452 | # fit 453 | trans = MinMaxScaler(feature_range=(0, 1)) 454 | trans.fit(status_value) 455 | # save 456 | if not os.path.exists(f"model/{toolConfig.MODE}"): 457 | os.makedirs(f"model/{toolConfig.MODE}") 458 | with open(f'model/{toolConfig.MODE}/trans.pkl', 'wb') as f: 459 | pickle.dump(trans, f) 460 | 461 | @staticmethod 462 | def load_trans(): 463 | # load dataset 464 | with open(f'model/{toolConfig.MODE}/trans.pkl', 'rb') as f: 465 | trans = pickle.load(f) 466 | return trans 467 | 468 | @staticmethod 469 | def series2segment_predict(data, has_param=False, dropnan=True): 470 | """ 471 | trans a numpy array data to segments. like (6, 32) to 3 (4,32) e.g., (3, 4, 32) 472 | :param has_param: 473 | :param data: 474 | :param dropnan: 475 | :return: 476 | """ 477 | 478 | # convert series to supervised learning 479 | df = pd.DataFrame(data) 480 | cols, names = list(), list() 481 | # input sequence (t-n, ... t-1) 482 | for i in range(toolConfig.INPUT_LEN - 1, -1, -1): 483 | cols.append(df.shift(i)) 484 | 485 | # put it all together 486 | agg = pd.concat(cols, axis=1) 487 | # drop rows with NaN values 488 | if dropnan: 489 | agg.dropna(inplace=True) 490 | return agg.to_numpy().reshape((-1, toolConfig.INPUT_LEN, toolConfig.DATA_LEN)) 491 | 492 | @classmethod 493 | def cal_deviation_old(cls, predicted_data, status_data): 494 | """ 495 | calculate vector deviation between status_data and predicted data 496 | :param status_data: real flight status data 497 | :param predicted_data: predicted data 498 | :return: status_deviation result which has been normalized 499 | """ 500 | deviation = np.abs(status_data - predicted_data) 501 | if len(predicted_data.shape) == 3: 502 | loss = deviation.sum(axis=tuple(range(1, 3))) 503 | else: 504 | loss = deviation.sum(axis=1).sum(axis=1) 505 | return loss 506 | 507 | @classmethod 508 | def cal_patch_deviation(cls, predicted_data, status_data): 509 | """ 510 | calculate matrix deviation between status_data and predicted data 511 | :param status_data: real flight status data 512 | :param predicted_data: predicted data 513 | :return: status_deviation result which has been normalized 514 | """ 515 | deviation = np.abs(status_data - predicted_data) 516 | if len(predicted_data.shape) == 3: 517 | loss = deviation.sum(axis=tuple(range(1, 3))) 518 | else: 519 | loss = deviation.sum(axis=1).sum(axis=1) 520 | return loss 521 | 522 | # @classmethod 523 | # def loss_discriminate(cls, patch_deviation: np.ndarray, loss_patch_size=5) -> np.ndarray: 524 | # patch_array = sliding_window_view(patch_deviation, loss_patch_size, axis=0) 525 | # patch_array_loss = patch_array.sum(axis=1).sum(axis=1) 526 | # return patch_array_loss 527 | 528 | def cal_average_loss(self, status_data): 529 | # create predicted status of this status patch 530 | predicted_data = self.predict(status_data) 531 | # calculate deviation between real and predicted 532 | patch_deviation = self.cal_patch_deviation(status_data, predicted_data) 533 | 534 | # average 535 | average_loss = patch_deviation.sum() 536 | 537 | 538 | class CyLSTM(Modeling): 539 | def __init__(self, epochs: int, batch_size: int, debug: bool = False): 540 | super(CyLSTM, self).__init__(debug) 541 | 542 | # param 543 | self.epochs = epochs 544 | self.batch_size: int = batch_size 545 | 546 | def data_split(self, values): 547 | if isinstance(values, pd.DataFrame): 548 | values = values.values 549 | 550 | # split into input and outputs 551 | X = values[:, :toolConfig.INPUT_DATA_LEN] 552 | # cut off parameter value in y 553 | y = values[:, toolConfig.INPUT_DATA_LEN:] 554 | # To 3D 555 | y = y.reshape((y.shape[0], toolConfig.OUTPUT_LEN, -1)) 556 | # Reduce parameter length and reshape to 2D 557 | Y = y[:, :, :-toolConfig.PARAM_LEN].reshape((y.shape[0], toolConfig.OUTPUT_DATA_LEN)) 558 | 559 | # reshape input to be 3D [samples, timesteps, features] 560 | X = X.reshape((X.shape[0], toolConfig.INPUT_LEN, toolConfig.DATA_LEN)) 561 | Y = Y.reshape((Y.shape[0], toolConfig.OUTPUT_DATA_LEN)) 562 | 563 | return X, Y 564 | 565 | def data_split_3d(self, values): 566 | values = values.values if type(values) is pd.DataFrame else values 567 | 568 | # split into input and outputs 569 | X = values[:, :, :toolConfig.INPUT_DATA_LEN] 570 | # cut off parameter value in y 571 | y = values[:, :, toolConfig.INPUT_DATA_LEN:-toolConfig.PARAM_LEN] 572 | # # To 3D 573 | # y = y.reshape((y.shape[0], toolConfig.OUTPUT_LEN, -1)) 574 | # # Reduce parameter length and reshape to 2D 575 | # Y = y[:, :, :-toolConfig.PARAM_LEN].reshape((y.shape[0], toolConfig.OUTPUT_DATA_LEN)) 576 | 577 | # reshape input to be 3D [samples, timesteps, features] 578 | X = X.reshape((-1, toolConfig.INPUT_LEN, toolConfig.DATA_LEN)) 579 | Y = y.reshape((-1, toolConfig.OUTPUT_DATA_LEN)) 580 | 581 | return X, Y 582 | 583 | def _fit_network(self, train_X, train_Y, valid_X, valid_Y, num=None): 584 | if not self.epochs: 585 | raise ValueError('set LSTM param at first!') 586 | 587 | model = self._build_model(train_X.shape) 588 | # fit network 589 | history = model.fit(train_X, train_Y, 590 | epochs=self.epochs, batch_size=self.batch_size, 591 | validation_data=(valid_X, valid_Y), 592 | verbose=2, 593 | shuffle=True) 594 | if num is not None: 595 | model.save(f'model/{toolConfig.MODE}/{self.in_out}/lstm{num}.h5') 596 | plt.plot(history.history['loss'], label=f'train-{num}') 597 | plt.plot(history.history['val_loss'], label=f'validation-{num}') 598 | else: 599 | model.save(f'model/{toolConfig.MODE}/{self.in_out}/lstm.h5') 600 | plt.plot(history.history['loss'], label='train') 601 | plt.plot(history.history['val_loss'], label='validation') 602 | # plot history 603 | axis_font = {'size': '18'} 604 | plt.ylabel('Loss', fontsize=18) 605 | plt.xlabel('Epochs Time', fontsize=18) 606 | plt.legend(prop=axis_font) 607 | # plt.show() 608 | plt.savefig(f'model/{toolConfig.MODE}/{self.in_out}/loss.pdf') 609 | return model 610 | 611 | def _build_model(self, train_shape: np.shape): 612 | model = Sequential() 613 | model.add(LSTM(128, input_shape=(train_shape[1], train_shape[2]))) 614 | model.add(Dropout(0.1)) 615 | model.add(Dense(64, activation='relu')) 616 | model.add(Dropout(0.1)) 617 | model.add(Dense(64, activation='relu')) 618 | model.add(Dense(toolConfig.OUTPUT_DATA_LEN)) 619 | model.compile(loss='mean_squared_error', optimizer='adam', metrics=['accuracy', 'mse']) 620 | model.summary() 621 | 622 | return model 623 | 624 | def read_model(self): 625 | self._model = load_model(f'model/{toolConfig.MODE}/{self.in_out}/lstm.h5') 626 | 627 | @classmethod 628 | def merge_file_data(cls, dir): 629 | file_list = [] 630 | for filename in os.listdir(dir): 631 | if filename.endswith(".csv"): 632 | file_list.append(filename) 633 | file_list.sort() 634 | 635 | col_name = pd.read_csv(f"{dir}/{file_list[0]}").columns 636 | pd_csv = pd.DataFrame(columns=col_name) 637 | 638 | for filename in tqdm(file_list): 639 | data = pd.read_csv(f"{dir}/{filename}") 640 | pd_csv = pd.concat([pd_csv, data]) 641 | # remove Times 642 | return pd_csv.drop(["TimeS"], axis=1) 643 | 644 | 645 | class CyTCN(Modeling): 646 | def __init__(self, epochs: int, batch_size: int, debug: bool = False): 647 | super(CyTCN, self).__init__(debug) 648 | 649 | # param 650 | self.epochs = epochs 651 | self.batch_size: int = batch_size 652 | 653 | def data_split(self, value): 654 | values = value.values 655 | 656 | # split into input and outputs 657 | X = values[:, :toolConfig.INPUT_DATA_LEN] 658 | # cut off parameter value in y 659 | y = values[:, toolConfig.INPUT_DATA_LEN:] 660 | # To 3D 661 | y = y.reshape((y.shape[0], toolConfig.OUTPUT_LEN, -1)) 662 | # Reduce parameter length and reshape to 2D 663 | Y = y[:, :, :-toolConfig.PARAM_LEN].reshape((y.shape[0], toolConfig.OUTPUT_DATA_LEN)) 664 | 665 | # reshape input to be 3D [samples, timesteps, features] 666 | X = X.reshape((X.shape[0], toolConfig.INPUT_LEN, toolConfig.DATA_LEN)) 667 | Y = Y.reshape((Y.shape[0], 1, toolConfig.OUTPUT_DATA_LEN)) 668 | 669 | return X, Y 670 | 671 | def _fit_network(self, train_X, train_Y, valid_X, valid_Y, num=None): 672 | if not self.epochs: 673 | raise ValueError('set TCN param at first!') 674 | 675 | model = self._build_model(train_X.shape) 676 | # fit network 677 | history = model.fit(train_X, train_Y, 678 | epochs=self.epochs, batch_size=self.batch_size, 679 | validation_data=(valid_X, valid_Y), 680 | verbose=2, 681 | shuffle=True) 682 | 683 | if num is not None: 684 | model.save(f'model/{toolConfig.MODE}/{self.in_out}/tcn{num}.h5') 685 | plt.plot(history.history['loss'], label=f'train-{num}') 686 | plt.plot(history.history['val_loss'], label=f'validation-{num}') 687 | else: 688 | model.save(f'model/{toolConfig.MODE}/{self.in_out}/tcn.h5') 689 | plt.plot(history.history['loss'], label='train') 690 | plt.plot(history.history['val_loss'], label='validation') 691 | # plot history 692 | axis_font = {'size': '18'} 693 | plt.ylabel('Loss', fontsize=18) 694 | plt.xlabel('Epochs Time', fontsize=18) 695 | plt.legend(prop=axis_font) 696 | # plt.show() 697 | plt.savefig(f'model/{toolConfig.MODE}/{self.in_out}/tcn_loss.pdf') 698 | return model 699 | 700 | def _build_model(self, train_shape: np.shape): 701 | # create model 702 | model = Sequential( 703 | layers=[ 704 | TCN(input_shape=(train_shape[1], train_shape[2])), # output.shape = (batch, 64) 705 | RepeatVector(1), # output.shape = (batch, output_timesteps, 64) 706 | Dense(toolConfig.OUTPUT_DATA_LEN) # output.shape = (batch, output_timesteps, output_dim) 707 | ] 708 | ) 709 | model.compile(loss="mse", 710 | optimizer="Adam", metrics=["accuracy"]) # 配置 711 | model.summary() 712 | 713 | return model 714 | 715 | def read_model(self): 716 | self._model = load_model(f'model/{toolConfig.MODE}/{self.in_out}/tcn.h5', 717 | custom_objects={"TCN": TCN}) 718 | -------------------------------------------------------------------------------- /Cptool/gaMavlink.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import json 3 | import logging 4 | import multiprocessing 5 | import os 6 | import random 7 | import shutil 8 | import time 9 | 10 | import numpy as np 11 | import pandas as pd 12 | import ray 13 | from pymavlink import mavutil, mavwp 14 | from pymavlink.DFReader import DFMessage 15 | from pymavlink.mavutil import mavserial 16 | from pyulog import ULog 17 | from tqdm import tqdm 18 | 19 | from Cptool.config import toolConfig 20 | from Cptool.mavtool import load_param, read_path_specified_file, select_sub_dict 21 | 22 | 23 | class DroneMavlink: 24 | def __init__(self, port, recv_msg_queue=None, send_msg_queue=None): 25 | super(DroneMavlink, self).__init__() 26 | self.recv_msg_queue = recv_msg_queue 27 | self.send_msg_queue = send_msg_queue 28 | self._master: mavserial = None 29 | self._port = port 30 | self.takeoff = False 31 | 32 | # Mavlink common operation 33 | 34 | def connect(self): 35 | """ 36 | Connect drone 37 | :return: 38 | """ 39 | self._master = mavutil.mavlink_connection('udp:0.0.0.0:{}'.format(self._port)) 40 | try: 41 | self._master.wait_heartbeat(timeout=30) 42 | except TimeoutError: 43 | return False 44 | logging.info("Heartbeat from system (system %u component %u) from %u" % ( 45 | self._master.target_system, self._master.target_component, self._port)) 46 | return True 47 | 48 | def ready2fly(self) -> bool: 49 | """ 50 | wait for IMU can work 51 | :return: 52 | """ 53 | while True: 54 | message = self._master.recv_match(type=['STATUSTEXT'], blocking=True, timeout=30) 55 | # message = self._master.recv_match(blocking=True, timeout=30) 56 | message = message.to_dict()["text"] 57 | # print(message) 58 | if toolConfig.MODE == "Ardupilot" and "IMU0 is using GPS" in message: 59 | logging.debug("Ready to fly.") 60 | return True 61 | # print(message) 62 | if toolConfig.MODE == "PX4" and "home set" in message: 63 | logging.debug("Ready to fly.") 64 | return True 65 | 66 | def set_mission(self, mission_file, israndom: bool = False, timeout=30) -> bool: 67 | """ 68 | Set mission 69 | :param israndom: random mission order 70 | :param mission_file: mission file 71 | :param timeout: 72 | :return: success 73 | """ 74 | if not self._master: 75 | logging.warning('Mavlink handler is not connect!') 76 | raise ValueError('Connect at first!') 77 | 78 | loader = mavwp.MAVWPLoader() 79 | loader.target_system = self._master.target_system 80 | loader.target_component = self._master.target_component 81 | loader.load(mission_file) 82 | logging.debug(f"Load mission file {mission_file}") 83 | 84 | # if px4, set home at first 85 | if toolConfig.MODE == "PX4": 86 | self.px4_set_home() 87 | 88 | if israndom: 89 | loader = self.random_mission(loader) 90 | # clear the waypoint 91 | self._master.waypoint_clear_all_send() 92 | # send the waypoint count 93 | self._master.waypoint_count_send(loader.count()) 94 | seq_list = [True] * loader.count() 95 | try: 96 | # looping to send each waypoint information 97 | # Ardupilot method 98 | while True in seq_list: 99 | msg = self._master.recv_match(type=['MISSION_REQUEST'], blocking=True) 100 | if msg is not None and seq_list[msg.seq] is True: 101 | self._master.mav.send(loader.wp(msg.seq)) 102 | seq_list[msg.seq] = False 103 | logging.debug(f'Sending waypoint {msg.seq}') 104 | mission_ack_msg = self._master.recv_match(type=['MISSION_ACK'], blocking=True, timeout=timeout) 105 | logging.info(f'Upload mission finish.') 106 | except TimeoutError: 107 | logging.warning('Upload mission timeout!') 108 | return False 109 | return True 110 | 111 | def start_mission(self): 112 | """ 113 | Arm and start the flight 114 | :return: 115 | """ 116 | if not self._master: 117 | logging.warning('Mavlink handler is not connect!') 118 | raise ValueError('Connect at first!') 119 | # self._master.set_mode_loiter() 120 | if toolConfig.MODE == "PX4": 121 | self._master.set_mode_auto() 122 | self._master.arducopter_arm() 123 | self._master.set_mode_auto() 124 | else: 125 | self._master.arducopter_arm() 126 | self._master.set_mode_auto() 127 | 128 | logging.info('Try to arm and start.') 129 | 130 | def set_param(self, param: str, value: float) -> None: 131 | """ 132 | set a value of specific parameter 133 | :param param: name of the parameter 134 | :param value: float value want to set 135 | """ 136 | if not self._master: 137 | raise ValueError('Connect at first!') 138 | 139 | self._master.param_set_send(param, value) 140 | self.get_param(param) 141 | 142 | def set_params(self, params_dict: dict) -> None: 143 | """ 144 | set multiple parameter 145 | :param params_dict: a dict consist of {parameter:values}... 146 | """ 147 | for param, value in params_dict.items(): 148 | self.set_param(param, value) 149 | 150 | def reset_params(self): 151 | self.set_param("FORMAT_VERSION", 0) 152 | 153 | def get_param(self, param: str) -> float: 154 | """ 155 | get current value of a parameter. 156 | :param param: name 157 | :return: value of parameter 158 | """ 159 | self._master.param_fetch_one(param) 160 | while True: 161 | message = self._master.recv_match(type=['PARAM_VALUE', 'PARM'], blocking=True).to_dict() 162 | if message['param_id'] == param: 163 | logging.debug('name: %s\t value: %f' % (message['param_id'], message['param_value'])) 164 | break 165 | return message['param_value'] 166 | 167 | def get_params(self, params: list) -> dict: 168 | """ 169 | get current value of a parameters. 170 | :param params: 171 | :return: value of parameter 172 | """ 173 | out_dict = {} 174 | for param in params: 175 | out_dict[param] = self.get_param(param) 176 | return out_dict 177 | 178 | def get_msg(self, msg_type, block=False): 179 | """ 180 | receive the mavlink message 181 | :param msg_type: 182 | :param block: 183 | :return: 184 | """ 185 | msg = self._master.recv_match(type=msg_type, blocking=block) 186 | return msg 187 | 188 | def set_mode(self, mode: str): 189 | """ 190 | Set flight mode 191 | :param mode: string type of a mode, it will be convert to an int values. 192 | :return: 193 | """ 194 | if not self._master: 195 | logging.warning('Mavlink handler is not connect!') 196 | raise ValueError('Connect at first!') 197 | mode_id = self._master.mode_mapping()[mode] 198 | 199 | self._master.mav.set_mode_send(self._master.target_system, 200 | mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 201 | mode_id) 202 | while True: 203 | message = self._master.recv_match(type='COMMAND_ACK', blocking=True).to_dict() 204 | if message['command'] == mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE: 205 | logging.debug(f'Mode: {mode} Set successful') 206 | break 207 | 208 | # Special operation 209 | def set_random_param_and_start(self): 210 | param_configuration = self.create_random_params(toolConfig.PARAM) 211 | self.set_params(param_configuration) 212 | # Unlock the uav 213 | self.start_mission() 214 | 215 | def px4_set_home(self): 216 | if toolConfig.HOME is None: 217 | self._master.mav.command_long_send(self._master.target_system, self._master.target_component, 218 | mavutil.mavlink.MAV_CMD_DO_SET_HOME, 219 | 1, 220 | 0, 221 | 0, 222 | 0, 223 | 0, 224 | -35.362758, 225 | 149.165135, 226 | 583.730592) 227 | else: 228 | self._master.mav.command_long_send(self._master.target_system, self._master.target_component, 229 | mavutil.mavlink.MAV_CMD_DO_SET_HOME, 230 | 1, 231 | 0, 232 | 0, 233 | 0, 234 | 0, 235 | 40.072842, 236 | -105.230575, 237 | 0.000000) 238 | msg = self._master.recv_match(type=['COMMAND_ACK'], blocking=True, timeout=30) 239 | logging.debug(f"Home set callback: {msg.command}") 240 | 241 | def gcs_msg_request(self): 242 | self._master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, 243 | mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) 244 | # self._master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER, 245 | # mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) 246 | 247 | def wait_complete(self): 248 | pass 249 | 250 | # Static method 251 | @staticmethod 252 | def create_random_params(param_choice): 253 | """ 254 | create a configuration with random values 255 | :param param_choice: parameters you want to choose 256 | :return: 257 | """ 258 | para_dict = load_param() 259 | 260 | param_choice_dict = select_sub_dict(para_dict, param_choice) 261 | 262 | out_dict = {} 263 | for key, param_range in param_choice_dict.items(): 264 | value = round(random.uniform(param_range['range'][0], param_range['range'][1]) / param_range['step']) * \ 265 | param_range['step'] 266 | out_dict[key] = value 267 | return out_dict 268 | 269 | @staticmethod 270 | def random_mission(loader): 271 | """ 272 | create random order of a mission 273 | :param loader: waypoint loader 274 | :return: 275 | """ 276 | index = random.sample(loader.wpoints[2:loader.count() - 1], loader.count() - 3) 277 | index = loader.wpoints[0:2] + index 278 | index.append(loader.wpoints[-1]) 279 | for i, points in enumerate(index): 280 | points.seq = i 281 | loader.wpoints = index 282 | return loader 283 | 284 | @staticmethod 285 | def extract_log_path(log_path, skip=True, threat=None): 286 | """ 287 | extract and convert bin file to csv 288 | :param skip: 289 | :param log_path: 290 | :param threat: multiple threat 291 | :return: 292 | """ 293 | 294 | # If px4, the log is ulg, if ardupilot the log is bin 295 | if toolConfig.MODE == "PX4": 296 | file_list = read_path_specified_file(log_path, 'ulg') 297 | else: 298 | file_list = read_path_specified_file(log_path, 'BIN') 299 | if not os.path.exists(f"{log_path}/csv"): 300 | os.makedirs(f"{log_path}/csv") 301 | 302 | # multiple 303 | if threat is not None: 304 | arrays = np.array_split(file_list, threat) 305 | threat_manage = [] 306 | ray.init(include_dashboard=True, dashboard_host="127.0.0.1", dashboard_port=8088) 307 | 308 | for array in arrays: 309 | if toolConfig.MODE == "PX4": 310 | threat_manage.append(GaMavlinkPX4.extract_log_path_threat.remote(log_path, array, skip)) 311 | else: 312 | threat_manage.append(GaMavlinkAPM.extract_log_path_threat.remote(log_path, array, skip)) 313 | ray.get(threat_manage) 314 | ray.shutdown() 315 | else: 316 | # 列出文件夹内所有.BIN结尾的文件并排序 317 | for file in tqdm(file_list): 318 | name, _ = file.split('.') 319 | if skip and os.path.exists(f'{log_path}/csv/{name}.csv'): 320 | continue 321 | # extract 322 | try: 323 | if toolConfig.MODE == "PX4": 324 | csv_data = GaMavlinkPX4.extract_log_file(log_path + f'/{file}') 325 | else: 326 | csv_data = GaMavlinkAPM.extract_log_file(log_path + f'/{file}') 327 | csv_data.to_csv(f'{log_path}/csv/{name}.csv', index=False) 328 | except Exception as e: 329 | logging.warning(f"Error processing {file} : {e}") 330 | continue 331 | 332 | 333 | class GaMavlinkAPM(DroneMavlink, multiprocessing.Process): 334 | """ 335 | Mainly responsible for initiating the communication link to interact with UAV 336 | """ 337 | 338 | def __init__(self, port, recv_msg_queue=None, send_msg_queue=None): 339 | super(GaMavlinkAPM, self).__init__(port, recv_msg_queue, send_msg_queue) 340 | 341 | @staticmethod 342 | def log_extract_apm(msg: DFMessage): 343 | """ 344 | parse the msg of mavlink 345 | :param msg: 346 | :return: 347 | """ 348 | out = None 349 | if msg.get_type() == 'ATT': 350 | if len(toolConfig.LOG_MAP): 351 | out = { 352 | 'TimeS': msg.TimeUS / 1000000, 353 | 'Roll': msg.Roll, 354 | 'Pitch': msg.Pitch, 355 | 'Yaw': msg.Yaw, 356 | } 357 | elif msg.get_type() == 'RATE': 358 | out = { 359 | 'TimeS': msg.TimeUS / 1000000, 360 | # deg to rad 361 | 'RateRoll': msg.R, 362 | 'RatePitch': msg.P, 363 | 'RateYaw': msg.Y, 364 | } 365 | # elif msg.get_type() == 'POS': 366 | # out = { 367 | # 'TimeS': msg.TimeUS / 1000000, 368 | # # deglongtitude 369 | # 'Lat': msg.Lat, 370 | # 'Lng': msg.Lng, 371 | # 'Alt': msg.Alt, 372 | # } 373 | elif msg.get_type() == 'IMU': 374 | out = { 375 | 'TimeS': msg.TimeUS / 1000000, 376 | 'AccX': msg.AccX, 377 | 'AccY': msg.AccY, 378 | 'AccZ': msg.AccZ, 379 | 'GyrX': msg.GyrX, 380 | 'GyrY': msg.GyrY, 381 | 'GyrZ': msg.GyrZ, 382 | } 383 | elif msg.get_type() == 'VIBE': 384 | out = { 385 | 'TimeS': msg.TimeUS / 1000000, 386 | # m/s^2 387 | 'VibeX': msg.VibeX, 388 | 'VibeY': msg.VibeY, 389 | 'VibeZ': msg.VibeZ, 390 | } 391 | elif msg.get_type() == 'MAG': 392 | out = { 393 | 'TimeS': msg.TimeUS / 1000000, 394 | 'MagX': msg.MagX, 395 | 'MagY': msg.MagY, 396 | 'MagZ': msg.MagZ, 397 | } 398 | elif msg.get_type() == 'PARM': 399 | out = { 400 | 'TimeS': msg.TimeUS / 1000000, 401 | msg.Name: msg.Value 402 | } 403 | return out 404 | 405 | @classmethod 406 | def fill_and_process_pd_log(cls, pd_array: pd.DataFrame): 407 | # Remain timestamp .1 and drop duplicate 408 | pd_array['TimeS'] = pd_array['TimeS'].round(1) 409 | pd_array = pd_array.drop_duplicates(keep='first') 410 | 411 | # merge data in same TimeS 412 | df_array = pd.DataFrame(columns=pd_array.columns) 413 | for group, group_item in pd_array.groupby('TimeS'): 414 | # fillna 415 | group_item = group_item.fillna(method='ffill') 416 | group_item = group_item.fillna(method='bfill') 417 | df_array.loc[len(df_array.index)] = group_item.mean() 418 | # Drop nan 419 | df_array = df_array.fillna(method='ffill') 420 | df_array = df_array.dropna() 421 | 422 | # Sort 423 | order_name = toolConfig.STATUS_ORDER.copy() 424 | param_seq = load_param().columns.to_list() 425 | param_name = df_array.keys().difference(order_name).to_list() 426 | param_name.sort(key=lambda item: param_seq.index(item)) 427 | # Status value + Parameter name 428 | order_name.extend(param_name) 429 | df_array = df_array[order_name] 430 | return df_array 431 | 432 | @staticmethod 433 | def extract_log_file(log_file): 434 | """ 435 | extract log message form a bin file. 436 | :param log_file: 437 | :return: 438 | """ 439 | accept_item = toolConfig.LOG_MAP 440 | 441 | logs = mavutil.mavlink_connection(log_file) 442 | # init 443 | out_data = [] 444 | accpet_param = load_param().columns.to_list() 445 | 446 | while True: 447 | msg = logs.recv_match(type=accept_item) 448 | if msg is None: 449 | break 450 | if msg.get_type() in ['ATT', 'RATE']: 451 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 452 | elif msg.get_type() in ['IMU', 'MAG']: 453 | if hasattr(msg, "I") and msg.I == 0: 454 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 455 | else: 456 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 457 | elif msg.get_type() == 'VIBE': 458 | if hasattr(msg, "IMU") and msg.IMU == 0: 459 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 460 | else: 461 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 462 | elif msg.get_type() == 'PARM' and msg.Name in accpet_param: 463 | out_data.append(GaMavlinkAPM.log_extract_apm(msg)) 464 | pd_array = pd.DataFrame(out_data) 465 | # Switch sequence, fill, and return 466 | pd_array = GaMavlinkAPM.fill_and_process_pd_log(pd_array) 467 | return pd_array 468 | 469 | @staticmethod 470 | @ray.remote 471 | def extract_log_path_threat(log_path, file_list, skip): 472 | for file in tqdm(file_list): 473 | name, _ = file.split('.') 474 | if skip and os.path.exists(f'{log_path}/csv/{name}.csv'): 475 | continue 476 | try: 477 | csv_data = GaMavlinkAPM.extract_log_file(log_path + f'/{file}') 478 | csv_data.to_csv(f'{log_path}/csv/{name}.csv', index=False) 479 | except Exception as e: 480 | logging.warning(f"Error processing {file} : {e}") 481 | continue 482 | return True 483 | 484 | @staticmethod 485 | def extract_log_file_des_and_ach(log_file): 486 | """ 487 | extract log message form a bin file with att desired and achieved 488 | :param log_file: 489 | :return: 490 | """ 491 | 492 | logs = mavutil.mavlink_connection(log_file) 493 | # init 494 | out_data = [] 495 | 496 | while True: 497 | msg = logs.recv_match(type=["ATT"]) 498 | if msg is None: 499 | break 500 | out = { 501 | 'TimeS': msg.TimeUS / 1000000, 502 | 'Roll': msg.Roll, 503 | 'DesRoll': msg.DesRoll, 504 | 'Pitch': msg.Pitch, 505 | 'DesPitch': msg.DesPitch, 506 | 'Yaw': msg.Yaw, 507 | 'DesYaw': msg.DesYaw 508 | } 509 | out_data.append(out) 510 | 511 | pd_array = pd.DataFrame(out_data) 512 | # Switch sequence, fill, and return 513 | pd_array['TimeS'] = pd_array['TimeS'].round(1) 514 | pd_array = pd_array.drop_duplicates(keep='first') 515 | return pd_array 516 | 517 | # Special function 518 | @classmethod 519 | def random_param_value(cls, param_json: dict): 520 | """ 521 | random create the value 522 | :param param_json: 523 | :return: 524 | """ 525 | out = {} 526 | for name, item in param_json.items(): 527 | range = item['range'] 528 | step = item['step'] 529 | random_sample = random.randrange(range[0], range[1], step) 530 | out[name] = random_sample 531 | return out 532 | 533 | @staticmethod 534 | def delete_current_log(): 535 | log_index = f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/LASTLOG.TXT" 536 | 537 | # Read last index 538 | with open(log_index, 'r') as f: 539 | num = int(f.readline()) 540 | # To string 541 | num = f'{num}' 542 | log_file = f"{toolConfig.ARDUPILOT_LOG_PATH}/logs/{num.rjust(8, '0')}.BIN" 543 | # Remove file 544 | if os.path.exists(log_file): 545 | os.remove(log_file) 546 | # Fix last index number 547 | last_num = f"{int(num) - 1}" 548 | with open(log_index, 'w') as f: 549 | f.write(last_num) 550 | 551 | def wait_complete(self, remain_fail=False, timeout=60 * 5): 552 | if not self._master: 553 | raise ValueError('Connect at first!') 554 | try: 555 | timeout_start = time.time() 556 | while time.time() < timeout_start + timeout: 557 | message = self._master.recv_match(type=['STATUSTEXT'], blocking=True, timeout=30) 558 | if message is None: 559 | continue 560 | # print(message) 561 | message = message.to_dict() 562 | out_msg = "None" 563 | line = message['text'] 564 | if message["severity"] == 6: 565 | if "Land" in line: 566 | # if successful landed, break the loop and return true 567 | logging.info(f"Successful break the loop.") 568 | return True 569 | elif message["severity"] == 2 or message["severity"] == 0: 570 | # Appear error, break loop and return false 571 | if "SIM Hit ground at" in line: 572 | pass 573 | elif "Potential Thrust Loss" in line: 574 | pass 575 | elif "Crash" in line: 576 | pass 577 | elif "PreArm" in line: 578 | pass 579 | # will not generate log file 580 | logging.info(f"Get error with {message['text']}") 581 | return True 582 | logging.info(f"Get error with {message['text']}") 583 | if remain_fail: 584 | # Keep problem log 585 | return True 586 | else: 587 | return False 588 | except TimeoutError: 589 | # Mission point time out, change other params 590 | logging.warning('Wp timeout!') 591 | return False 592 | except KeyboardInterrupt: 593 | logging.info('Key bordInterrupt! exit') 594 | return False 595 | return False 596 | 597 | # Ardupilot 598 | 599 | def run(self): 600 | """ 601 | loop check 602 | :return: 603 | """ 604 | 605 | while True: 606 | msg = self._master.recv_match(type=['STATUSTEXT'], blocking=False) 607 | if msg is not None: 608 | msg = msg.to_dict() 609 | # print(msg2) 610 | if msg['severity'] in [0, 2]: 611 | # self.send_msg_queue.put('crash') 612 | logging.info('ArduCopter detect Crash.') 613 | self.send_msg_queue.put('error') 614 | break 615 | 616 | 617 | class GaMavlinkPX4(DroneMavlink, multiprocessing.Process): 618 | """ 619 | Mainly responsible for initiating the communication link to interact with UAV 620 | """ 621 | 622 | def __init__(self, port, recv_msg_queue=None, send_msg_queue=None): 623 | super(GaMavlinkPX4, self).__init__(port, recv_msg_queue, send_msg_queue) 624 | self.log_file = None 625 | 626 | def wait_complete(self, remain_fail=False, timeout=60 * 5): 627 | if not self._master: 628 | raise ValueError('Connect at first!') 629 | try: 630 | timeout_start = time.time() 631 | while time.time() < timeout_start + timeout: 632 | # PX4 needs manual send the heartbeat for GCS 633 | self.gcs_msg_request() 634 | message = self._master.recv_match(type=['STATUSTEXT'], blocking=False, timeout=30) 635 | if message is None: 636 | continue 637 | message = message.to_dict() 638 | out_msg = "None" 639 | line = message['text'] 640 | if message["severity"] == 6: 641 | if "landed" in line: 642 | # if successful landed, break the loop and return true 643 | logging.info(f"Successful break the loop.") 644 | return True 645 | elif message["severity"] == 2 or message["severity"] == 0: 646 | # Appear error, break loop and return false 647 | if "SIM Hit ground at" in line: 648 | pass 649 | elif "Potential Thrust Loss" in line: 650 | pass 651 | elif "Crash" in line: 652 | pass 653 | elif "PreArm" in line: 654 | pass 655 | # will not generate log file 656 | logging.info(f"Get error with {message['text']}") 657 | return True 658 | logging.info(f"Get error with {message['text']}") 659 | if remain_fail: 660 | # Keep problem log 661 | return True 662 | else: 663 | return False 664 | return False 665 | except TimeoutError: 666 | # Mission point time out, change other params 667 | logging.warning('Wp timeout!') 668 | return False 669 | except KeyboardInterrupt: 670 | logging.info('Key bordInterrupt! exit') 671 | return False 672 | 673 | def init_ulg_log_file(self, device_i=None): 674 | if device_i is None: 675 | log_path = f"{toolConfig.PX4_LOG_PATH}/*.ulg" 676 | 677 | list_of_files = glob.glob(log_path) # * means all if need specific format then *.csv 678 | latest_file = max(list_of_files, key=os.path.getctime) 679 | self.log_file = latest_file 680 | logging.info(f"Current log file: {latest_file}") 681 | else: 682 | now = time.localtime() 683 | now_time = time.strftime("%Y-%m-%d", now) 684 | log_path = f"{toolConfig.PX4_RUN_PATH}/build/px4_sitl_default/instance_{device_i}/log/{now_time}/*.ulg" 685 | 686 | list_of_files = glob.glob(log_path) # * means all if need specific format then *.csv 687 | latest_file = max(list_of_files, key=os.path.getctime) 688 | self.log_file = latest_file 689 | logging.info(f"Current log file: {latest_file}") 690 | 691 | @staticmethod 692 | def fill_and_process_pd_log(pd_array: pd.DataFrame): 693 | # Round TimesS 694 | pd_array["TimeS"] = pd_array["TimeS"] / 1000000 695 | pd_array['TimeS'] = pd_array['TimeS'].round(1) 696 | 697 | pd_array = pd_array.drop_duplicates(keep='first') 698 | 699 | # merge data in same TimeS 700 | df_array = pd.DataFrame(columns=pd_array.columns) 701 | 702 | for group, group_item in pd_array.groupby('TimeS'): 703 | # fillna 704 | group_item = group_item.fillna(method='ffill') 705 | group_item = group_item.fillna(method='bfill') 706 | df_array.loc[len(df_array.index)] = group_item.mean() 707 | # Drop nan 708 | df_array = df_array.fillna(method='ffill') 709 | df_array = df_array.dropna() 710 | 711 | return df_array 712 | 713 | @staticmethod 714 | def extract_log_file(log_file): 715 | """ 716 | extract log message form a bin file. 717 | :param log_file: 718 | :return: 719 | """ 720 | 721 | ulog = ULog(log_file) 722 | 723 | att = pd.DataFrame(ulog.get_dataset('vehicle_attitude_setpoint').data)[["timestamp", 724 | "roll_body", "pitch_body", "yaw_body"]] 725 | rate = pd.DataFrame(ulog.get_dataset('vehicle_rates_setpoint').data)[["timestamp", 726 | "roll", "pitch", "yaw"]] 727 | acc_gyr = pd.DataFrame(ulog.get_dataset('sensor_combined').data)[["timestamp", 728 | "gyro_rad[0]", "gyro_rad[1]", "gyro_rad[2]", 729 | "accelerometer_m_s2[0]", 730 | "accelerometer_m_s2[1]", 731 | "accelerometer_m_s2[2]"]] 732 | mag = pd.DataFrame(ulog.get_dataset('sensor_mag').data)[["timestamp", "x", "y", "z"]] 733 | vibe = pd.DataFrame(ulog.get_dataset('sensor_accel').data)[["timestamp", "x", "y", "z"]] 734 | # Param 735 | param = pd.Series(ulog.initial_parameters) 736 | param = param[toolConfig.PARAM] 737 | # select parameters 738 | for t, name, value in ulog.changed_parameters: 739 | if name in toolConfig.PARAM: 740 | param[name] = round(value, 5) 741 | 742 | att.columns = ["TimeS", "Roll", "Pitch", "Yaw"] 743 | rate.columns = ["TimeS", "RateRoll", "RatePitch", "RateYaw"] 744 | acc_gyr.columns = ["TimeS", "GyrX", "GyrY", "GyrZ", "AccX", "AccY", "AccZ"] 745 | mag.columns = ["TimeS", "MagX", "MagY", "MagZ"] 746 | vibe.columns = ["TimeS", "VibeX", "VibeY", "VibeZ"] 747 | # Merge values 748 | pd_array = pd.concat([att, rate, acc_gyr, mag, vibe]).sort_values(by='TimeS') 749 | 750 | # Process 751 | df_array = GaMavlinkPX4.fill_and_process_pd_log(pd_array) 752 | # Add parameters 753 | param_values = np.tile(param.values, df_array.shape[0]).reshape(df_array.shape[0], -1) 754 | df_array[toolConfig.PARAM] = param_values 755 | 756 | # Sort 757 | order_name = toolConfig.STATUS_ORDER.copy() 758 | param_seq = load_param().columns.to_list() 759 | param_name = df_array.keys().difference(order_name).to_list() 760 | param_name.sort(key=lambda item: param_seq.index(item)) 761 | 762 | return df_array 763 | 764 | @staticmethod 765 | @ray.remote 766 | def extract_log_path_threat(log_path, file_list, skip): 767 | for file in tqdm(file_list): 768 | name, _ = file.split('.') 769 | if skip and os.path.exists(f'{log_path}/csv/{name}.csv'): 770 | continue 771 | try: 772 | csv_data = GaMavlinkPX4.extract_log_file(log_path + f'/{file}') 773 | csv_data.to_csv(f'{log_path}/csv/{name}.csv', index=False) 774 | except Exception as e: 775 | logging.warning(f"Error processing {file} : {e}") 776 | continue 777 | return True 778 | 779 | @classmethod 780 | def delete_current_log(cls): 781 | log_path = f"{toolConfig.PX4_LOG_PATH}/*.ulg" 782 | 783 | list_of_files = glob.glob(log_path) # * means all if need specific format then *.csv 784 | latest_file = max(list_of_files, key=os.path.getctime) 785 | # Remove file 786 | if os.path.exists(latest_file): 787 | os.remove(latest_file) 788 | 789 | 790 | class LogHandler: 791 | """Centralized log handling""" 792 | 793 | def __init__(self, log_dir): 794 | self.log_dir = log_dir 795 | 796 | def extract_log(self, log_file, log_type): 797 | """Extract log data based on type""" 798 | if log_type == 'px4': 799 | return self._extract_px4_log(log_file) 800 | elif log_type == 'ardupilot': 801 | return self._extract_apm_log(log_file) 802 | 803 | def process_logs(self, files, processors=4): 804 | """Process multiple log files in parallel""" 805 | from multiprocessing import Pool 806 | with Pool(processors) as p: 807 | return p.map(self.extract_log, files) --------------------------------------------------------------------------------