From 9fd733ad8e5cc72d385498058ec56604461014c9 Mon Sep 17 00:00:00 2001 From: Ilia-Loginov Date: Wed, 14 Aug 2024 12:26:38 +0400 Subject: [PATCH] Added supporting hitl testing for the mavsdk_test_runner On branch feature_hitl_testing_support_via_mavsdk_test_runner Changes to be committed: modified: ROMFS/px4fmu_common/init.d/CMakeLists.txt new file: ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil modified: ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt new file: ROMFS/px4fmu_common/init.d/rc.hitl_testing new file: Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf modified: src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake modified: test/mavsdk_tests/CMakeLists.txt new file: test/mavsdk_tests/configs/hitl.json new file: test/mavsdk_tests/configs/hitl_gz_harm.json new file: test/mavsdk_tests/configs/sitl_gz_harm.json new file: test/mavsdk_tests/mavsdk_preparing.cpp modified: test/mavsdk_tests/mavsdk_test_runner.py modified: test/mavsdk_tests/process_helper.py --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 1 + .../airframes/1401_ssrc_holybro_x500.hil | 41 +++ .../init.d/airframes/CMakeLists.txt | 1 + ROMFS/px4fmu_common/init.d/rc.hitl_testing | 20 ++ .../models/ssrc_standard_vtol/model_hitl.sdf | 18 ++ .../sitl_targets_gz-sim.cmake | 12 + test/mavsdk_tests/CMakeLists.txt | 9 + test/mavsdk_tests/configs/hitl.json | 14 + test/mavsdk_tests/configs/hitl_gz_harm.json | 24 ++ test/mavsdk_tests/configs/sitl_gz_harm.json | 20 ++ test/mavsdk_tests/mavsdk_preparing.cpp | 164 +++++++++++ test/mavsdk_tests/mavsdk_test_runner.py | 277 +++++++++++++++--- test/mavsdk_tests/process_helper.py | 105 ++++++- 13 files changed, 665 insertions(+), 41 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil create mode 100644 ROMFS/px4fmu_common/init.d/rc.hitl_testing create mode 100644 Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf create mode 100644 test/mavsdk_tests/configs/hitl.json create mode 100644 test/mavsdk_tests/configs/hitl_gz_harm.json create mode 100644 test/mavsdk_tests/configs/sitl_gz_harm.json create mode 100644 test/mavsdk_tests/mavsdk_preparing.cpp diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 3f19c22f01d6..28c3ca5467d1 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -56,4 +56,5 @@ px4_add_romfs_files( rc.vehicle_setup rc.vtol_apps rc.vtol_defaults + rc.hitl_testing ) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil b/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil new file mode 100644 index 000000000000..7daed3eff97c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil @@ -0,0 +1,41 @@ +#!/bin/sh +# +# @name Gazebo x500 for hitl testing +# +# @type Quadrotor x +# @class Copter + +. ${R}etc/init.d/rc.mc_defaults +. ${R}etc/init.d/rc.hitl_testing + +# Default rates +param set-default IMU_GYRO_CUTOFF 30 +param set-default IMU_GYRO_NF0_FRQ 75 +param set-default IMU_DGYRO_CUTOFF 30 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.3 +param set-default MC_PITCHRATE_I 0.3 +param set-default MC_ROLLRATE_D 0.004 +param set-default MC_PITCHRATE_D 0.004 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +# Set takeoff ramp to disabled for a more decisive takeoff action +param set-default MPC_TKO_RAMP_T 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index c93aef7ed84d..de0bdef6f954 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_romfs_files( 1100_rc_quad_x_sih.hil 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil + 1401_ssrc_holybro_x500.hil # [2000, 2999] Standard planes" 2100_standard_plane diff --git a/ROMFS/px4fmu_common/init.d/rc.hitl_testing b/ROMFS/px4fmu_common/init.d/rc.hitl_testing new file mode 100644 index 000000000000..b0bc6b0dc653 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.hitl_testing @@ -0,0 +1,20 @@ +#!/bin/sh +# +# HITL testing parameters. +# + + +param set SYS_HITL 1 +param set UAVCAN_ENABLE 0 +param set-default SYS_FAILURE_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +# disable some checks to allow to fly +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default CBRK_IO_SAFETY 22027 diff --git a/Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf b/Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf new file mode 100644 index 000000000000..119635258a79 --- /dev/null +++ b/Tools/simulation/gz/models/ssrc_standard_vtol/model_hitl.sdf @@ -0,0 +1,18 @@ + + + + + model://ssrc_standard_vtol + + + + /link/base_link/sensor/imu_sensor/imu + /pose/info + 192.168.200.101 + 14542 + 14543 + 4560 + 0 + + + diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake index 680ca2715456..f434ba340c40 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake @@ -48,5 +48,17 @@ if(gz-sim8_FOUND) BUILD_ALWAYS 1 BUILD_COMMAND ${CMAKE_COMMAND} --build -- -j ${parallel_jobs} ) + + ExternalProject_Add(mavsdk_tests + SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests + DEPENDS mavlink_c_generate + INSTALL_COMMAND "" + USES_TERMINAL_CONFIGURE true + USES_TERMINAL_BUILD true + EXCLUDE_FROM_ALL true + BUILD_ALWAYS 1 + ) endif() diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 90b27f943aac..1677b9682b20 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -42,6 +42,15 @@ if(MAVSDK_FOUND) -Werror -Wno-error=deprecated-declarations ) + + add_executable(mavsdk_preparing + mavsdk_preparing.cpp + ) + + target_link_libraries(mavsdk_preparing + MAVSDK::mavsdk + ${CMAKE_THREAD_LIBS_INIT} + ) else() message("MAVSDK C++ not found, skipping mavsdk_tests build..") endif() diff --git a/test/mavsdk_tests/configs/hitl.json b/test/mavsdk_tests/configs/hitl.json new file mode 100644 index 000000000000..ab687f514d53 --- /dev/null +++ b/test/mavsdk_tests/configs/hitl.json @@ -0,0 +1,14 @@ +{ + "mode": "hitl", + "simulator": "gazebo", + "mavlink_connection": "udp://:14540", + "tests": + [ + { + "model": "iris_hitl_set_your_model", + "vehicle": "iris_hitl_set_your_model", + "test_filter": "[multicopter],[offboard][offboard_attitude]", + "timeout_min": 10 + } + ] + } diff --git a/test/mavsdk_tests/configs/hitl_gz_harm.json b/test/mavsdk_tests/configs/hitl_gz_harm.json new file mode 100644 index 000000000000..d3591bdeb647 --- /dev/null +++ b/test/mavsdk_tests/configs/hitl_gz_harm.json @@ -0,0 +1,24 @@ +{ + "mode": "hitl", + "simulator": "gz_sim", + "mavlink_connection": "udp://:14540", + "tests": + [ + { + "model": "ssrc_holybro_x500", + "vehicle": "ssrc_holybro_x500", + "model_file": "model_hitl", + "sys_autostart": 1401, + "test_filter": "[multicopter],[offboard][offboard_attitude]", + "timeout_min": 10 + }, + { + "model": "ssrc_standard_vtol", + "vehicle": "ssrc_standard_vtol", + "model_file": "model_hitl", + "sys_autostart": 1404, + "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]", + "timeout_min": 10 + } + ] +} diff --git a/test/mavsdk_tests/configs/sitl_gz_harm.json b/test/mavsdk_tests/configs/sitl_gz_harm.json new file mode 100644 index 000000000000..b53a9c16a042 --- /dev/null +++ b/test/mavsdk_tests/configs/sitl_gz_harm.json @@ -0,0 +1,20 @@ +{ + "mode": "sitl", + "simulator": "gz_sim", + "mavlink_connection": "udp://:14540", + "tests": + [ + { + "model": "x500", + "vehicle": "x500", + "test_filter": "[multicopter],[offboard][offboard_attitude]", + "timeout_min": 10 + }, + { + "model": "ssrc_standard_vtol", + "vehicle": "ssrc_standard_vtol", + "test_filter": "[vtol], [vtol_wind], [vtol_airspeed_fail]", + "timeout_min": 10 + } + ] +} diff --git a/test/mavsdk_tests/mavsdk_preparing.cpp b/test/mavsdk_tests/mavsdk_preparing.cpp new file mode 100644 index 000000000000..5b9ba2935869 --- /dev/null +++ b/test/mavsdk_tests/mavsdk_preparing.cpp @@ -0,0 +1,164 @@ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mavsdk; +using std::chrono::seconds; +using std::this_thread::sleep_for; + +template +bool poll_condition_with_timeout_( + std::function fun, std::chrono::duration duration) +{ + static constexpr unsigned check_resolution = 100; + + const std::chrono::microseconds duration_us(duration); + const auto start_time = std::chrono::steady_clock::now(); + + while (!fun()) { + std::this_thread::sleep_for(duration_us / check_resolution); + const auto elapsed_time_us = std::chrono::duration_cast(std::chrono::steady_clock::now() - + start_time); + + if (elapsed_time_us > duration_us) { + std::cout << "Timeout, waiting for the vehicle for " + << elapsed_time_us.count() * std::chrono::steady_clock::period::num + / static_cast(std::chrono::steady_clock::period::den) + << " seconds\n"; + return false; + } + } + return true; +} + +int main(int argc, char **argv) +{ + const std::string arg_url {"--url"}; + const std::string arg_command {"--command"}; + const std::string command_set_param {"set_sys_autostart"}; + const std::string command_check {"check"}; + const std::string command_reboot {"reboot"}; + + std::string connection_url {}; + std::string command {}; + int param_value {}; + for (int i = 0; i < argc; ++i) { + const std::string argv_string(argv[i]); + if (argv_string == arg_url && (argc > (i + 1))) { + connection_url = argv[i + 1]; + i++; + } + else if (argv_string == arg_command && (argc > (i + 1))) { + command = argv[i + 1]; + i++; + + if (command == command_set_param) { + if ((argc > (i + 1))){ + param_value = std::atoi(argv[i + 1]); + i++; + } else { + command.erase(); + } + } + } + + } + + if (connection_url.empty()) { + std::cerr << "No connection URL was supplied" << std::endl; + return 1; + } + + Mavsdk mavsdk{}; + std::cout << "connection url " << connection_url << std::endl; + ConnectionResult connection_result = mavsdk.add_any_connection(connection_url); + if (connection_result != ConnectionResult::Success) + { + std::cerr << "Connect was failed" << std::endl; + return 1; + } + std::cout << "Waiting for system connect" << std::endl; + + if(!poll_condition_with_timeout_( + [&]() { return mavsdk.systems().size() > 0; }, std::chrono::seconds(25))) + { + std::cerr << "Polling was failed" << std::endl; + return 1; + } + + auto system = mavsdk.systems().at(0); + + if(!system) + { + std::cerr << "The system wasn't be found" << std::endl; + return 1; + } + + if (command_check == command) + { + std::cout << "Success! The connection was checked." << std::endl; + return 0; + } + + if (command_set_param == command) + { + //TODO: For some reason when we set the new value it sometimes takes several attempts to change a parameter. + int attempt = 1; + const int max_attemp = 5; + const auto param_name = "SYS_AUTOSTART"; + for(auto i = 0; i <= max_attemp; i++) + { + using namespace std::chrono_literals; + auto param = Param(system); + + + if (param.set_param_int(param_name, param_value) != Param::Result::Success) + { + std::cerr << "Fail setting param: " << param_name << ", value: " + << param_value < NoReturn: parser.add_argument("--build-dir", type=str, default='build/px4_sitl_default/', help="relative path where the built files are stored") + parser.add_argument("--connection", type=str, default="ethernet", + help="the type of connection: serial or ethernet. Using only for --hitl") args = parser.parse_args() with open(args.config_file) as json_file: config = json.load(json_file) - if config["mode"] != "sitl" and args.gui: - print("--gui is not compatible with the mode '{}'" - .format(config["mode"])) - sys.exit(1) - if not is_everything_ready(config, args.build_dir): sys.exit(1) @@ -84,7 +82,8 @@ def main() -> NoReturn: args.gui, args.verbose, args.upload, - args.build_dir + args.build_dir, + args.connection ) signal.signal(signal.SIGINT, tester.sigint_handler) @@ -146,7 +145,8 @@ def __init__(self, gui: bool, verbose: bool, upload: bool, - build_dir: str): + build_dir: str, + connection: str): self.config = config self.build_dir = build_dir self.active_runners: List[ph.Runner] @@ -161,6 +161,7 @@ def __init__(self, self.upload = upload self.start_time = datetime.datetime.now() self.log_fd: Any[TextIO] = None + self.connection = connection @staticmethod def wildcard_match(pattern: str, potential_match: str) -> bool: @@ -209,6 +210,150 @@ def run(self) -> bool: self.show_overall_result() return self.was_overall_pass() + def reboot_using_serial(self): + if (not self.check_dev()): + return False + + if (self.send_command_to_px4("reboot")): + return True + + return False + + def reboot_using_ethernen(self): + return self.send_command_to_px4("./build/px4_sitl_default/mavsdk_tests/mavsdk_preparing", + ["--url", self.config['mavlink_connection'], "--command", "reboot"], False) + + def check_connection_px4_ethernet(self): + return self.send_command_to_px4("./build/px4_sitl_default/mavsdk_tests/mavsdk_preparing", + ["--url", self.config['mavlink_connection'], "--command", "check"], False) + + def check_connection_px4_serial(self): + if (not self.check_dev()): + return False + + return self.send_command_to_px4("ver mcu") + + def set_sys_autostart(self, value): + time.sleep(5) + res = False + if (self.connection == "serial"): + res = self.set_sys_autostart_px4_serial(value) + elif (self.connection == "ethernet"): + res = self.set_sys_autostart_px4_ethernet(value) + else: + print("Chose incorrect connection") + + if (res): + print("The device has successfully changed a parameter") + else: + print("Changing a parameter failed") + + return res + + + def set_sys_autostart_px4_ethernet(self, value): + return self.send_command_to_px4("./build/px4_sitl_default/mavsdk_tests/mavsdk_preparing", + ["--url", self.config['mavlink_connection'], "--command", "set_sys_autostart", str(value)], False) + + def set_sys_autostart_px4_serial(self, value): + if (not self.check_dev()): + return False + + if (not self.send_command_to_px4("param set SYS_AUTOSTART " + str(value))): + return False + + if (self.send_command_to_px4("reboot")): + return True + + return False + + def send_command_to_px4(self, command, args=None, use_shell=True): + + if use_shell: + shell = "./Tools/mavlink_shell.py /dev/ttyACM0" + prosces = subprocess.Popen(shell, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + prosces.stdin.write(command + '\n') + else: + prosces = subprocess.Popen([command] + args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True) + + prosces.stdin.flush() + output, errors = prosces.communicate() + + prosces.stdin.close() + prosces.stdout.close() + prosces.stderr.close() + + if (len(errors)> 0): + print("Error run command ", command, " with args ", args) + if(self.verbose): + print("=========================") + print("errors:", errors) + print("=========================") + return False + + if (use_shell): + if ( len(output) < len("Connecting to MAVLINK...")): + print("Device doesn't answer") + return False + + if (self.verbose): + print("=========================") + print("output:", output) + print("=========================") + + + print("Command ", command, " was run with args ", args) + return True + + def reboot_px4(self): + time.sleep(5) + res = False + if (self.connection == "serial"): + res = self.reboot_using_serial() + elif (self.connection == "ethernet"): + res = self.reboot_using_ethernen() + else: + print("Chose incorrect connection") + + if (res): + print("The device has successfully rebooted") + time.sleep(15) + else: + print("Rebooting failed") + + + + def check_dev(self): + device_path = "/dev/ttyACM0" + if os.path.exists(device_path): + return True + + return False + + def check_connection_px4(self): + if self.connection == "serial": + res= self.check_connection_px4_serial() + + elif self.connection == "ethernet": + res = self.check_connection_px4_ethernet() + + if res: + print("Device has ", self.connection, " connection") + else: + print("Device doesn't has ",self.connection, " connection") + + return res + + def check_connection_px4_times(self, num_cases = 3): + while(num_cases > 0 and (not self.check_connection_px4())): + num_cases -= 1 + time.sleep(0.5) + + if (num_cases > 0): + return True + + return False + def show_plans(self) -> None: print() print(colorize( @@ -286,6 +431,11 @@ def run_iteration(self, iteration: int) -> bool: "==> Running tests for {}".format(test['model']), color.BOLD)) + if self.config['mode'] == 'hitl': + if 'sys_autostart' in test: + if not self.set_sys_autostart(test['sys_autostart']): + return False + test_i = 0 for key, case_value in test['cases'].items(): if not case_value['selected']: @@ -400,51 +550,91 @@ def start_runners(self, case: str) -> None: self.active_runners = [] - if self.config['mode'] == 'sitl': - if self.config['simulator'] == 'gazebo': - gzserver_runner = ph.GzserverRunner( + if self.config['simulator'] == 'gazebo': + gzserver_runner = ph.GzserverRunner( + os.getcwd(), + log_dir, + test['vehicle'], + case, + self.get_max_speed_factor(test), + self.verbose, + self.build_dir) + self.active_runners.append(gzserver_runner) + + gzmodelspawn_runner = ph.GzmodelspawnRunner( + os.getcwd(), + log_dir, + test['vehicle'], + case, + self.verbose, + self.build_dir) + self.active_runners.append(gzmodelspawn_runner) + + if self.gui: + gzclient_runner = ph.GzclientRunner( os.getcwd(), log_dir, - test['vehicle'], + test['model'], case, - self.get_max_speed_factor(test), - self.verbose, - self.build_dir) - self.active_runners.append(gzserver_runner) + self.verbose) + self.active_runners.append(gzclient_runner) + + elif self.config['simulator'] == 'gz_sim': + gzserver_runner = ph.GzHarmonicServer( + os.getcwd(), + log_dir, + test['vehicle'], + case, + self.get_max_speed_factor(test), + self.verbose, + self.build_dir) + self.active_runners.append(gzserver_runner) + + if self.config['mode'] == 'hitl': + if "model_file" in test: + model_file = test['model_file'] + else: + model_file = 'model' - gzmodelspawn_runner = ph.GzmodelspawnRunner( + gzmodelspawn_runner = ph.GzHarmonicModelSpawnRunner( os.getcwd(), log_dir, test['vehicle'], case, self.verbose, - self.build_dir) + self.build_dir, + model_file) self.active_runners.append(gzmodelspawn_runner) - if self.gui: - gzclient_runner = ph.GzclientRunner( - os.getcwd(), - log_dir, - test['model'], - case, - self.verbose) - self.active_runners.append(gzclient_runner) - - # We must start the PX4 instance at the end, as starting - # it in the beginning, then connecting Gazebo server freaks - # out the PX4 (it needs to have data coming in when started), - # and can lead to EKF to freak out, or the instance itself - # to die unexpectedly. - px4_runner = ph.Px4Runner( + if self.gui: + gzclient_runner = ph.GzHarmonicClientRunner( os.getcwd(), log_dir, test['model'], case, - self.get_max_speed_factor(test), - self.debugger, - self.verbose, - self.build_dir) - self.active_runners.append(px4_runner) + self.verbose) + self.active_runners.append(gzclient_runner) + + + # We must start the PX4 instance at the end, as starting + # it in the beginning, then connecting Gazebo server freaks + # out the PX4 (it needs to have data coming in when started), + # and can lead to EKF to freak out, or the instance itself + # to die unexpectedly. + if self.config['mode'] == 'sitl': + px4_runner = ph.Px4Runner( + os.getcwd(), + log_dir, + test['model'], + case, + self.get_max_speed_factor(test), + self.debugger, + self.verbose, + self.build_dir, + self.config['simulator']) + #for env_key in test.get('env', []): + # px4_runner.env[env_key] = str(test['env'][env_key]) + self.active_runners.append(px4_runner) mavsdk_tests_runner = ph.TestRunner( os.getcwd(), @@ -454,9 +644,20 @@ def start_runners(self, self.config['mavlink_connection'], self.speed_factor, self.verbose, - self.build_dir) + self.build_dir, + self.config['mode'] == 'hitl') self.active_runners.append(mavsdk_tests_runner) + if self.config['mode'] == 'hitl': + self.reboot_px4() + print("Reboot was finished") + if (not self.check_connection_px4_times(3)): + print("Could not start runners. Lost connection") + self.collect_runner_output() + self.stop_combined_log() + self.stop_runners() + sys.exit(1) + abort = False for runner in self.active_runners: runner.set_log_filename( diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index fa6bc95b4e32..22ae29fb5b86 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -10,11 +10,15 @@ import errno from typing import Any, Dict, List, TextIO, Optional +PX4_SITL_GZ_SIM_PATH = "Tools/simulation/gz" PX4_SITL_GAZEBO_PATH = "Tools/simulation/gazebo-classic/sitl_gazebo-classic" PX4_GAZEBO_MODELS = PX4_SITL_GAZEBO_PATH + "/models" PX4_GAZEBO_WORLDS = PX4_SITL_GAZEBO_PATH + "/worlds" +PX4_GZ_SIM_MODELS = PX4_SITL_GZ_SIM_PATH + "/models" +PX4_GZ_SIM_WORLDS = PX4_SITL_GZ_SIM_PATH + "/worlds" +PX4_GZ_SIM_PLUGIN = "build_gz-sim_plugins" class Runner: def __init__(self, @@ -48,6 +52,12 @@ def start(self) -> None: if self.verbose: print("Running: {}".format(" ".join([self.cmd] + self.args))) + if self.name == "mavsdk_tests": + if self.env["HIL_MODE"] == "hitl": + delay = 10 + print("Waiting ", delay, " seconds for connection to be established... ") + time.sleep(10) + print("Running test...") atexit.register(self.stop) if self.verbose: @@ -148,11 +158,22 @@ def stop(self) -> int: def time_elapsed_s(self) -> float: return time.time() - self.start_time + def update_gz_sim_enviement(self, workspace_dir, build_dir): + self.env["PX4_GZ_MODELS"] = \ + os.path.join(workspace_dir, PX4_GZ_SIM_MODELS) + self.env["PX4_GZ_WORLDS"] = \ + os.path.join(workspace_dir, PX4_GZ_SIM_WORLDS) + self.env["GZ_SIM_RESOURCE_PATH"] = \ + self.env["PX4_GZ_WORLDS"] + ":" + self.env["PX4_GZ_MODELS"] + self.env["GZ_SIM_SYSTEM_PLUGIN_PATH"] = \ + os.path.join(workspace_dir, build_dir, PX4_GZ_SIM_PLUGIN) + class Px4Runner(Runner): def __init__(self, workspace_dir: str, log_dir: str, model: str, case: str, speed_factor: float, - debugger: str, verbose: bool, build_dir: str): + debugger: str, verbose: bool, build_dir: str, + gazebo_type: str = "gazebo"): super().__init__(log_dir, model, case, verbose) self.name = "px4" self.cmd = os.path.join(workspace_dir, build_dir, "bin/px4") @@ -166,7 +187,13 @@ def __init__(self, workspace_dir: str, log_dir: str, os.path.join(workspace_dir, "test_data"), "-d" ] - self.env["PX4_SIM_MODEL"] = "gazebo-classic_" + self.model + if gazebo_type == "gz_sim": + gz_model = "gz_" + self.model + self.env["PX4_GZ_STANDALONE"] = "1" + else: + gz_model = "gazebo-classic_" + self.model + + self.env["PX4_SIM_MODEL"] = gz_model self.env["PX4_SIM_SPEED_FACTOR"] = str(speed_factor) self.debugger = debugger self.clear_rootfs() @@ -331,6 +358,72 @@ def __init__(self, self.cmd = "gzclient" self.args = ["--verbose"] +class GzHarmonicServer(Runner): + def __init__(self, + workspace_dir: str, + log_dir: str, + model: str, + case: str, + speed_factor: float, + verbose: bool, + build_dir: str): + super().__init__(log_dir, model, case, verbose) + self.name = "gz-sim server" + self.cwd = workspace_dir + self. update_gz_sim_enviement(workspace_dir, build_dir) + + word_path = os.path.join(workspace_dir, + PX4_GZ_SIM_WORLDS, 'default.sdf') + + if not os.path.isfile(word_path): + raise Exception("Word was not found: ", word_path) + + self.cmd = "gz" + self.args = ["sim", "--verbose=1", "-r", "-s", word_path] + +class GzHarmonicModelSpawnRunner(Runner): + def __init__(self, + workspace_dir: str, + log_dir: str, + model: str, + case: str, + verbose: bool, + build_dir: str, + model_file: str): + super().__init__(log_dir, model, case, verbose) + self.name = "gzmodelspawn" + self.cwd = workspace_dir + self.update_gz_sim_enviement(workspace_dir, build_dir) + + model_path = os.path.join(workspace_dir, + PX4_GZ_SIM_MODELS, + self.model, model_file + '.sdf') + + if not os.path.isfile(model_path): + raise Exception("Model not found:", model_path) + + self.cmd = "gz" + self.args = ["service", + "-s", '/world/default/create', + '--reqtype', 'gz.msgs.EntityFactory', + '--reptype', 'gz.msgs.Boolean', + '--timeout', '1000', + '--req', 'sdf_filename: "{}", name: "{}" pose: {{position: {{x: 1.01, y: 0.98, z: 0.83}}}}'.format( + model_path, f'{self.model}')] + + +class GzHarmonicClientRunner(Runner): + def __init__(self, + workspace_dir: str, + log_dir: str, + model: str, + case: str, + verbose: bool): + super().__init__(log_dir, model, case, verbose) + self.name = "gz-sim client" + self.cwd = workspace_dir + self.cmd = "gz" + self.args = ["sim", "-g", "--verbose"] class TestRunner(Runner): def __init__(self, @@ -341,11 +434,17 @@ def __init__(self, mavlink_connection: str, speed_factor: float, verbose: bool, - build_dir: str): + build_dir: str, + hitl_mode: bool = False): super().__init__(log_dir, model, case, verbose) self.name = "mavsdk_tests" self.cwd = workspace_dir self.cmd = "nice" + if (hitl_mode): + self.env["HIL_MODE"] = "hitl" + else: + self.env["HIL_MODE"] = "sitl" + self.args = ["-5", os.path.join( workspace_dir,