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..ed89d23734e2
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.hitl_testing
@@ -0,0 +1,22 @@
+#!/bin/sh
+#
+# HITL testing parameters.
+#
+
+
+param set SYS_HITL 1
+param set UAVCAN_ENABLE 0
+param set-default SYS_FAILURE_EN 1
+
+param set MAV_HITL_SHOME 0
+
+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..65d587d893f4 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..447bc3588d12
--- /dev/null
+++ b/test/mavsdk_tests/mavsdk_preparing.cpp
@@ -0,0 +1,169 @@
+
+#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{};
+ mavsdk::Mavsdk mavsdk{};
+
+ 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 << "Connection URL not provided" << std::endl;
+ return 1;
+ }
+
+ 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;
+
+
+ auto res_polling = poll_condition_with_timeout([&]() { return mavsdk.systems().size() > 0;}, std::chrono::seconds(25));
+
+ if (not res_polling) {
+ std::cerr << "Polling failed" << std::endl;
+ return 1;
+ }
+
+ auto system = mavsdk.systems().at(0);
+
+ if (!system) {
+ std::cerr << "The system could not 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 (attempt = 0; attempt <= max_attemp; attempt++) {
+ 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 parameter: " << param_name << ", value: "
+ << param_value << std::endl;
+ return 1;
+ }
+
+ auto res = param.get_param_int(param_name);
+
+ if (res.first == Param::Result::Success && res.second == param_value) {
+ break;
+ }
+
+ std::this_thread::sleep_for(1s);
+ }
+
+ if (attempt <= max_attemp) {
+ std::cout << "Parameter " << param_name << " was successly set after " << attempt
+ << " attempt. Tne new value " << param_value << std::endl;
+
+ } else {
+ std::cerr << "Fail. Setting parameter: " << param_name << " wasn't set to value: "
+ << param_value << "after " << max_attemp << " attempts. " << std::endl;
+ return 1;
+ }
+ }
+
+ if (command_reboot == command) {
+ auto action = Action{system};
+
+ if (Action::Result::Success != action.reboot()) {
+ std::cerr << "Reboot doesn't work" << std::endl;
+ return 1;
+ }
+
+ std::cout << "Rebooting...\n";
+ }
+
+ return 0;
+}
diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py
index 47ccd62d58cb..2ba89046db99 100755
--- a/test/mavsdk_tests/mavsdk_test_runner.py
+++ b/test/mavsdk_tests/mavsdk_test_runner.py
@@ -7,6 +7,7 @@
import math
import os
import psutil # type: ignore
+import re
import signal
import subprocess
import sys
@@ -55,16 +56,13 @@ def main() -> 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("Wrong connection selected")
+
+ if (res):
+ print("The device has successfully rebooted")
+ time.sleep(15)
+ else:
+ print("Reboot 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 have ",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,