From f2a573ee3e30ca2de0be1dbad358d8aa00eeac6c Mon Sep 17 00:00:00 2001 From: frederik Date: Wed, 6 Dec 2023 16:07:46 +0100 Subject: [PATCH] adds simulation script to run simulation Signed-off-by: frederik --- .github/workflows/model-download.yml | 32 +++++ models/OakD-Lite/model.sdf | 2 +- simulation-gazebo | 113 +++++++++++++++ tests/test_model.py | 202 +++++++++++++++++++++++++++ 4 files changed, 348 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/model-download.yml create mode 100644 simulation-gazebo create mode 100644 tests/test_model.py diff --git a/.github/workflows/model-download.yml b/.github/workflows/model-download.yml new file mode 100644 index 0000000..aa34803 --- /dev/null +++ b/.github/workflows/model-download.yml @@ -0,0 +1,32 @@ +name: Model Download + +on: + workflow_dispatch: + push: + branches: + - '*' + +jobs: + test: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: 3.9 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pytest + + - name: Run pytest + run: | + pytest -v -s tests/test_model.py + + + diff --git a/models/OakD-Lite/model.sdf b/models/OakD-Lite/model.sdf index b0fad4d..81f5927 100644 --- a/models/OakD-Lite/model.sdf +++ b/models/OakD-Lite/model.sdf @@ -78,4 +78,4 @@ ogre2 --> - \ No newline at end of file + diff --git a/simulation-gazebo b/simulation-gazebo new file mode 100644 index 0000000..c3ab1f1 --- /dev/null +++ b/simulation-gazebo @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 + +import platform +import argparse +import subprocess +import os +import shutil + +DEFAULT_DOWNLOAD_DIR = "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip" + + +def run(cmd): + subprocess.run(['bash', '-c', cmd], cwd='.') + + +def main(): + is_windows = platform.system() == 'Windows' + + if is_windows: + print('Windows is not supported at this time.') + exit(1) + + parser = argparse.ArgumentParser(description='Gazebo simulation') + + parser.add_argument('--world', help='World to run in Gazebo', required=False, default="default") + parser.add_argument('--gz_partition', help='Gazebo partition to run in', required=False) + parser.add_argument('--gz_ip', help='Outgoing network interface to use for traffic',required=False) + parser.add_argument('--interactive',help='Run in interactive mode', required=False, default=False, action='store_true') + parser.add_argument('--model_download_source', help='Path to directory containing model files', required=False, default=DEFAULT_DOWNLOAD_DIR) + parser.add_argument('--model_store', help='Path to model storage directory', required=False, default="~/.simulation-gazebo") + parser.add_argument('--overwrite', help='Overwrite existing model directories', required=False, default=False, action='store_true') + parser.add_argument('--dryrun', help='Test in dryrun. Do not launch gazebo', required=False, default=False, action='store_true') + + args = parser.parse_args() + + # Set up environment variables to look for models for simulation + args.model_store = os.path.expanduser(args.model_store) + + # Check whether the model storage directory exists. + if not os.path.exists(args.model_store): + print("Making model storage directory") + os.makedirs(args.model_store) + + + model_count = int(subprocess.check_output(f'find {args.model_store} -type f | wc -l', shell=True, text=True)) + models_exist = True if model_count > 0 else False + print(f"Found: {model_count} files in {args.model_store}") + + if models_exist and not args.overwrite: + print("Models directory not empty. Overwrite not set. Not downloading models.") + + elif args.overwrite and models_exist: + try: + subdirectories = [os.path.join(args.model_store, d) for d in os.listdir(args.model_store) if os.path.isdir(os.path.join(args.model_store, d))] + for directory in subdirectories: + shutil.rmtree(directory) + print("Overwrite set. Removed existing model subdirectories.") + except: + print("No models dir present, overwrite did not remove a directory.") + + + if ((not models_exist) or args.overwrite): + # Download model from model_download_source to model_store + if args.interactive and not args.dryrun: + + input_a = input("Interactive mode set. Would you like to provide a custom download directory? (Y/N): ") + if "y" in input_a.lower(): + args.model_download_source = input("Enter download path (specify zip file): ") + print(f"Using custom download directory: {args.model_download_source}") + + if args.model_download_source.startswith("http"): + os.system(f'curl -L -o {args.model_store}/resources.zip {args.model_download_source}') + else: + print(f'Using local file system: {args.model_download_source}') + os.system(f'cp -r {args.model_download_source} {args.model_store}/resources.zip') + + else: + print(f"Using default download directory: {DEFAULT_DOWNLOAD_DIR}") + os.system(f'curl -L -o {args.model_store}/resources.zip {DEFAULT_DOWNLOAD_DIR}') + + if not args.interactive or args.dryrun: + print('Downloading from Github...') + os.system(f'curl -L -o {args.model_store}/resources.zip {DEFAULT_DOWNLOAD_DIR}') + + + try: + shutil.unpack_archive(f'{args.model_store}/resources.zip', args.model_store, 'zip') + except: + print(f'Warning: Could not unzip model files at {args.model_store}/resources.zip. Check model file format.') + + os.system(f'mkdir {args.model_store}/tmp') + os.system(f'mv {args.model_store}/PX4-gazebo-models-main/models {args.model_store}/tmp/') + os.system(f'mv {args.model_store}/PX4-gazebo-models-main/worlds {args.model_store}/tmp/') + os.system(f'rm -rf {args.model_store}/PX4-gazebo-models-main/') + os.system(f'rm {args.model_store}/resources.zip') + os.system(f'mv {args.model_store}/tmp/models {args.model_store}/models') + os.system(f'mv {args.model_store}/tmp/worlds {args.model_store}/worlds') + os.system(f'rm -rf {args.model_store}/tmp') + + # Launch gazebo simulation + print('> Launching gazebo simulation...') + if not args.dryrun: + cmd = f'GZ_SIM_RESOURCE_PATH={args.model_store} gz sim -r {args.model_store}/worlds/{args.world}.sdf' + + if args.gz_partition: + cmd = f'GZ_PARTITION={args.gz_partition} {cmd}' + if args.gz_ip: + cmd = f'GZ_IP={args.gz_ip} {cmd}' + + run(cmd) + +if __name__ == "__main__": + main() diff --git a/tests/test_model.py b/tests/test_model.py new file mode 100644 index 0000000..2a17ad2 --- /dev/null +++ b/tests/test_model.py @@ -0,0 +1,202 @@ +import os +import subprocess +import pytest + +@pytest.mark.parametrize("world, model_download_source, model_store, interactive, overwrite, dryrun",[ + # Default directory already exists + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", False, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", False, True, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", True, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", True, True, True) + +]) +def test_model_exist_default(world, interactive, model_download_source, model_store, overwrite, dryrun): + """ + Test model generation + """ + # Ensure directory exists and has mockfile in it + model_store = os.path.expanduser(model_store) + if not os.path.exists(model_store): + os.makedirs(model_store) + + mock_file_path = os.path.join(model_store, "mock_file.txt") + with open(mock_file_path, "w") as f: + f.write("This is a mock file") + + output_directory = f"{model_store}/models/x500" + + command = ( + f"python simulation-gazebo " + f"--world {world} " + f"--model_download_source {model_download_source} " + f"--model_store {model_store} " + ) + + if interactive: + command += "--interactive " + if overwrite: + command += "--overwrite " + if dryrun: + command += "--dryrun " + + subprocess.run(command, shell=True, check=True) + try: + assert os.path.exists(output_directory) + except AssertionError: + print(f"Output directory not generated correctly: {model_download_source}") + + finally: + #Clean up + parent_dir = "~/.simulation-gazebo" + parent_dir = os.path.expanduser(parent_dir) + os.system(f"rm -r {parent_dir}") + + + + +@pytest.mark.parametrize("world, model_download_source, model_store, interactive, overwrite, dryrun",[ + + # Default directory does not exist + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", False, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", False, True, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", True, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/.simulation-gazebo", True, True, True) + +]) +def test_model_not_exist_default(world, interactive, model_download_source, model_store, overwrite, dryrun): + """ + Test model generation + """ + model_store = os.path.expanduser(model_store) + + parent_dir = "~/.simulation-gazebo" + parent_dir = os.path.expanduser(parent_dir) + try: + os.system(f"rm -r {parent_dir}") + except: + pass + + output_directory = f"{model_store}/models/x500" + + command = ( + f"python simulation-gazebo " + f"--world {world} " + f"--model_download_source {model_download_source} " + f"--model_store {model_store} " + ) + + if interactive: + command += "--interactive " + if overwrite: + command += "--overwrite " + if dryrun: + command += "--dryrun " + + subprocess.run(command, shell=True, check=True) + try: + assert os.path.exists(output_directory) + except AssertionError: + print(f"Output directory not generated correctly: {model_download_source}") + + finally: + ##Clean up + os.system(f"rm -r {parent_dir}") + + + +@pytest.mark.parametrize("world, model_download_source, model_store, interactive, overwrite, dryrun",[ + # Custom directory already exists + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", False, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", False, True, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", True, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", True, True, True) +]) +def test_model_exist_custom(world, interactive, model_download_source, model_store, overwrite, dryrun): + """ + Test model generation + """ + # Ensure directory exists and has mockfile in it + model_store = os.path.expanduser(model_store) + if not os.path.exists(model_store): + os.makedirs(model_store) + + mock_file_path = os.path.join(model_store, "mock_file.txt") + with open(mock_file_path, "w") as f: + f.write("This is a mock file") + + output_directory = f"{model_store}/models/x500" + + command = ( + f"python simulation-gazebo " + f"--world {world} " + f"--model_download_source {model_download_source} " + f"--model_store {model_store} " + ) + + if interactive: + command += "--interactive " + if overwrite: + command += "--overwrite " + if dryrun: + command += "--dryrun " + + subprocess.run(command, shell=True, check=True) + try: + assert os.path.exists(output_directory) + except AssertionError: + print(f"Output directory not generated correctly: {model_download_source}") + + finally: + #Clean up + parent_dir = "~/simulation-gz-custom-test" + parent_dir = os.path.expanduser(parent_dir) + os.system(f"rm -r {parent_dir}") + + + +@pytest.mark.parametrize("world, model_download_source, model_store, interactive, overwrite, dryrun",[ + # Custom directory does not exist + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", False, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", False, True, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", True, False, True), + ("default", "https://github.com/PX4/PX4-gazebo-models/archive/refs/heads/main.zip", "~/simulation-gz-custom-test", True, True, True) +]) +def test_model_not_exist_custom(world, interactive, model_download_source, model_store, overwrite, dryrun): + """ + Test model generation + """ + model_store = os.path.expanduser(model_store) + + parent_dir = "~/simulation-gz-custom-test" + parent_dir = os.path.expanduser(parent_dir) + try: + os.system(f"rm -r {parent_dir}") + except: + pass + + + output_directory = f"{model_store}/models/x500" + + command = ( + f"python simulation-gazebo " + f"--world {world} " + f"--model_download_source {model_download_source} " + f"--model_store {model_store} " + ) + + if interactive: + command += "--interactive " + if overwrite: + command += "--overwrite " + if dryrun: + command += "--dryrun " + + subprocess.run(command, shell=True, check=True) + try: + assert os.path.exists(output_directory) + except AssertionError: + print(f"Output directory not generated correctly: {model_download_source}") + + finally: + #Clean up + os.system(f"rm -r {parent_dir}")