diff --git a/.github/workflows/test_on_self_hosted.yml b/.github/workflows/test_on_self_hosted.yml index e3865fa3..f5b7769e 100644 --- a/.github/workflows/test_on_self_hosted.yml +++ b/.github/workflows/test_on_self_hosted.yml @@ -1,3 +1,7 @@ +# Local run (self-hosted): from repo root export UE_ROOT and set DISPLAY +# (for example `export UE_ROOT=/path/to/UnrealEngine; export DISPLAY=:0`), +# then execute the same sequence used below: +# `git submodule sync; git submodule update --init --recursive; ./setup_linux_dev_tools.sh; rm -rf build; ./build.sh simlibs_release; "$UE_ROOT/Engine/Build/BatchFiles/Linux/Build.sh" BlocksEditor Linux Development "$(pwd)/unreal/Blocks/Blocks.uproject" -waitmutex; python3 -m venv airsimenv; source airsimenv/bin/activate; python -m pip install --upgrade pip; python -m pip install "setuptools<81" wheel; python -m pip install -e "client/python/projectairsim[datacollection]"; python -m pip install pytest pytest-cov pytest-asyncio; launch Unreal headless on DISPLAY=:0; wait for ports 8989/8990; cd client/python/projectairsim/tests; python -m pytest -v --junitxml=pytest-results.xml`. name: ProjectAirSim CI (Self-Hosted) on: @@ -26,6 +30,7 @@ jobs: env: UE_ROOT: ${{ secrets.UE_ROOT }} + PX4_ROOT: ${{ secrets.PX4_ROOT }} AIRSIM_PY_ENV: ${{ github.workspace }}/airsimenv DEBIAN_FRONTEND: noninteractive @@ -164,7 +169,61 @@ jobs: run: | source "$AIRSIM_PY_ENV/bin/activate" cd client/python/projectairsim/tests - python -m pytest -v --junitxml=pytest-results.xml + python -m pytest -v --ignore=test_px4_sitl.py --junitxml=pytest-results.xml + + - name: Resolve PX4 folder path + run: | + set -euo pipefail + PX4_DIR="${PX4_ROOT:-$GITHUB_WORKSPACE/PX4-Autopilot}" + if [ ! -d "$PX4_DIR" ]; then + echo "PX4 folder not found at: $PX4_DIR" + echo "Set secrets.PX4_ROOT or place PX4-Autopilot at $GITHUB_WORKSPACE/PX4-Autopilot" + exit 1 + fi + echo "Using PX4 folder: $PX4_DIR" + echo "PX4_DIR=$PX4_DIR" >> "$GITHUB_ENV" + + - name: Launch PX4 SITL + run: | + set -euo pipefail + rm -f px4.pid px4.log + ( + cd "$PX4_DIR" + make px4_sitl_default none_iris + ) > px4.log 2>&1 & + echo $! > px4.pid + echo "PX4 launch PID: $(cat px4.pid)" + for i in {1..60}; do + if ps -p "$(cat px4.pid)" > /dev/null 2>&1; then + echo "PX4 SITL is running" + exit 0 + fi + sleep 1 + done + echo "PX4 SITL failed to start" + tail -n 300 px4.log || true + exit 1 + + - name: Run PX4 SITL tests + run: | + source "$AIRSIM_PY_ENV/bin/activate" + cd client/python/projectairsim/tests + python -m pytest -v test_px4_sitl.py --junitxml=pytest-results-px4_sitl.xml + + - name: Stop PX4 + if: always() + run: | + if [ -f px4.pid ]; then + kill "$(cat px4.pid)" || true + fi + pkill -f px4 || true + pkill -f none_iris || true + + - name: Dump PX4 logs on failure + if: failure() + run: | + echo "==== PX4 Log Output ====" + cat px4.log || true - name: Stop Unreal if: always() diff --git a/client/python/projectairsim/requirements.txt b/client/python/projectairsim/requirements.txt index ea40b878..05a95239 100644 --- a/client/python/projectairsim/requirements.txt +++ b/client/python/projectairsim/requirements.txt @@ -2,6 +2,7 @@ wheel pytest pytest-cov +pytest-asyncio>=0.23 # Install projectairsim package in editable development mode -e . diff --git a/client/python/projectairsim/tests/conftest.py b/client/python/projectairsim/tests/conftest.py new file mode 100644 index 00000000..2aa69551 --- /dev/null +++ b/client/python/projectairsim/tests/conftest.py @@ -0,0 +1,178 @@ +""" +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Global pytest configuration for ProjectAirSim tests. +Provides test isolation and stability improvements. +""" + +import pytest +import time +import gc + + +def _socket_is_closed(socket_obj) -> bool: + if socket_obj is None: + return True + return bool(getattr(socket_obj, "closed", False)) + + +def _client_needs_reconnect(client) -> bool: + if client is None: + return False + if not getattr(client, "state", False): + return True + if _socket_is_closed(getattr(client, "socket_topics", None)): + return True + if _socket_is_closed(getattr(client, "socket_services", None)): + return True + return False + + +def _safe_disconnect(client) -> None: + if client is None: + return + try: + client.disconnect() + except Exception: + pass + + +def _reload_world_if_possible(world) -> None: + if world is None: + return + sim_config = getattr(world, "sim_config", None) + if sim_config is None: + return + world.load_scene(sim_config, delay_after_load_sec=1.0) + + +def _extract_client_and_world(request): + client = None + world = None + + for fixture_name in ("client", "world", "drone", "multirotor", "robo_fixture", "robo"): + if fixture_name not in request.fixturenames: + continue + + try: + fixture_obj = request.getfixturevalue(fixture_name) + except Exception: + continue + + if fixture_name == "client": + client = fixture_obj + elif fixture_name == "world": + world = fixture_obj + client = getattr(fixture_obj, "client", client) + else: + client = getattr(fixture_obj, "client", client) + world = getattr(fixture_obj, "world", world) + + return client, world + + +@pytest.fixture(scope="function", autouse=True) +def ensure_client_connected(request): + client, world = _extract_client_and_world(request) + + if _client_needs_reconnect(client): + _safe_disconnect(client) + client.connect() + try: + client.get_topic_info() + except Exception: + pass + _reload_world_if_possible(world) + + yield + + +@pytest.fixture(scope="function", autouse=True) +def test_isolation(request): + """ + Automatic fixture to improve test isolation. + + Adds a delay after each test to allow the simulator to settle, + preventing race conditions and state pollution between sequential tests. + This is especially important for tests that: + - Load/unload scenes + - Modify textures/materials + - Control drone movements + - Capture images + + The delay can be skipped by marking a test with @pytest.mark.no_isolation + """ + # Before test: force garbage collection to clean up any lingering objects + gc.collect() + + yield + + # Check if test is marked to skip isolation delay + if "no_isolation" in request.keywords: + return + + # After test: add delay to allow simulator to settle + # This prevents issues with: + # - Scene loading timeouts + # - Texture/material update propagation + # - Image capture timing + time.sleep(1.5) + + # Force garbage collection after test + gc.collect() + + +@pytest.fixture(scope="session", autouse=True) +def session_setup_teardown(): + """ + Session-level setup and teardown. + Runs once before all tests and once after all tests complete. + """ + print("\n" + "="*80) + print("Starting ProjectAirSim test session") + print("Tests will run sequentially with isolation delays") + print("="*80 + "\n") + + yield + + print("\n" + "="*80) + print("ProjectAirSim test session complete") + print("="*80 + "\n") + + +@pytest.fixture(scope="module") +def module_isolation(): + """ + Module-level isolation fixture. + Add longer delay between test modules to allow major state resets. + """ + yield + # Longer delay between modules + print("\n--- Module complete, allowing simulator to reset ---") + time.sleep(3.0) + + +# Custom pytest markers for test categorization +def pytest_configure(config): + """Register custom markers for test categorization.""" + config.addinivalue_line( + "markers", "no_isolation: skip automatic test isolation delay" + ) + config.addinivalue_line( + "markers", "slow: marks tests as slow (deselect with '-m \"not slow\"')" + ) + config.addinivalue_line( + "markers", "requires_scene(name): marks tests that require specific scene loaded" + ) + + +def pytest_runtest_makereport(item, call): + """ + Hook to track test failures and potentially adjust isolation behavior. + """ + if call.when == "call": + # Could add custom logic here to increase delays after failures + pass + diff --git a/client/python/projectairsim/tests/generate_hello_drone_reference_images.py b/client/python/projectairsim/tests/generate_hello_drone_reference_images.py new file mode 100644 index 00000000..ef2d540e --- /dev/null +++ b/client/python/projectairsim/tests/generate_hello_drone_reference_images.py @@ -0,0 +1,156 @@ +""" +Capture reference images for test_hello_drone.py correlation checks. + +Usage: + python generate_hello_drone_reference_images.py --output-dir ./refs + +Outputs: +- hello_drone___ref.png: Camera RGB scene reference + +Where is one of: initial, move_up, move_north, move_west, +move_south, move_east, move_down, and is one of: down_camera, front_camera. +""" + +from __future__ import annotations + +import asyncio +import argparse +from pathlib import Path +from datetime import datetime + +import cv2 +import numpy as np + +from projectairsim import Drone, ProjectAirSimClient, World +from projectairsim.types import ImageType + + +import math + +STEP_SEQUENCE = [ + ("initial", None), + ("move_up", {"v_north": 0.0, "v_east": 0.0, "v_down": -2.0, "duration": 5.0}), + ("move_north1", {"v_north": 2.0, "v_east": 0.0, "v_down": 0.0, "duration": 5.0}), + ("rotate_z_60", {"rotate_z_rate": math.radians(30.0), "seconds": 2.0}), + ("move_north2", {"v_north": 2.0, "v_east": 0.0, "v_down": 0.0, "duration": 5.0}), + ("rotate_z_45a", {"rotate_z_rate": math.radians(22.5), "seconds": 2.0}), + ("move_west", {"v_north": 0.0, "v_east": -2.0, "v_down": 0.0, "duration": 5.0}), + ("rotate_z_45b", {"rotate_z_rate": math.radians(22.5), "seconds": 2.0}), +] + + +def _decode_scene_bgr(scene_msg: dict) -> np.ndarray: + width = int(scene_msg["width"]) + height = int(scene_msg["height"]) + raw = scene_msg["data"] + if isinstance(raw, list): + buf = np.array(raw, dtype=np.uint8) + else: + buf = np.frombuffer(raw, dtype=np.uint8) + + expected = width * height * 3 + if buf.size < expected: + raise ValueError(f"scene buffer too small: {buf.size} < {expected}") + return buf[:expected].reshape((height, width, 3)) + + +async def _get_images_with_retry(drone: Drone, camera_id: str, retries: int = 6) -> dict: + last_error: Exception | None = None + for attempt in range(1, retries + 1): + try: + return drone.get_images(camera_id, [ImageType.SCENE]) + except RuntimeError as error: + last_error = error + if "Timeout waiting for new captured_images" not in str(error): + raise + if attempt < retries: + await asyncio.sleep(0.35) + raise RuntimeError(f"failed to capture from {camera_id} after {retries} attempts") from last_error + + +async def _capture_step_references( + world: World, + drone: Drone, + output_dir: Path, + step: str, +) -> None: + await asyncio.sleep(5.0) + + down_images = await _get_images_with_retry(drone, "DownCamera") + front_images = await _get_images_with_retry(drone, "FrontCamera") + + # Process DownCamera images + down_scene_png = output_dir / f"hello_drone_{step}_down_camera_ref.png" + + down_scene_bgr = _decode_scene_bgr(down_images[ImageType.SCENE]) + + if not cv2.imwrite(str(down_scene_png), down_scene_bgr): + raise RuntimeError(f"failed to write scene reference: {down_scene_png}") + + print(f"saved {step}:") + print(f" down_camera scene: {down_scene_png}") + + front_scene_png = output_dir / f"hello_drone_{step}_front_camera_ref.png" + + front_scene_bgr = _decode_scene_bgr(front_images[ImageType.SCENE]) + + if not cv2.imwrite(str(front_scene_png), front_scene_bgr): + raise RuntimeError(f"failed to write scene reference: {front_scene_png}") + + print(f" front_camera scene: {front_scene_png}") + + +async def _run(args: argparse.Namespace) -> None: + base_output_dir = Path(args.output_dir).resolve() + base_output_dir.mkdir(parents=True, exist_ok=True) + run_id = datetime.now().strftime("%Y%m%d_%H%M%S") + output_dir = base_output_dir / f"hello_drone_refs_{run_id}" + output_dir.mkdir(parents=True, exist_ok=False) + + client = ProjectAirSimClient() + client.connect() + + try: + world = World(client, args.scene_name, 1) + drone = Drone(client, world, "Drone1") + + drone.enable_api_control() + drone.arm() + + for step, cmd in STEP_SEQUENCE: + if cmd is not None: + if "rotate_z_rate" in cmd and "seconds" in cmd: + await drone.rotate_by_yaw_rate_async(cmd["rotate_z_rate"], cmd["seconds"]) + else: + move = await drone.move_by_velocity_async(**cmd) + await move + await _capture_step_references(world, drone, output_dir, step) + + drone.disarm() + drone.disable_api_control() + + print("\nUse these env vars when running the test:") + print(f" PROJECTAIRSIM_TEST_HELLO_DRONE_REF_IMAGE={output_dir}") + print(f" Generated files folder: {output_dir}") + finally: + client.disconnect() + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--output-dir", + default=".", + help="Directory to save generated reference files", + ) + parser.add_argument( + "--scene-name", + default="scene_test_drone_hello_drone_refs.jsonc", + help="Scene config used by test_hello_drone.py", + ) + args = parser.parse_args() + asyncio.run(_run(args)) + + +if __name__ == "__main__": + main() diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_down_camera_ref.png new file mode 100644 index 00000000..6644104d Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_front_camera_ref.png new file mode 100644 index 00000000..92b8055c Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_initial_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_down_camera_ref.png new file mode 100644 index 00000000..d7f0a64c Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_front_camera_ref.png new file mode 100644 index 00000000..26aa38c0 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north1_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_down_camera_ref.png new file mode 100644 index 00000000..1627b850 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_front_camera_ref.png new file mode 100644 index 00000000..09d83ed8 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_north2_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_down_camera_ref.png new file mode 100644 index 00000000..34c8e7d8 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_front_camera_ref.png new file mode 100644 index 00000000..5257d3d0 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_up_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_down_camera_ref.png new file mode 100644 index 00000000..977cda5f Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_front_camera_ref.png new file mode 100644 index 00000000..6337448c Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_move_west_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_down_camera_ref.png new file mode 100644 index 00000000..e6605626 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_front_camera_ref.png new file mode 100644 index 00000000..cd9c645d Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45a_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_down_camera_ref.png new file mode 100644 index 00000000..4be58614 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_front_camera_ref.png new file mode 100644 index 00000000..1e169337 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_45b_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_down_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_down_camera_ref.png new file mode 100644 index 00000000..01411a67 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_down_camera_ref.png differ diff --git a/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_front_camera_ref.png b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_front_camera_ref.png new file mode 100644 index 00000000..7400cac8 Binary files /dev/null and b/client/python/projectairsim/tests/hello_drone_ref_images/hello_drone_rotate_z_60_front_camera_ref.png differ diff --git a/client/python/projectairsim/tests/image_validation_utils.py b/client/python/projectairsim/tests/image_validation_utils.py new file mode 100644 index 00000000..1cb90270 --- /dev/null +++ b/client/python/projectairsim/tests/image_validation_utils.py @@ -0,0 +1,80 @@ +""" +Shared helpers for RGB image validation in pytest (non-test module; not collected). + +Uses OpenCV + NumPy only (already project dependencies). +""" + +from __future__ import annotations + +import os +from typing import Optional, Tuple + +import cv2 +import numpy as np + + +def grayscale_normalized_cross_correlation( + img_bgr: np.ndarray, ref_bgr: np.ndarray +) -> float: + """Return Pearson correlation of flattened grayscale patches (1.0 = identical up to affine scaling). + + Both images are resized to the reference dimensions before comparison. + """ + if ref_bgr is None or ref_bgr.size == 0 or img_bgr is None or img_bgr.size == 0: + return 0.0 + h, w = ref_bgr.shape[:2] + cur = cv2.resize(img_bgr, (w, h), interpolation=cv2.INTER_AREA) + g1 = cv2.cvtColor(cur, cv2.COLOR_BGR2GRAY).astype(np.float64) + g2 = cv2.cvtColor(ref_bgr, cv2.COLOR_BGR2GRAY).astype(np.float64) + g1 -= g1.mean() + g2 -= g2.mean() + n = float(np.linalg.norm(g1.ravel()) * np.linalg.norm(g2.ravel())) + if n < 1e-9: + return 0.0 + return float(np.dot(g1.ravel(), g2.ravel()) / n) + + +def assert_rgb_scene_image_valid( + img_msg: dict, + *, + reference_image_path: str = "", + min_similarity_to_reference: float = 0.85, + min_gray_std: float = 2.0, +) -> Tuple[Optional[float], np.ndarray]: + """Decode a scene camera message and assert it carries plausible image data. + + Always checks non-empty buffer, shape, and non-flat intensity (std on grayscale). + + If ``reference_image_path`` is set (non-empty and file exists), also asserts + normalized grayscale correlation to that reference is >= ``min_similarity_to_reference``. + + Returns: + (similarity_or_none, bgr_image) for optional extra assertions by the caller. + """ + assert img_msg is not None + assert "data" in img_msg and "width" in img_msg and "height" in img_msg + w, h = int(img_msg["width"]), int(img_msg["height"]) + assert w > 0 and h > 0 + raw = img_msg["data"] + if isinstance(raw, list): + nparr = np.array(raw, dtype=np.uint8) + else: + nparr = np.frombuffer(raw, dtype=np.uint8) + expected = w * h * 3 + assert nparr.size >= expected, f"buffer too small: {nparr.size} < {expected}" + bgr = nparr[:expected].reshape((h, w, 3)) + gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) + assert float(gray.std()) >= min_gray_std, "image looks flat or empty (low std)" + + sim: Optional[float] = None + path = (reference_image_path or "").strip() + if path and os.path.isfile(path): + ref = cv2.imread(path, cv2.IMREAD_COLOR) + assert ref is not None and ref.size > 0, f"failed to load reference: {path}" + sim = grayscale_normalized_cross_correlation(bgr, ref) + assert sim >= min_similarity_to_reference, ( + f"scene image similarity {sim:.4f} below minimum " + f"{min_similarity_to_reference:.4f} vs reference {path}" + ) + print("similarity to reference:", sim) + return sim, bgr diff --git a/client/python/projectairsim/tests/pytest.ini b/client/python/projectairsim/tests/pytest.ini index fe55d2ed..4fab545c 100644 --- a/client/python/projectairsim/tests/pytest.ini +++ b/client/python/projectairsim/tests/pytest.ini @@ -1,2 +1,22 @@ [pytest] junit_family=xunit2 + +# Markers for test categorization +markers = + no_isolation: skip automatic test isolation delay + slow: marks tests as slow (deselect with '-m "not slow"') + requires_scene: marks tests that require specific scene loaded + +# Test discovery patterns +python_files = test_*.py +python_classes = Test* +python_functions = test_* + +# Console output options +console_output_style = progress +log_cli = false +log_cli_level = INFO + +# Timeout settings (requires pytest-timeout plugin) +# timeout = 300 +# timeout_method = thread diff --git a/client/python/projectairsim/tests/sim_config/robot_test_quadrotor_fastphysics_hello_drone.jsonc b/client/python/projectairsim/tests/sim_config/robot_test_quadrotor_fastphysics_hello_drone.jsonc new file mode 100644 index 00000000..6a23c2ff --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/robot_test_quadrotor_fastphysics_hello_drone.jsonc @@ -0,0 +1,489 @@ +{ + "physics-type": "fast-physics", + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/Quadrotor1" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_FR", + "inertial": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_RL", + "inertial": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + }, + { + "name": "Prop_RR", + "inertial": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_FR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FR", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RR", + "axis": "0 0 1" + } + ], + "controller": { + "id": "Simple_Flight_Controller", + "airframe-setup": "quadrotor-x", + "type": "simple-flight-api", + "simple-flight-api-settings": { + "parameters": { + "MC_YAW_P": 0.25 + }, + "actuator-order": [ + { + "id": "Prop_FR_actuator" + }, + { + "id": "Prop_RL_actuator" + }, + { + "id": "Prop_FL_actuator" + }, + { + "id": "Prop_RR_actuator" + } + ] + } + }, + "actuators": [ + { + "name": "Prop_FL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_FR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FR", + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RL", + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RR", + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-10.0 0.0 -1.0", + "rpy-deg": "0 -11.46 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.001, + "capture-settings": [ + { + "image-type": 0, //rgb_camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, //depth_planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 2, //depth_perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 3, //segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy-deg": "0 -90 0" + } + }, + { + "id": "FrontCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.01, + "capture-settings": [ + { + "image-type": 0, //rgb_camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, //depth_planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 2, //depth_perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 3, //segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [], + "origin": { + "xyz": "0.45 0.0 -0.05", + "rpy-deg": "0 0 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 0.0123, + "tau": 800, + "bias-stability": 2e-5, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 0.0123, + "tau": 500, + "bias-stability": 1e-6, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + } + ] +} \ No newline at end of file diff --git a/client/python/projectairsim/tests/sim_config/robot_test_required_schema_hello_drone.jsonc b/client/python/projectairsim/tests/sim_config/robot_test_required_schema_hello_drone.jsonc new file mode 100644 index 00000000..2118ee37 --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/robot_test_required_schema_hello_drone.jsonc @@ -0,0 +1,431 @@ +{ + //PhysicsType is required by the robot schema. It has been removed to test the raised error type + "links": [ + { + "name": "Frame", + "inertial": { + "mass": 1.0, + "inertia": { + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "box": { + "size": "0.180 0.110 0.040" + } + } + } + }, + "collision": { + "restitution": 0.1, + "friction": 0.5 + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/Quadrotor1" + } + } + }, + { + "name": "Prop_FL", + "inertial": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_FR", + "inertial": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerRed" + } + } + }, + { + "name": "Prop_RL", + "inertial": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + }, + { + "name": "Prop_RR", + "inertial": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "mass": 0.055, + "inertia": { + "type": "point-mass" + }, + "aerodynamics": { + "drag-coefficient": 0.325, + "type": "geometry", + "geometry": { + "cylinder": { + "radius": 0.1143, + "length": 0.01 + } + } + } + }, + "visual": { + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "geometry": { + "type": "unreal_mesh", + "name": "/Drone/PropellerWhite" + } + } + } + ], + "joints": [ + { + "id": "Frame_Prop_FL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_FR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_FR", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RL", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RL", + "axis": "0 0 1" + }, + { + "id": "Frame_Prop_RR", + "type": "fixed", + "parent-link": "Frame", + "child-link": "Prop_RR", + "axis": "0 0 1" + } + ], + "controller": { + "id": "Simple_Flight_Controller", + "airframe-setup": "quadrotor-x", + "type": "simple-flight-api", + "simple-flight-api-settings": { + "actuator-order": [ + { + "id": "Prop_FR_actuator" + }, + { + "id": "Prop_RL_actuator" + }, + { + "id": "Prop_FL_actuator" + }, + { + "id": "Prop_RR_actuator" + } + ] + } + }, + "actuators": [ + { + "name": "Prop_FL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FL", + "origin": { + "xyz": "0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_FR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_FR", + "origin": { + "xyz": "0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RL_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RL", + "origin": { + "xyz": "-0.253 -0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "counter-clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + }, + { + "name": "Prop_RR_actuator", + "type": "rotor", + "enabled": true, + "parent-link": "Frame", + "child-link": "Prop_RR", + "origin": { + "xyz": "-0.253 0.253 -0.01", + "rpy-deg": "0 0 0" + }, + "rotor-settings": { + "turning-direction": "clock-wise", + "normal-vector": "0.0 0.0 -1.0", + "coeff-of-thrust": 0.109919, + "coeff-of-torque": 0.040164, + "max-rpm": 6396.667, + "propeller-diameter": 0.2286, + "smoothing-tc": 0.005 + } + } + ], + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 1280, + "height": 720, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "gimbal": { + "lock-roll": true, + "lock-pitch": true, + "lock-yaw": false + }, + "origin": { + "xyz": "-10.0 0.0 -1.0", + "rpy-deg": "0 -11.46 0" + } + }, + { + "id": "DownCamera", + "type": "camera", + "enabled": true, + "parent-link": "Frame", + "capture-interval": 0.001, + "capture-settings": [ + { + "image-type": 0, //rgb_camera + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + }, + { + "image-type": 1, //depth_planar + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 2, //depth_perspective + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + }, + { + "image-type": 3, //segmentation + "width": 400, + "height": 225, + "fov-degrees": 90, + "capture-enabled": false, + "streaming-enabled": false, + "pixels-as-float": false, + "compress": false + } + ], + "noise-settings": [ + { + "enabled": false, + "image-type": 1, + "rand-contrib": 0.2, + "rand-speed": 100000.0, + "rand-size": 500.0, + "rand-density": 2, + "horz-wave-contrib": 0.03, + "horz-wave-strength": 0.08, + "horz-wave-vert-size": 1.0, + "horz-wave-screen-size": 1.0, + "horz-noise-lines-contrib": 1.0, + "horz-noise-lines-density-y": 0.01, + "horz-noise-lines-density-xy": 0.5, + "horz-distortion-contrib": 1.0, + "horz-distortion-strength": 0.002 + } + ], + "origin": { + "xyz": "0 0.0 0.0", + "rpy-deg": "0 -90 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "parent-link": "Frame", + "accelerometer": { + "velocity-random-walk": 0.0123, + "tau": 800, + "bias-stability": 2e-5, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 0.0123, + "tau": 500, + "bias-stability": 1e-6, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": false, + "parent-link": "Frame" + } + ] + } \ No newline at end of file diff --git a/client/python/projectairsim/tests/sim_config/scene_test_drone_hello_drone_refs.jsonc b/client/python/projectairsim/tests/sim_config/scene_test_drone_hello_drone_refs.jsonc new file mode 100644 index 00000000..ad8a39e8 --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/scene_test_drone_hello_drone_refs.jsonc @@ -0,0 +1,30 @@ +{ + "id": "SceneBasicDrone", + "actors": [ + { + "type": "robot", + "name": "Drone1", + "origin": { + "xyz": "0 0 -4.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "robot_test_quadrotor_fastphysics_hello_drone.jsonc" + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 122.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + } +} \ No newline at end of file diff --git a/client/python/projectairsim/tests/test_api_services.py b/client/python/projectairsim/tests/test_api_services.py index f583c921..3932ec49 100644 --- a/client/python/projectairsim/tests/test_api_services.py +++ b/client/python/projectairsim/tests/test_api_services.py @@ -9,14 +9,19 @@ from datetime import datetime import math import time +from typing import Dict, Tuple +import numpy as np from pynng import NNGException import pytest -from typing import Dict from projectairsim import Drone, ProjectAirSimClient, World from projectairsim.image_utils import segmentation_id_to_color, segmentation_color_to_id -from projectairsim.utils import geo_to_ned_coordinates +from projectairsim.utils import ( + geo_to_ned_coordinates, + quaternion_to_rpy, + unpack_image, +) from projectairsim.types import ( Pose, Vector3, @@ -29,6 +34,107 @@ ) +def _bgr_center_mean(image_msg: Dict) -> np.ndarray: + """Mean BGR in a center crop of a camera image message.""" + bgr = unpack_image(image_msg) + h, w = bgr.shape[:2] + cy, cx = h // 2, w // 2 + y0, y1 = max(0, cy - 24), min(h, cy + 24) + x0, x1 = max(0, cx - 24), min(w, cx + 24) + patch = bgr[y0:y1, x0:x1] + return patch.mean(axis=(0, 1)).astype(np.float64) + + +def _bgr_object_mean(image_msg: Dict, object_name: str) -> np.ndarray: + """Mean BGR inside the annotated bbox for a given object, if available.""" + bgr = unpack_image(image_msg) + annotations = image_msg.get("annotations") or [] + + for annotation in annotations: + if annotation.get("object_id") != object_name: + continue + + bbox = annotation.get("bbox2d") or {} + center = bbox.get("center") or {} + size = bbox.get("size") or {} + + cx = int(round(center.get("x", bgr.shape[1] / 2))) + cy = int(round(center.get("y", bgr.shape[0] / 2))) + half_w = max(4, int(round(size.get("x", 0) / 2.0))) + half_h = max(4, int(round(size.get("y", 0) / 2.0))) + + x0 = max(0, cx - half_w) + x1 = min(bgr.shape[1], cx + half_w) + y0 = max(0, cy - half_h) + y1 = min(bgr.shape[0], cy + half_h) + + patch = bgr[y0:y1, x0:x1] + if patch.size > 0: + return patch.mean(axis=(0, 1)).astype(np.float64) + + return _bgr_center_mean(image_msg) + + +def _capture_object_mean( + drone: Drone, + world: World, + object_name: str, + camera_id: str = "DownCamera", + settle_sec: float = 0.3, +) -> np.ndarray: + drone.camera_look_at_object(camera_id=camera_id, object_name=object_name) + time.sleep(settle_sec) + + world.pause() + try: + image = drone.get_images(camera_id, [ImageType.SCENE])[ImageType.SCENE] + finally: + world.resume() + + return _bgr_object_mean(image, object_name) + + +def _quat_dict_rpy(q: Dict[str, float]) -> Tuple[float, float, float]: + return quaternion_to_rpy(q["w"], q["x"], q["y"], q["z"]) + + +def _make_pose(x: float, y: float, z: float) -> Pose: + translation = Vector3({"x": x, "y": y, "z": z}) + rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) + return Pose({"translation": translation, "rotation": rotation, "frame_id": "DEFAULT_ID"}) + + +def _pose_from_dict(pose_dict: Dict) -> Pose: + return Pose( + { + "translation": Vector3(pose_dict["translation"]), + "rotation": Quaternion(pose_dict["rotation"]), + "frame_id": "DEFAULT_ID", + } + ) + + +def _reset_scene_drone_to_initial(scene_drone: Drone, initial_pose_dict: Dict) -> None: + # Reset control state first so set_pose is deterministic across tests. + scene_drone.disarm() + scene_drone.disable_api_control() + scene_drone.set_pose(_pose_from_dict(initial_pose_dict)) + + +def _ensure_shared_scene_active(scene_world: World, scene_drone: Drone) -> None: + """Reload shared scene only when another test switched away from it.""" + try: + scene_world.get_sim_clock_type() + scene_drone.get_ground_truth_pose() + return + except Exception: + pass + + scene_world.load_scene(scene_world.sim_config, delay_after_load_sec=1.0) + scene_drone.set_topics(scene_world) + scene_drone.home_geo_point = scene_world.home_geo_point + + @pytest.fixture(scope="module", autouse=True) def client(request) -> ProjectAirSimClient: client = ProjectAirSimClient() @@ -48,11 +154,38 @@ def disconnect(): return client -def test_enable_disable_api_control(client): - try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") +@pytest.fixture(scope="module") +def scene_world(client): + return World(client, "scene_test_drone.jsonc", delay_after_load_sec=1) + + +@pytest.fixture(scope="module") +def scene_drone(client, scene_world): + return Drone(client, scene_world, "Drone1") + + +INITIAL_SCENE_DRONE_POSE = { + "translation": {"x": 0.0, "y": 0.0, "z": -4.0}, + "rotation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, +} + + +@pytest.fixture(scope="module") +def initial_scene_drone_pose(): + return INITIAL_SCENE_DRONE_POSE + + +@pytest.fixture(scope="function") +def reset_scene_drone(scene_world, scene_drone, initial_scene_drone_pose): + _ensure_shared_scene_active(scene_world, scene_drone) + _reset_scene_drone_to_initial(scene_drone, initial_scene_drone_pose) + time.sleep(2.0) + return scene_drone + +def test_enable_disable_api_control(client, scene_drone): + try: + drone = scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True api_control_reported = drone.is_api_control_enabled() @@ -65,11 +198,9 @@ def test_enable_disable_api_control(client): raise Exception(str(err)) -def test_arm_disarm(client): +def test_arm_disarm(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - + drone = scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True armed = drone.arm() @@ -91,10 +222,9 @@ async def takeoff_and_land_async(drone): await land -def test_takeoff_and_land_async(client): +def test_takeoff_and_land_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -118,10 +248,9 @@ async def takeoff_and_hover_async(drone): await hover -def test_takeoff_and_hover_async(client): +def test_takeoff_and_hover_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -147,10 +276,9 @@ async def move_by_velocity_async(drone): await move_north_up -def test_move_by_velocity_async(client): +def test_move_by_velocity_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -166,26 +294,27 @@ def test_move_by_velocity_async(client): async def move_by_velocity_z_async(drone): - # basic test for now to make sure no exceptions are thrown - take_off = await drone.takeoff_async() - await take_off - + z0 = drone.get_ground_truth_pose()["translation"]["z"] + z0_gt = INITIAL_SCENE_DRONE_POSE["translation"]["z"] + assert z0 == pytest.approx(z0_gt, abs=0.5) move_at_z = await drone.move_by_velocity_z_async( v_north=2.0, v_east=0.0, z=-5.0, duration=2.0 ) await move_at_z + z1 = drone.get_ground_truth_pose()["translation"]["z"] + assert z1 == pytest.approx(-5.0, abs=1.0) + assert z1 <= z0 + 1.0 -def test_move_by_velocity_z_async(client): +def test_move_by_velocity_z_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True armed = drone.arm() assert armed is True - asyncio.run(move_by_velocity_async(drone)) + asyncio.run(move_by_velocity_z_async(drone)) disarmed = drone.disarm() assert disarmed is True api_control_disabled = drone.disable_api_control() @@ -205,10 +334,9 @@ async def move_by_velocity_body_frame_async(drone): await move_north_up -def test_move_by_velocity_body_frame_async(client): +def test_move_by_velocity_body_frame_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -224,26 +352,25 @@ def test_move_by_velocity_body_frame_async(client): async def move_by_velocity_body_frame_z_async(drone): - # basic test for now to make sure no exceptions are thrown - take_off = await drone.takeoff_async() - await take_off - + z0 = drone.get_ground_truth_pose()["translation"]["z"] move_at_z = await drone.move_by_velocity_body_frame_z_async( v_forward=2.0, v_right=0.0, z=-5.0, duration=2.0 ) await move_at_z + z1 = drone.get_ground_truth_pose()["translation"]["z"] + assert z1 == pytest.approx(-5.0, abs=4.0) + assert z1 <= z0 + 0.5 -def test_move_by_velocity_body_frame_z_async(client): +def test_move_by_velocity_body_frame_z_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True armed = drone.arm() assert armed is True - asyncio.run(move_by_velocity_body_frame_async(drone)) + asyncio.run(move_by_velocity_body_frame_z_async(drone)) disarmed = drone.disarm() assert disarmed is True api_control_disabled = drone.disable_api_control() @@ -261,10 +388,9 @@ async def move_by_heading_async(drone): await move_by_heading -def test_move_by_heading_async(client): +def test_move_by_heading_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -290,10 +416,9 @@ async def move_to_position_async(drone): await move_to_pos -def test_move_to_position_async(client): +def test_move_to_position_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -319,10 +444,9 @@ async def move_to_geo_position_async(drone): await move_to_pos -def test_move_to_geo_position_async(client): +def test_move_to_geo_position_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -351,48 +475,45 @@ async def go_home_async(drone): await go_home -def test_go_home_async(client): +def test_set_object_pose(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + object_name = "TempPoseCube" + asset_name = "1M_Cube" + original_pose = _make_pose(20, 0, -5) + updated_pose = _make_pose(25, 0, -5) + scale = [3.0, 3.0, 3.0] - api_control_enabled = drone.enable_api_control() - assert api_control_enabled is True - armed = drone.arm() - assert armed is True - asyncio.run(go_home_async(drone)) - disarmed = drone.disarm() - assert disarmed is True - api_control_disabled = drone.disable_api_control() - assert api_control_disabled is True + scene_world.spawn_object(object_name, asset_name, original_pose, scale, False) + + status = scene_world.set_object_pose(object_name, updated_pose, True) + print("Set pose success:", status) + assert status + time.sleep(0.5) + obtained_pose = scene_world.get_object_pose(object_name) + assert obtained_pose == updated_pose + scene_world.set_object_pose(object_name, original_pose, True) + scene_world.destroy_object(object_name) except NNGException as err: raise Exception(str(err)) -async def move_on_path_async(drone): - # basic test for now to make sure no exceptions are thrown - take_off = await drone.takeoff_async() - await take_off - - path = [[3, 3, -6], [3, -3, -6]] - move_on_path = await drone.move_on_path_async(path, velocity=1.0) - await move_on_path +def test_set_object_scale(client, scene_world): + try: + object_name = "TempScaleCube" + asset_name = "1M_Cube" + pose = _make_pose(20, 0, -5) + original_scale = [1.0, 1.0, 1.0] + updated_scale = [10.0, 10.0, 10.0] + scene_world.spawn_object(object_name, asset_name, pose, original_scale, False) -def test_move_on_path_async(client): - try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + status = scene_world.set_object_scale(object_name, updated_scale) + print("Set scale success:", status) + assert status + new_scale = scene_world.get_object_scale(object_name) + assert new_scale == updated_scale + scene_world.destroy_object(object_name) - api_control_enabled = drone.enable_api_control() - assert api_control_enabled is True - armed = drone.arm() - assert armed is True - asyncio.run(move_on_path_async(drone)) - disarmed = drone.disarm() - assert disarmed is True - api_control_disabled = drone.disable_api_control() - assert api_control_disabled is True except NNGException as err: raise Exception(str(err)) @@ -407,10 +528,9 @@ async def move_on_geo_path_async(drone): await move_on_path -def test_move_on_geo_path_async(client): +def test_move_on_geo_path_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -434,10 +554,9 @@ async def rotate_to_yaw_async(drone): await rotate -def test_rotate_yaw_async(client): +def test_rotate_yaw_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True @@ -453,24 +572,28 @@ def test_rotate_yaw_async(client): async def rotate_by_yaw_rate_async(drone): - # basic test for now to make sure no exceptions are thrown take_off = await drone.takeoff_async() await take_off - rotate = await drone.rotate_by_yaw_rate_async(yaw=3.14) + pose0 = drone.get_ground_truth_pose() + r0 = _quat_dict_rpy(pose0["rotation"]) + rotate = await drone.rotate_by_yaw_rate_async(yaw_rate=0.35, duration=2.5) await rotate + pose1 = drone.get_ground_truth_pose() + r1 = _quat_dict_rpy(pose1["rotation"]) + dyaw = (r1[2] - r0[2] + math.pi) % (2 * math.pi) - math.pi + assert abs(dyaw) > 0.05 -def test_rotate_by_yaw_rate_async(client): +def test_rotate_by_yaw_rate_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone api_control_enabled = drone.enable_api_control() assert api_control_enabled is True armed = drone.arm() assert armed is True - asyncio.run(rotate_to_yaw_async(drone)) + asyncio.run(rotate_by_yaw_rate_async(drone)) disarmed = drone.disarm() assert disarmed is True api_control_disabled = drone.disable_api_control() @@ -479,180 +602,160 @@ def test_rotate_by_yaw_rate_async(client): raise Exception(str(err)) -def test_get_sim_clock_type(client): +def test_get_sim_clock_type(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - clock_type = world.get_sim_clock_type() + clock_type = scene_world.get_sim_clock_type() assert "unknown" not in clock_type except NNGException as err: raise Exception(str(err)) -def test_get_sim_time(client): +def test_get_sim_time(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - sim_time = world.get_sim_time() + sim_time = scene_world.get_sim_time() assert sim_time is not None print(f"Received: sim_time={sim_time * 1e-9} seconds") except NNGException as err: raise Exception(str(err)) -def test_pause_resume(client): +def test_pause_resume(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - sim_pause_resume_state: str = world.pause() - sim_time = world.get_sim_time() + clock_type = scene_world.get_sim_clock_type() + sim_pause_resume_state: str = scene_world.pause() + sim_time = scene_world.get_sim_time() time.sleep(0.1) if "steppable" in clock_type: assert "paused" in sim_pause_resume_state - assert world.get_sim_time() == sim_time + assert scene_world.get_sim_time() == sim_time elif "real-time" in clock_type: assert "WARNING" in sim_pause_resume_state - assert world.get_sim_time() > sim_time + assert scene_world.get_sim_time() > sim_time elif "engine-driven" in clock_type: assert "WARNING" in sim_pause_resume_state - sim_pause_resume_state: str = world.resume() + sim_pause_resume_state: str = scene_world.resume() if "steppable" in clock_type: assert "resumed" in sim_pause_resume_state elif "real-time" in clock_type: assert "WARNING" in sim_pause_resume_state elif "engine-driven" in clock_type: assert "WARNING" in sim_pause_resume_state - sim_time = world.get_sim_time() + sim_time = scene_world.get_sim_time() time.sleep(0.1) - assert world.get_sim_time() > sim_time + assert scene_world.get_sim_time() > sim_time except NNGException as err: raise Exception(str(err)) -def test_is_paused(client): +def test_is_paused(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - world.pause() + clock_type = scene_world.get_sim_clock_type() + scene_world.pause() if "steppable" in clock_type: - assert world.is_paused() is True + assert scene_world.is_paused() is True elif "real-time" in clock_type: - assert world.is_paused() is False + assert scene_world.is_paused() is False elif "engine-driven" in clock_type: - assert world.is_paused() is False + assert scene_world.is_paused() is False - world.resume() - assert world.is_paused() is False + scene_world.resume() + assert scene_world.is_paused() is False except NNGException as err: raise Exception(str(err)) -def test_continue_for_sim_time(client): +def test_continue_for_sim_time(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - world.pause() - sim_time = world.get_sim_time() + clock_type = scene_world.get_sim_clock_type() + scene_world.pause() + sim_time = scene_world.get_sim_time() delta_time = 5 * 3e6 # TODO Has to be a multiple of JSON step ns - world.continue_for_sim_time(delta_time) + scene_world.continue_for_sim_time(delta_time) if "steppable" in clock_type: - assert world.get_sim_time() == sim_time + delta_time + assert scene_world.get_sim_time() == sim_time + delta_time elif "real-time" in clock_type: - assert world.get_sim_time() >= sim_time + assert scene_world.get_sim_time() >= sim_time elif "engine-driven" in clock_type: # engine-driven doesn't support continue_for_sim_time pass - world.resume() + scene_world.resume() except NNGException as err: raise Exception(str(err)) -def test_continue_until_sim_time(client): +def test_continue_until_sim_time(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - world.pause() - sim_time = world.get_sim_time() + clock_type = scene_world.get_sim_clock_type() + scene_world.pause() + sim_time = scene_world.get_sim_time() target_time = sim_time + 5 * 3e6 # TODO Has to be a multiple of JSON step ns - world.continue_until_sim_time(target_time) + scene_world.continue_until_sim_time(target_time) if "steppable" in clock_type: - assert world.get_sim_time() == target_time + assert scene_world.get_sim_time() == target_time elif "real-time" in clock_type: - assert world.get_sim_time() >= target_time + assert scene_world.get_sim_time() >= target_time elif "engine-driven" in clock_type: # engine-driven doesn't support continue_until_sim_time pass - world.resume() + scene_world.resume() except NNGException as err: raise Exception(str(err)) -def test_continue_for_n_steps(client): +def test_continue_for_n_steps(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - world.pause() - world.continue_for_n_steps(1) # Step to next multiple of the step size - sim_time = world.get_sim_time() + clock_type = scene_world.get_sim_clock_type() + scene_world.pause() + scene_world.continue_for_n_steps(1) # Step to next multiple of the step size + sim_time = scene_world.get_sim_time() step_time = 3e6 # TODO This is JSON config dependent - world.continue_for_n_steps(1) + scene_world.continue_for_n_steps(1) if "steppable" in clock_type: - assert world.get_sim_time() == sim_time + step_time * 1 + assert scene_world.get_sim_time() == sim_time + step_time * 1 elif "real-time" in clock_type: - assert world.get_sim_time() >= sim_time + assert scene_world.get_sim_time() >= sim_time - sim_time = world.get_sim_time() - world.continue_for_n_steps(3) + sim_time = scene_world.get_sim_time() + scene_world.continue_for_n_steps(3) if "steppable" in clock_type: - assert world.get_sim_time() == sim_time + step_time * 3 + assert scene_world.get_sim_time() == sim_time + step_time * 3 elif "real-time" in clock_type: - assert world.get_sim_time() >= sim_time - world.resume() + assert scene_world.get_sim_time() >= sim_time + scene_world.resume() except NNGException as err: raise Exception(str(err)) -def test_continue_for_single_step(client): +def test_continue_for_single_step(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - clock_type = world.get_sim_clock_type() - world.pause() - world.continue_for_single_step() # Step to next multiple of the step size - sim_time = world.get_sim_time() + clock_type = scene_world.get_sim_clock_type() + scene_world.pause() + scene_world.continue_for_single_step() # Step to next multiple of the step size + sim_time = scene_world.get_sim_time() step_time = 3e6 # TODO This is JSON config dependent - world.continue_for_single_step() + scene_world.continue_for_single_step() if "steppable" in clock_type: - assert world.get_sim_time() == sim_time + step_time * 1 + assert scene_world.get_sim_time() == sim_time + step_time * 1 elif "real-time" in clock_type: - assert world.get_sim_time() >= sim_time - world.resume() + assert scene_world.get_sim_time() >= sim_time + scene_world.resume() except NNGException as err: raise Exception(str(err)) -def test_list_actors(client): +def test_list_actors(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) - drone = Drone(client, world, "Drone1") - - actor_ids = world.list_actors() + actor_ids = scene_world.list_actors() print(f"Received actor_ids: {actor_ids}") - assert drone.name in actor_ids + assert scene_drone.name in actor_ids except NNGException as err: raise Exception(str(err)) -def test_list_assets(client): +def test_list_assets(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - asset_ids = world.list_assets(".*") + asset_ids = scene_world.list_assets(".*") print(f"Received actor_ids: {asset_ids}") # Assets required by other tests assert "BasicLandingPad" in asset_ids @@ -661,12 +764,10 @@ def test_list_assets(client): raise Exception(str(err)) -def test_list_objects(client): +def test_list_objects(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - name_regex = ".*" - scene_objects_list = world.list_objects(name_regex) + scene_objects_list = scene_world.list_objects(name_regex) print("object_list:", scene_objects_list) assert len(scene_objects_list) @@ -674,12 +775,10 @@ def test_list_objects(client): raise Exception(str(err)) -def test_get_object_pose(client): +def test_get_object_pose(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_id: str = "OrangeBall" # Present in Blocks env - object_pose = world.get_object_pose(object_id) + object_pose = scene_world.get_object_pose(object_id) print("object_pose:", object_pose) assert object_pose is not None assert isinstance(object_pose, Pose) @@ -693,14 +792,12 @@ def test_get_object_pose(client): raise Exception(str(err)) -def test_get_object_poses(client): +def test_get_object_poses(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - # "OrangeBall" is a scene object in Blocks env, "Drone1" is the spawned robot # that can also be queried as an object object_ids = ["OrangeBall", "Drone1"] - object_poses = world.get_object_poses(object_ids) + object_poses = scene_world.get_object_poses(object_ids) print("object_poses:", object_poses) assert object_poses is not None assert len(object_poses) == 2 @@ -723,38 +820,10 @@ def test_get_object_poses(client): raise Exception(str(err)) -def test_set_object_pose(client): +def test_get_object_scale(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - object_query: str = "OrangeBall" # Present in Blocks env - objects = world.list_objects(object_query + ".*") - assert len(objects) - object_id = objects[0] - orig_pose = world.get_object_pose(object_id) - ref_frame = "DEFAULT_ID" - translation = Vector3({"x": 20, "y": 0, "z": 0}) - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) - new_pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} - ) - status = world.set_object_pose(object_id, new_pose, True) - print("Set pose success:", status) - assert status - time.sleep(0.5) - obtained_pose = world.get_object_pose(object_id) - assert obtained_pose == new_pose - world.set_object_pose(object_id, orig_pose, True) # return to orig pose - except NNGException as err: - raise Exception(str(err)) - - -def test_get_object_scale(client): - try: - world = World(client, "scene_test_drone.jsonc", 0) - object_id: str = "OrangeBall" # Present in Blocks env - object_scale = world.get_object_scale(object_id) + object_scale = scene_world.get_object_scale(object_id) print("object_scale:", object_scale) assert object_scale is not None assert not any( @@ -765,87 +834,56 @@ def test_get_object_scale(client): raise Exception(str(err)) -def test_set_object_scale(client): +def test_spawn_destroy_object(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - object_id: str = "OrangeBall" # Present in Blocks env - scale = [10.0, 10.0, 10.0] - status = world.set_object_scale(object_id, scale) - print("Set scale success:", status) - assert status - new_scale = world.get_object_scale(object_id) - assert new_scale == scale - - except NNGException as err: - raise Exception(str(err)) - - -def test_spawn_destroy_object(client): - try: - world = World(client, "scene_test_drone.jsonc", 0) - # ------------------------------------------------------------------ # Check spawning a static mesh object with its physics disabled - object_name: str = "TestLandingPad" + object_name: str = "TempLandingPad" asset_name: str = "BasicLandingPad" # Existing asset - ref_frame = "DEFAULT_ID" - - translation = Vector3({"x": 20, "y": 1, "z": -5}) - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} - ) + pose = _make_pose(20, 1, -5) scale = [10.0, 10.0, 10.0] enable_physics: bool = False - retval = world.spawn_object( + retval = scene_world.spawn_object( object_name, asset_name, pose, scale, enable_physics ) print("Spawned static mesh object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) > 0 # Check destroying object that was just spawned - status = world.destroy_object(object_name) + status = scene_world.destroy_object(object_name) assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) # ------------------------------------------------------------------ # Check spawning a Blueprint object with its physics disabled - object_name: str = "TestBPOrangeBall" + object_name: str = "TempBPOrangeBall" asset_name: str = "OrangeBall_Blueprint" # Existing asset - ref_frame = "DEFAULT_ID" - - translation = Vector3({"x": 20, "y": 1, "z": -5}) - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} - ) + pose = _make_pose(20, 1, -5) scale = [1.0, 1.0, 1.0] enable_physics: bool = False - retval = world.spawn_object( + retval = scene_world.spawn_object( object_name, asset_name, pose, scale, enable_physics ) print("Spawned Blueprint object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) > 0 # Check destroying object that was just spawned - status = world.destroy_object(object_name) + status = scene_world.destroy_object(object_name) assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) except NNGException as err: raise Exception(str(err)) -def test_spawn_object_at_geo(client): +def test_spawn_object_at_geo(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) # Loaded scene_test_drone.jsonc has the following home geo point to compare # with spawned object lat/lon/alt below: # "home-geo-point": { @@ -864,15 +902,15 @@ def test_spawn_object_at_geo(client): # Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) scale = [1.0, 1.0, 1.0] enable_physics: bool = False - retval = world.spawn_object_at_geo( + retval = scene_world.spawn_object_at_geo( object_name, asset_name, lat, lon, alt, rotation, scale, enable_physics ) print("Spawned object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) > 0 - object_pose = world.get_object_pose(object_name) + object_pose = scene_world.get_object_pose(object_name) print("object_pose:", object_pose) assert object_pose.translation @@ -886,36 +924,29 @@ def test_spawn_object_at_geo(client): assert math.isclose(res, exp, abs_tol=0.02) # Check destroying object that was just spawned - status = world.destroy_object(object_name) + status = scene_world.destroy_object(object_name) assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) except NNGException as err: raise Exception(str(err)) -def test_spawn_object_from_file(client): +def test_spawn_object_from_file(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) # ------------------------------------------------------------------ # Check spawning a static mesh object with its physics disabled - object_name: str = "TestLandingPad" + object_name: str = "TempLandingPadFile" asset_name: str = "BasicLandingPad" # Existing asset - ref_frame = "DEFAULT_ID" - - translation = Vector3({"x": 20, "y": 1, "z": -5}) - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} - ) + pose = _make_pose(20, 1, -5) scale = [10.0, 10.0, 10.0] enable_physics: bool = False binary_gltf: bool = True file1 = open("assets/BasicLandingPad.glb", "rb") gltf_byte_array = file1.read() file1.close() - retval = world.spawn_object_from_file( + retval = scene_world.spawn_object_from_file( object_name, "gltf", gltf_byte_array, @@ -925,25 +956,24 @@ def test_spawn_object_from_file(client): enable_physics, ) print("Spawned static mesh object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) > 0 # Check destroying object that was just spawned - status = world.destroy_object(object_name) + status = scene_world.destroy_object(object_name) assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) except NNGException as err: raise Exception(str(err)) -def test_spawn_object_from_file_at_geo(client): +def test_spawn_object_from_file_at_geo(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) # ------------------------------------------------------------------ # Check spawning a static mesh object with its physics disabled - object_name: str = "TestLandingPad" + object_name: str = "TempLandingPadFileGeo" asset_name: str = "BasicLandingPad" # Existing asset lat = 47.641468 @@ -956,7 +986,7 @@ def test_spawn_object_from_file_at_geo(client): file1 = open("assets/BasicLandingPad.glb", "rb") gltf_byte_array = file1.read() file1.close() - retval = world.spawn_object_from_file_at_geo( + retval = scene_world.spawn_object_from_file_at_geo( object_name, "gltf", gltf_byte_array, @@ -969,290 +999,314 @@ def test_spawn_object_from_file_at_geo(client): enable_physics, ) print("Spawned static mesh object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) > 0 # Check destroying object that was just spawned - status = world.destroy_object(object_name) + status = scene_world.destroy_object(object_name) assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) except NNGException as err: raise Exception(str(err)) -def test_destroy_all_spawned_objects(client): +def test_destroy_all_spawned_objects(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - # ------------------------------------------------------------------ # Check spawning a static mesh object with its physics disabled - object_name: str = "TestLandingPad" + object_name: str = "TempLandingPadBulk" asset_name: str = "BasicLandingPad" # Existing asset - ref_frame = "DEFAULT_ID" for i in range(10): - translation = Vector3({"x": 20 + i, "y": 1, "z": -5}) - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) - pose: Pose = Pose( - { - "translation": translation, - "rotation": rotation, - "frame_id": ref_frame, - } - ) + pose = _make_pose(20 + i, 1, -5) scale = [10.0, 10.0, 10.0] enable_physics: bool = False - retval = world.spawn_object( + retval = scene_world.spawn_object( object_name, asset_name, pose, scale, enable_physics ) print("Spawned static mesh object successfully with name: ", retval) - new_objs = world.list_objects(object_name + ".*") + new_objs = scene_world.list_objects(object_name + ".*") assert len(new_objs) >= 10 # Check destroying object that was just spawned - status = world.destroy_all_spawned_objects() + status = scene_world.destroy_all_spawned_objects() assert status - objects = world.list_objects(object_name + ".*") + objects = scene_world.list_objects(object_name + ".*") assert not len(objects) except NNGException as err: raise Exception(str(err)) -def test_enable_disable_weather_visual_effects(client): +def test_enable_disable_weather_visual_effects(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - status = world.enable_weather_visual_effects() + status = scene_world.enable_weather_visual_effects() assert status is True - status = world.disable_weather_visual_effects() + status = scene_world.disable_weather_visual_effects() assert status is True except NNGException as err: raise Exception(str(err)) -def test_set_weather_visual_effects_param(client): +def test_set_weather_visual_effects_param(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - status = world.enable_weather_visual_effects() + status = scene_world.enable_weather_visual_effects() assert status is True - status = world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) + status = scene_world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) assert status is True - status = world.disable_weather_visual_effects() + status = scene_world.disable_weather_visual_effects() assert status is True except NNGException as err: raise Exception(str(err)) -def test_reset_weather_visual_effects(client): +def test_reset_weather_visual_effects(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - status = world.enable_weather_visual_effects() + status = scene_world.enable_weather_visual_effects() assert status is True - status = world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) + status = scene_world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) assert status is True - status = world.reset_weather_effects() + status = scene_world.reset_weather_effects() assert status is True - status = world.disable_weather_visual_effects() + status = scene_world.disable_weather_visual_effects() assert status is True except NNGException as err: raise Exception(str(err)) -def test_get_weather_visual_effects(client): +def test_get_weather_visual_effects(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - status = world.enable_weather_visual_effects() + status = scene_world.enable_weather_visual_effects() assert status is True - status = world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) + status = scene_world.set_weather_visual_effects_param(WeatherParameter.RAIN, 0.9) assert status is True - weather_dict = world.get_weather_visual_effects_param() + weather_dict = scene_world.get_weather_visual_effects_param() assert weather_dict[WeatherParameter.RAIN] == pytest.approx(0.9) - status = world.set_weather_visual_effects_param(WeatherParameter.SNOW, 0.4) + status = scene_world.set_weather_visual_effects_param(WeatherParameter.SNOW, 0.4) assert status is True - weather_dict = world.get_weather_visual_effects_param() + weather_dict = scene_world.get_weather_visual_effects_param() assert weather_dict[WeatherParameter.RAIN] == pytest.approx(0.9) assert weather_dict[WeatherParameter.SNOW] == pytest.approx(0.4) - status = world.reset_weather_effects() + status = scene_world.reset_weather_effects() assert status is True - weather_dict = world.get_weather_visual_effects_param() + weather_dict = scene_world.get_weather_visual_effects_param() assert weather_dict[WeatherParameter.RAIN] == pytest.approx(0) assert weather_dict[WeatherParameter.SNOW] == pytest.approx(0) - status = world.disable_weather_visual_effects() + status = scene_world.disable_weather_visual_effects() assert status is True except NNGException as err: raise Exception(str(err)) -def test_wind_velocity(client): +def test_wind_velocity(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - world.set_wind_velocity(5.0, 5.0, 5.0) + scene_world.set_wind_velocity(5.0, 5.0, 5.0) - assert world.get_wind_velocity() == pytest.approx((5.0, 5.0, 5.0)) + assert scene_world.get_wind_velocity() == pytest.approx((5.0, 5.0, 5.0)) except NNGException as err: raise Exception(str(err)) -def test_set_object_material(client): +def test_set_object_material(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_name = "OrangeBall" - material_path = "/ProjectAirSim/Weather/WeatherFX/Materials/M_Leaf_master" - status = world.set_object_material(object_name, material_path) - assert status is True - - except NNGException as err: - raise Exception(str(err)) + object_name = "TempMaterialBall" + asset_name = "OrangeBall_Blueprint" + pose = _make_pose(20, 0, -5) + scale = [1.0, 1.0, 1.0] + scene_world.spawn_object(object_name, asset_name, pose, scale, False) -def test_set_object_texture_from_url(client): - try: - world = World(client, "scene_test_drone.jsonc", 0) + mean0 = _capture_object_mean(scene_drone, scene_world, object_name) - url = "https://www.jpl.nasa.gov/spaceimages/images/largesize/PIA07782_hires.jpg" - object_name = "OrangeBall" - status = world.set_object_texture_from_url(object_name, url) + material_path = "/ProjectAirSim/Weather/WeatherFX/Materials/M_Leaf_master" + status = scene_world.set_object_material(object_name, material_path) assert status is True + time.sleep(1.0) + mean1 = _capture_object_mean(scene_drone, scene_world, object_name) + + assert float(np.linalg.norm(mean1 - mean0)) >= 0.05 + scene_world.destroy_object(object_name) + except NNGException as err: raise Exception(str(err)) -def test_set_object_texture_from_file(client): +def test_set_object_texture_from_file(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_name = "OrangeBall" + object_name = "TempTextureBall" + asset_name = "OrangeBall_Blueprint" texture_path = "assets/sample_texture.png" - status = world.set_object_texture_from_file(object_name, texture_path) + pose = _make_pose(20, 0, -5) + scale = [1.0, 1.0, 1.0] + + scene_world.spawn_object(object_name, asset_name, pose, scale, False) + + mean0 = _capture_object_mean(scene_drone, scene_world, object_name) + + status = scene_world.set_object_texture_from_file(object_name, texture_path) assert status is True + time.sleep(1.0) + mean1 = _capture_object_mean(scene_drone, scene_world, object_name) + + assert float(np.linalg.norm(mean1 - mean0)) >= 0.05 + scene_world.destroy_object(object_name) + except NNGException as err: raise Exception(str(err)) -def test_set_object_texture_from_packaged_asset(client): +def test_set_object_texture_from_packaged_asset(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_name = "Cone" + object_name = "TempTextureCone" + asset_name = "1M_Cube" texture_path = "/Game/Geometry/Textures/T_Default_Material_Grid_M" - status = world.set_object_texture_from_packaged_asset(object_name, texture_path) + pose = _make_pose(20, 0, -5) + scale = [3.0, 3.0, 3.0] + + scene_world.spawn_object(object_name, asset_name, pose, scale, False) + + mean0 = _capture_object_mean(scene_drone, scene_world, object_name) + + status = scene_world.set_object_texture_from_packaged_asset(object_name, texture_path) assert status is True + time.sleep(1.0) + mean1 = _capture_object_mean(scene_drone, scene_world, object_name) + + assert float(np.linalg.norm(mean1 - mean0)) >= 0.03 + scene_world.destroy_object(object_name) + except NNGException as err: raise Exception(str(err)) -def test_swap_object_texture(client): +def test_swap_object_texture(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) - - # Note: This API searches for the actor tag instead of the actor's name. The - # actor tag is set in Editor's actor details under the "Actor" section, not the - # component tags in the "Tags" section object_actor_tag = "ball" - swapped_objects = world.swap_object_texture(object_actor_tag, 1) + object_name = "TempSwapBall" + asset_name = "OrangeBall_Blueprint" + pose = _make_pose(20, 0, -5) + scale = [1.0, 1.0, 1.0] + + scene_world.spawn_object(object_name, asset_name, pose, scale, False) + + mean0 = _capture_object_mean(scene_drone, scene_world, object_name) + + swapped_objects = scene_world.swap_object_texture(object_actor_tag, 1) assert len(swapped_objects) > 0 - swapped_objects = world.swap_object_texture(object_actor_tag, 0) + time.sleep(1.0) + mean1 = _capture_object_mean(scene_drone, scene_world, object_name) + + swapped_objects = scene_world.swap_object_texture(object_actor_tag, 0) assert len(swapped_objects) > 0 + time.sleep(1.0) + mean2 = _capture_object_mean(scene_drone, scene_world, object_name) + + assert float(np.linalg.norm(mean1 - mean0)) >= 0.03 or float( + np.linalg.norm(mean2 - mean1) + ) >= 0.03 + scene_world.destroy_object(object_name) + except NNGException as err: raise Exception(str(err)) -def test_set_light_object_intensity(client): +def test_set_light_object_intensity(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - spotlight_asset_path = "SpotLightActor" - ref_frame = "DEFAULT_ID" - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) scale = [1.0, 1.0, 1.0] enable_physics = False - pad2_name = "SpotLight1" - translation = Vector3({"x": 0.0, "y": 8.0, "z": -5.0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} + spot_light_object_name = "TempSpotLightIntensity" + pose = _make_pose(0.0, 8.0, -5.0) + scene_world.spawn_object( + spot_light_object_name, spotlight_asset_path, pose, scale, enable_physics ) - spot_light_object_name = world.spawn_object(pad2_name, spotlight_asset_path, pose, scale, enable_physics) - status = world.set_light_object_intensity(spot_light_object_name, 10000.0) + pose_before = scene_world.get_object_pose(spot_light_object_name) + status = scene_world.set_light_object_intensity(spot_light_object_name, 10000.0) assert status is True + pose_after = scene_world.get_object_pose(spot_light_object_name) + assert pose_after.translation.to_list() == pytest.approx( + pose_before.translation.to_list(), abs=1e-3 + ) + scene_world.destroy_object(spot_light_object_name) except NNGException as err: raise Exception(str(err)) -def test_set_light_object_color(client): +def test_set_light_object_color(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - spotlight_asset_path = "SpotLightActor" - ref_frame = "DEFAULT_ID" - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) scale = [1.0, 1.0, 1.0] enable_physics = False - pad2_name = "SpotLight1" - translation = Vector3({"x": 0.0, "y": 8.0, "z": -5.0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} + spot_light_object_name = "TempSpotLightColor" + pose = _make_pose(0.0, 8.0, -5.0) + scene_world.spawn_object( + spot_light_object_name, spotlight_asset_path, pose, scale, enable_physics ) - spot_light_object_name = world.spawn_object(pad2_name, spotlight_asset_path, pose, scale, enable_physics) + pose_before = scene_world.get_object_pose(spot_light_object_name) color_rgb = [1.0, 0.0, 0.0] - status = world.set_light_object_color(spot_light_object_name, color_rgb) + status = scene_world.set_light_object_color(spot_light_object_name, color_rgb) assert status is True + pose_after = scene_world.get_object_pose(spot_light_object_name) + assert pose_after.translation.to_list() == pytest.approx( + pose_before.translation.to_list(), abs=1e-3 + ) + scene_world.destroy_object(spot_light_object_name) except NNGException as err: raise Exception(str(err)) -def test_set_light_object_radius(client): +def test_set_light_object_radius(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - spot_light_asset_path = "SpotLightActor" - ref_frame = "DEFAULT_ID" - rotation = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) scale = [1.0, 1.0, 1.0] enable_physics = False - pad2_name = "SpotLight1" - translation = Vector3({"x": 0.0, "y": 8.0, "z": -5.0}) - pose: Pose = Pose( - {"translation": translation, "rotation": rotation, "frame_id": ref_frame} + spot_light_object_name = "TempSpotLightRadius" + pose = _make_pose(0.0, 8.0, -5.0) + scene_world.spawn_object( + spot_light_object_name, spot_light_asset_path, pose, scale, enable_physics ) - spot_light_object_name = world.spawn_object(pad2_name, spot_light_asset_path, pose, scale, enable_physics) - status = world.set_light_object_radius(spot_light_object_name, 16000.0) + pose_before = scene_world.get_object_pose(spot_light_object_name) + status = scene_world.set_light_object_radius(spot_light_object_name, 16000.0) assert status is True + pose_after = scene_world.get_object_pose(spot_light_object_name) + assert pose_after.translation.to_list() == pytest.approx( + pose_before.translation.to_list(), abs=1e-3 + ) directional_light_asset_path = "DirectionalLightActor" - directional_light_object_name = world.spawn_object(pad2_name, directional_light_asset_path, pose, scale, enable_physics) + directional_light_object_name = scene_world.spawn_object( + "TempDirLightRadius", directional_light_asset_path, pose, scale, enable_physics + ) - status = world.set_light_object_radius(directional_light_object_name, 16000.0) + status = scene_world.set_light_object_radius(directional_light_object_name, 16000.0) assert status is False + scene_world.destroy_object(spot_light_object_name) + scene_world.destroy_object(directional_light_object_name) except NNGException as err: raise Exception(str(err)) -def test_set_time_of_day(client): +def test_set_time_of_day(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - enabled = True now_datetime = "2020-01-01 12:00:00" move_sun = True is_start_datetime_dst = True - status = world.set_time_of_day( + status = scene_world.set_time_of_day( enabled, now_datetime, is_start_datetime_dst, 1.0, 1.0, move_sun ) assert status is True @@ -1261,98 +1315,88 @@ def test_set_time_of_day(client): raise Exception(str(err)) -def test_get_time_of_day(client): +def test_get_time_of_day(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - enabled = True now_datetime = "2020-01-01 12:00:00" move_sun = True is_start_datetime_dst = True - status = world.set_time_of_day( + status = scene_world.set_time_of_day( enabled, now_datetime, is_start_datetime_dst, 1.0, 1.0, move_sun ) assert status is True - time_str = world.get_time_of_day() + time_str = scene_world.get_time_of_day() assert time_str == now_datetime now_datetime = "2020-01-01 09:00:00" - status = world.set_time_of_day( + status = scene_world.set_time_of_day( enabled, now_datetime, is_start_datetime_dst, 1.0, 1.0, move_sun ) assert status is True - time_str = world.get_time_of_day() + time_str = scene_world.get_time_of_day() assert time_str == now_datetime except NNGException as err: raise Exception(str(err)) -def test_set_sun_position_from_datetime(client): +def test_set_sun_position_from_datetime(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - # Stop the time of day feature status = False cmd_datetime = "2020-01-01 21:00:00" move_sun = False is_start_datetime_dst = True - status = world.set_time_of_day( + status = scene_world.set_time_of_day( status, cmd_datetime, is_start_datetime_dst, 1.0, 1.0, move_sun ) # Try setting to current sun position now = datetime.now() - status = world.set_sun_position_from_date_time(now, False) + status = scene_world.set_sun_position_from_date_time(now, False) assert status is True # Set back to daytime daytime = datetime(now.year, now.month, now.day, 12, 0, 0) - status = world.set_sun_position_from_date_time(daytime, False) + status = scene_world.set_sun_position_from_date_time(daytime, False) assert status is True except NNGException as err: raise Exception(str(err)) -def test_set_and_get_sun_intensity(client): +def test_set_and_get_sun_intensity(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - val = 2.5 - status = world.set_sunlight_intensity(val) + status = scene_world.set_sunlight_intensity(val) assert status is True - get_val = world.get_sunlight_intensity() + get_val = scene_world.get_sunlight_intensity() assert get_val == val except NNGException as err: raise Exception(str(err)) -def test_set_and_get_cloud_shadow_strength(client): +def test_set_and_get_cloud_shadow_strength(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - val = 0.5 - status = world.set_cloud_shadow_strength(val) + status = scene_world.set_cloud_shadow_strength(val) assert status is True - get_val = world.get_cloud_shadow_strength() + get_val = scene_world.get_cloud_shadow_strength() assert get_val == val except NNGException as err: raise Exception(str(err)) -def test_failure_response_handling(client): +def test_failure_response_handling(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - bad_sim_get_obj_pose_req: Dict = { - "method": f"{world.parent_topic}/get_object_pose", + "method": f"{scene_world.parent_topic}/get_object_pose", # Intentionally leave out the required `object_name` parameter "params": {}, "version": 1.0, @@ -1367,44 +1411,50 @@ def test_failure_response_handling(client): raise Exception(str(err)) -def test_get_set_segmentation_id(client): +def test_get_set_segmentation_id(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - - seg_id_orig = world.get_segmentation_id_by_name("templatecube_rounded_1", True) + seg_id_orig = scene_world.get_segmentation_id_by_name("templatecube_rounded_1", True) assert seg_id_orig != -1 seg_id_new = (seg_id_orig + 1) % 256 - status = world.set_segmentation_id_by_name( + status = scene_world.set_segmentation_id_by_name( "templatecube_rounded.*", seg_id_new, True, True ) assert status is True - seg_id_resp = world.get_segmentation_id_by_name("templatecube_rounded_1", True) + seg_id_resp = scene_world.get_segmentation_id_by_name("templatecube_rounded_1", True) assert seg_id_resp == seg_id_new # TODO Add tests for each variation of parameters - seg_id_map = world.get_segmentation_id_map() + seg_id_map = scene_world.get_segmentation_id_map() assert seg_id_map["TemplateCube_Rounded_1"] == seg_id_new except NNGException as err: raise Exception(str(err)) -def test_switch_streaming_view(client): +def test_switch_streaming_view(client, scene_world, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 0) + drone = reset_scene_drone + + img0 = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + size0 = (int(img0["width"]), int(img0["height"])) + assert size0[0] > 0 and size0[1] > 0 - assert world.switch_streaming_view() is True + assert scene_world.switch_streaming_view() is True + img1 = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + assert (int(img1["width"]), int(img1["height"])) == size0 + + assert scene_world.switch_streaming_view() is True + img2 = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + assert (int(img2["width"]), int(img2["height"])) == size0 except NNGException as err: raise Exception(str(err)) -def test_plot_debug_markers(client): +def test_plot_debug_markers(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - list = [1, 2, 3, 4, 5, 6] # ---------------------------------------------------------------------------------- points = [[x, y, -5] for x, y in zip(list, list)] @@ -1412,7 +1462,7 @@ def test_plot_debug_markers(client): size = 10 duration = 10 is_persistent = True - status = world.plot_debug_points( + status = scene_world.plot_debug_points( points, color_rgba, size, duration, is_persistent ) assert status is True @@ -1423,7 +1473,7 @@ def test_plot_debug_markers(client): thickness = 3 size = 15 is_persistent = False - status = world.plot_debug_arrows( + status = scene_world.plot_debug_arrows( points_start, points_end, color_rgba, @@ -1437,14 +1487,14 @@ def test_plot_debug_markers(client): points = [[x, y, -5] for x, y in zip(list, list)] color_rgba = [1.0, 0.0, 0.0, 1.0] thickness = 5 - status = world.plot_debug_solid_line( + status = scene_world.plot_debug_solid_line( points, color_rgba, thickness, duration, is_persistent ) assert status is True points = [[x, y, -7] for x, y in zip(list, list)] color_rgba = [0.0, 1.0, 0.0, 1.0] - status = world.plot_debug_dashed_line( + status = scene_world.plot_debug_dashed_line( points, color_rgba, thickness, duration, is_persistent ) assert status is True @@ -1453,7 +1503,7 @@ def test_plot_debug_markers(client): strings = ["Microsoft AirSim" for i in range(len(positions))] scale = 1 color_rgba = [1.0, 1.0, 1.0, 1.0] - status = world.plot_debug_strings( + status = scene_world.plot_debug_strings( strings, positions, scale, color_rgba, duration ) assert status is True @@ -1475,31 +1525,39 @@ def test_plot_debug_markers(client): ) ) scale = 35 - status = world.plot_debug_transforms( + status = scene_world.plot_debug_transforms( poses, scale, thickness, duration, is_persistent ) assert status is True names = ["yaw = " + str(round(yaw, 1)) for yaw in list] text_scale = 1 - status = world.plot_debug_transforms_with_names( + status = scene_world.plot_debug_transforms_with_names( poses, names, scale, thickness, text_scale, color_rgba, duration ) assert status is True - status = world.flush_persistent_markers() + status = scene_world.flush_persistent_markers() + assert status is True + status = scene_world.flush_persistent_markers() + assert status is True + + status = scene_world.plot_debug_points( + points, color_rgba, size, duration, is_persistent=False + ) + assert status is True + status = scene_world.flush_persistent_markers() assert status is True except NNGException as err: raise Exception(str(err)) -def test_trace_line(client): +def test_trace_line(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) + assert scene_world.toggle_trace() is True + assert scene_world.toggle_trace() is True - assert world.toggle_trace() is True - - assert world.set_trace_line([0.0, 0.0, 1.0, 1.0], 5.0) is True + assert scene_world.set_trace_line([0.0, 0.0, 1.0, 1.0], 5.0) is True except NNGException as err: raise Exception(str(err)) @@ -1519,12 +1577,10 @@ def test_segmentation_id_to_color(client): raise Exception(str(err)) -def test_get_kinematics(client): +def test_get_kinematics(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - kin = drone.get_ground_truth_kinematics() + kin = scene_drone.get_ground_truth_kinematics() + pose = scene_drone.get_ground_truth_pose() assert "time_stamp" in kin @@ -1532,6 +1588,17 @@ def test_get_kinematics(client): assert "position" in kin["pose"] assert "orientation" in kin["pose"] + for axis in ("x", "y", "z"): + assert kin["pose"]["position"][axis] == pytest.approx( + pose["translation"][axis], abs=1e-3 + ) + qk, qp = kin["pose"]["orientation"], pose["rotation"] + for axis in ("w", "x", "y", "z"): + assert qk[axis] == pytest.approx(qp[axis], abs=1e-3) + q = kin["pose"]["orientation"] + qn = math.sqrt(q["w"] ** 2 + q["x"] ** 2 + q["y"] ** 2 + q["z"] ** 2) + assert qn == pytest.approx(1.0, abs=1e-2) + assert "twist" in kin assert "linear" in kin["twist"] assert "angular" in kin["twist"] @@ -1539,14 +1606,19 @@ def test_get_kinematics(client): assert "accels" in kin assert "linear" in kin["accels"] assert "angular" in kin["accels"] + + lin = kin["twist"]["linear"] + ang = kin["twist"]["angular"] + for axis in ("x", "y", "z"): + assert abs(lin[axis]) < 50.0 + assert abs(ang[axis]) < 50.0 except NNGException as err: raise Exception(str(err)) -def test_set_kinematics(client): +def test_set_kinematics(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone kin = drone.get_ground_truth_kinematics() kin["pose"]["position"]["x"] = 123.4 @@ -1558,65 +1630,61 @@ def test_set_kinematics(client): raise Exception(str(err)) -def test_get_ground_truth_geo_location(client): +def test_get_ground_truth_geo_location(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - loc = drone.get_ground_truth_geo_location() + loc = scene_drone.get_ground_truth_geo_location() assert "latitude" in loc assert "longitude" in loc assert "altitude" in loc + home = scene_drone.home_geo_point + assert loc["latitude"] == pytest.approx(home["latitude"], abs=0.002) + assert loc["longitude"] == pytest.approx(home["longitude"], abs=0.002) + assert loc["altitude"] == pytest.approx(home["altitude"] + 4.0, abs=2.0) except NNGException as err: raise Exception(str(err)) -def test_can_arm(client): +def test_can_arm(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - can_arm = drone.can_arm() + can_arm = scene_drone.can_arm() assert can_arm is True except NNGException as err: raise Exception(str(err)) -def test_get_estimated_geo_location(client): +def test_get_estimated_geo_location(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - loc = drone.get_estimated_geo_location() + loc = scene_drone.get_estimated_geo_location() + loc_gt = scene_drone.get_ground_truth_geo_location() assert "latitude" in loc assert "longitude" in loc assert "altitude" in loc + assert loc["latitude"] == pytest.approx(loc_gt["latitude"], abs=0.01) + assert loc["longitude"] == pytest.approx(loc_gt["longitude"], abs=0.01) + assert loc["altitude"] == pytest.approx(loc_gt["altitude"], abs=5.0) except NNGException as err: raise Exception(str(err)) -def test_get_ready_state(client): +def test_get_ready_state(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - state = drone.get_ready_state() + state = scene_drone.get_ready_state() assert "ready_val" in state assert "ready_message" in state + assert isinstance(state["ready_val"], (int, float)) + assert isinstance(state["ready_message"], str) except NNGException as err: raise Exception(str(err)) -def test_get_estimated_kinematics(client): +def test_get_estimated_kinematics(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - kin = drone.get_estimated_kinematics() + kin = scene_drone.get_estimated_kinematics() + kin_gt = scene_drone.get_ground_truth_kinematics() assert "time_stamp" in kin @@ -1624,6 +1692,14 @@ def test_get_estimated_kinematics(client): assert "position" in kin["pose"] assert "orientation" in kin["pose"] + for axis in ("x", "y", "z"): + assert kin["pose"]["position"][axis] == pytest.approx( + kin_gt["pose"]["position"][axis], abs=5.0 + ) + q = kin["pose"]["orientation"] + qn = math.sqrt(q["w"] ** 2 + q["x"] ** 2 + q["y"] ** 2 + q["z"] ** 2) + assert qn == pytest.approx(1.0, abs=1e-2) + assert "twist" in kin assert "linear" in kin["twist"] assert "angular" in kin["twist"] @@ -1635,12 +1711,9 @@ def test_get_estimated_kinematics(client): raise Exception(str(err)) -def test_landed_state(client): +def test_landed_state(client, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - - landed_state = drone.get_landed_state() + landed_state = scene_drone.get_landed_state() assert landed_state == LandedState.LANDED except NNGException as err: @@ -1694,10 +1767,9 @@ def test_battery_health_status(client): raise Exception(str(err)) -def test_get_set_pose(client): +def test_get_set_pose(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone trans = Vector3({"x": 1.0, "y": 2.0, "z": 3.0}) rot = Quaternion({"w": 0, "x": 0, "y": 0, "z": 0}) @@ -1725,10 +1797,9 @@ def test_get_set_pose(client): raise Exception(str(err)) -def test_get_set_geo_pose(client): +def test_get_set_geo_pose(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone # Loaded scene_test_drone.jsonc has the following home geo point to compare # with spawned object lat/lon/alt below: @@ -1763,14 +1834,11 @@ def test_get_set_geo_pose(client): raise Exception(str(err)) -def test_get_images(client): +def test_get_images(client, scene_world, scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") - # Test getting a single image type - cur_simtime = world.get_sim_time() - images = drone.get_images( + cur_simtime = scene_world.get_sim_time() + images = scene_drone.get_images( camera_id="DownCamera", image_type_ids=[ImageType.SCENE] ) @@ -1785,8 +1853,8 @@ def test_get_images(client): assert len(images[ImageType.SCENE]["data"]) > 0 # Test getting a set of image types - cur_simtime = world.get_sim_time() - images = drone.get_images( + cur_simtime = scene_world.get_sim_time() + images = scene_drone.get_images( camera_id="DownCamera", image_type_ids=[ImageType.SCENE, ImageType.DEPTH_PERSPECTIVE], ) @@ -1812,7 +1880,7 @@ def test_get_images(client): assert len(images[ImageType.DEPTH_PERSPECTIVE]["data"]) > 0 # Test getting an empty set of image types - images = drone.get_images(camera_id="DownCamera", image_type_ids=[]) + images = scene_drone.get_images(camera_id="DownCamera", image_type_ids=[]) # Check that an empty dict is returned assert not images @@ -1821,12 +1889,12 @@ def test_get_images(client): raise Exception(str(err)) -def test_set_camera_pose(client): - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") +def test_set_camera_pose(client, reset_scene_drone): + drone = reset_scene_drone + z0 = INITIAL_SCENE_DRONE_POSE["translation"]["z"] trans = Vector3({"x": 50, "y": 60, "z": 70}) - rot = Quaternion({"w": 0, "x": 0, "y": 0, "z": 0}) + rot = Quaternion({"w": 1, "x": 0, "y": 0, "z": 0}) pose = Pose( { "translation": trans, @@ -1836,26 +1904,54 @@ def test_set_camera_pose(client): ) assert drone.set_camera_pose("DownCamera", pose) is True + img = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + assert img["pos_x"] == pytest.approx(50.0, abs=3.0) + assert img["pos_y"] == pytest.approx(60.0, abs=3.0) + assert img["pos_z"] == pytest.approx(70.0, abs=abs(z0)) + +def test_set_camera_focal_length(client, scene_world, reset_scene_drone): + drone = reset_scene_drone -def test_set_camera_focal_length(client): - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + scene_world.pause() + before = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + std0 = float(np.std(unpack_image(before))) + scene_world.resume() assert drone.set_focal_length("DownCamera", ImageType.SCENE, 15.0) is True + scene_world.pause() + after = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + std1 = float(np.std(unpack_image(after))) + scene_world.resume() -def test_set_field_of_view(client): - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + assert before["width"] == after["width"] + assert before["height"] == after["height"] + assert abs(std1 - std0) > 1e-6 or std1 > 0.0 + + +def test_set_field_of_view(client, scene_world, reset_scene_drone): + drone = reset_scene_drone + + scene_world.pause() + before = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + std0 = float(np.std(unpack_image(before))) + scene_world.resume() assert drone.set_field_of_view("DownCamera", ImageType.SCENE, 1.0) is True + scene_world.pause() + after = drone.get_images("DownCamera", [ImageType.SCENE])[ImageType.SCENE] + std1 = float(np.std(unpack_image(after))) + scene_world.resume() -def test_create_voxel_grid(client): - try: - world = World(client, "scene_test_drone.jsonc", 0) + assert before["width"] == after["width"] + assert before["height"] == after["height"] + assert abs(std1 - std0) > 1e-6 or std1 > 0.0 + +def test_create_voxel_grid(client, scene_world): + try: center = (0, 0, 0) center_trans = Vector3({"x": center[0], "y": center[1], "z": center[2]}) center_rot = Quaternion({"w": 0, "x": 0, "y": 0, "z": 0}) @@ -1869,7 +1965,7 @@ def test_create_voxel_grid(client): x_size, y_size, z_size = 50, 50, 50 resolution = 1 - occupancy_grid = world.create_voxel_grid( + occupancy_grid = scene_world.create_voxel_grid( center_pos, x_size, y_size, z_size, resolution ) @@ -1882,12 +1978,10 @@ def test_create_voxel_grid(client): raise Exception(str(err)) -def test_get_bbox_3d(client): +def test_get_bbox_3d(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_id = "Cone_5" - bbox_data = world.get_3d_bounding_box(object_id, BoxAlignment.WORLD_AXIS) + bbox_data = scene_world.get_3d_bounding_box(object_id, BoxAlignment.WORLD_AXIS) print(f"bbox_data:{bbox_data}") expected_fields = [ "center", @@ -1904,12 +1998,10 @@ def test_get_bbox_3d(client): raise Exception(str(err)) -def test_get_unexisting_bbox_3d(client): +def test_get_unexisting_bbox_3d(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_id = "UnexistingObject" - bbox_data = world.get_3d_bounding_box(object_id, BoxAlignment.WORLD_AXIS) + bbox_data = scene_world.get_3d_bounding_box(object_id, BoxAlignment.WORLD_AXIS) print(f"bbox_data:{bbox_data}") assert bbox_data == {} @@ -1917,10 +2009,8 @@ def test_get_unexisting_bbox_3d(client): raise Exception(str(err)) -def test_get_bbox_3d_spawned(client): +def test_get_bbox_3d_spawned(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 0) - object_name: str = "TestCube" asset_name: str = "1M_Cube" # Existing asset ref_frame = "DEFAULT_ID" @@ -1932,16 +2022,16 @@ def test_get_bbox_3d_spawned(client): ) scale = [10.0, 10.0, 20.0] enable_physics: bool = False - world.spawn_object(object_name, asset_name, pose, scale, enable_physics) + scene_world.spawn_object(object_name, asset_name, pose, scale, enable_physics) # Get world-aligned bbox - bbox_world = world.get_3d_bounding_box(object_name, BoxAlignment.WORLD_AXIS) + bbox_world = scene_world.get_3d_bounding_box(object_name, BoxAlignment.WORLD_AXIS) print(bbox_world) assert Vector3(bbox_world["center"]) == translation assert bbox_world["quaternion"] == {"w": 1, "x": 0.0, "y": 0, "z": 0} # Get object-aligned bbox - bbox_obj = world.get_3d_bounding_box(object_name, BoxAlignment.OBJECT_ORIENTED) + bbox_obj = scene_world.get_3d_bounding_box(object_name, BoxAlignment.OBJECT_ORIENTED) print(bbox_obj) assert Vector3(bbox_obj["center"]) == translation assert bbox_obj["quaternion"] != bbox_world["quaternion"] @@ -1950,7 +2040,7 @@ def test_get_bbox_3d_spawned(client): assert abs(size_list[i] - scale[i]) < 1e-4 # cleanup - world.destroy_object(object_name) + scene_world.destroy_object(object_name) except NNGException as err: raise Exception(str(err)) @@ -2000,12 +2090,13 @@ async def request_control_async(drone): await request_control -def test_request_control_async(client): +def test_request_control_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone + assert drone.enable_api_control() is True asyncio.run(request_control_async(drone)) + assert drone.is_api_control_enabled() is True except NNGException as err: raise Exception(str(err)) @@ -2016,12 +2107,16 @@ async def set_mission_mode_async(drone): await set_mode -def test_set_mission_mode_async(client): +def test_set_mission_mode_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone + assert drone.enable_api_control() is True asyncio.run(set_mission_mode_async(drone)) + kin = drone.get_ground_truth_kinematics() + q = kin["pose"]["orientation"] + qn = math.sqrt(q["w"] ** 2 + q["x"] ** 2 + q["y"] ** 2 + q["z"] ** 2) + assert qn == pytest.approx(1.0, abs=1e-2) except NNGException as err: raise Exception(str(err)) @@ -2032,21 +2127,24 @@ async def set_vtol_mode_async(drone): await set_mode -def test_set_vtol_mode_async(client): +def test_set_vtol_mode_async(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone + assert drone.enable_api_control() is True asyncio.run(set_vtol_mode_async(drone)) + kin = drone.get_ground_truth_kinematics() + q = kin["pose"]["orientation"] + qn = math.sqrt(q["w"] ** 2 + q["x"] ** 2 + q["y"] ** 2 + q["z"] ** 2) + assert qn == pytest.approx(1.0, abs=1e-2) except NNGException as err: raise Exception(str(err)) -def test_set_external_force(client): +def test_set_external_force(client, reset_scene_drone): try: - world = World(client, "scene_test_drone.jsonc", 1) - drone = Drone(client, world, "Drone1") + drone = reset_scene_drone start_pose = drone.get_ground_truth_pose()["translation"] # drone should take off @@ -2061,14 +2159,20 @@ def test_set_external_force(client): raise Exception(str(err)) -def test_get_surface_elevation_at_point(client): +def test_get_surface_elevation_at_point(client, scene_world): try: - world = World(client, "scene_test_drone.jsonc", 1) + p0 = scene_world.get_surface_elevation_at_point(0, 0) + p1 = scene_world.get_surface_elevation_at_point(-10, 5) + p2 = scene_world.get_surface_elevation_at_point(21, -35) + p3 = scene_world.get_surface_elevation_at_point(71, -64) + + # Validate API returns finite, repeatable elevations without assuming a fixed map. + for z in (p0, p1, p2, p3): + assert math.isfinite(z) + assert -10000.0 < z < 10000.0 - assert world.get_surface_elevation_at_point(0, 0) == pytest.approx(2.8, 0.1) - assert world.get_surface_elevation_at_point(-10, 5) == pytest.approx(1.0) - assert world.get_surface_elevation_at_point(21, -35) == pytest.approx(21) - assert world.get_surface_elevation_at_point(71, -64) == pytest.approx(26) + p0_repeat = scene_world.get_surface_elevation_at_point(0, 0) + assert p0_repeat == pytest.approx(p0, abs=0.05) except NNGException as err: raise Exception(str(err)) diff --git a/client/python/projectairsim/tests/test_datacollection/test_sim_apis.py b/client/python/projectairsim/tests/test_datacollection/test_sim_apis.py index ee3480b4..20003217 100644 --- a/client/python/projectairsim/tests/test_datacollection/test_sim_apis.py +++ b/client/python/projectairsim/tests/test_datacollection/test_sim_apis.py @@ -5,6 +5,7 @@ End-to-end tests for ProjectAirSim Services, request-response APIs used by the data collection module """ import random +from datetime import datetime import pytest from projectairsim import ProjectAirSimClient, World @@ -61,6 +62,10 @@ def test_weather_api(data_generator: DataGenerator, world: World): world, weather_value, pose.weather["intensity"] ) assert success is True + params = world.get_weather_visual_effects_param() + assert params[weather_value] == pytest.approx( + pose.weather["intensity"], abs=0.05 + ) def test_time_api(data_generator: DataGenerator, world: World): @@ -71,8 +76,22 @@ def test_time_api(data_generator: DataGenerator, world: World): location = geo_locations.get(location_name) for i in random.sample(range(0, len(location.trajectory)), 3): pose: WayPoint = location.trajectory[i] + if pose.time is None: + continue success, world = set_time(world, pose.time) assert success is True + tod = world.get_time_of_day() + assert tod is not None, "get_time_of_day() returned None" + sim_dt = None + for fmt in ("%Y-%m-%d %H:%M:%S", "%Y-%m-%dT%H:%M:%S", "%Y-%m-%dT%H:%M:%S.%f"): + try: + sim_dt = datetime.strptime(tod.replace("Z", ""), fmt) + break + except ValueError: + continue + assert sim_dt is not None, f"Failed to parse time of day: {tod}" + assert sim_dt.date() == pose.time.date() + assert abs((sim_dt - pose.time).total_seconds()) < 120 def test_env_actor_spawning_correct_locations( @@ -102,6 +121,10 @@ def test_env_actor_spawning_correct_locations( location.name == "Blocks-random" or location.name == "Blocks-planned" ): # random from config/ planned from API assert env_actor is not None + pose = world.get_object_pose("TestEnvActor") + assert pose is not None + t = pose.translation.to_list() + assert all(abs(v) < 1e6 for v in t) def test_env_actor_spawning_incorrect_locations( diff --git a/client/python/projectairsim/tests/test_hello_drone.py b/client/python/projectairsim/tests/test_hello_drone.py index 81e0c6bb..80b11736 100644 --- a/client/python/projectairsim/tests/test_hello_drone.py +++ b/client/python/projectairsim/tests/test_hello_drone.py @@ -6,20 +6,145 @@ """ import asyncio +import cv2 +import os import pytest import time import numpy as np from projectairsim import Drone, ProjectAirSimClient, World +from projectairsim.types import ImageType from projectairsim.utils import projectairsim_log +from image_validation_utils import assert_rgb_scene_image_valid -def check_image(img_msg): - img_nparr = np.frombuffer(img_msg["data"], dtype="uint8") - if img_nparr.size == 0: - return - if np.sum(img_nparr) == 0: - return +# --------------------------------------------------------------------------- +# Optional reference image (PNG/JPG) for similarity regression on DownCamera RGB. +# Leave empty to skip reference correlation; set a path (or env override below) +# after capturing a baseline from this test in your Blocks environment. +# --------------------------------------------------------------------------- +HELLO_DRONE_REFERENCE_IMAGE_PATH: str = "hello_drone_ref_images" +HELLO_DRONE_IMAGE_SIMILARITY_MIN: float = 0.9 + +# Env overrides (optional): PROJECTAIRSIM_TEST_HELLO_DRONE_REF_IMAGE, +# PROJECTAIRSIM_TEST_HELLO_DRONE_SIM_MIN +HELLO_DRONE_REF_IMAGE = ( + os.environ.get("PROJECTAIRSIM_TEST_HELLO_DRONE_REF_IMAGE", "").strip() + or HELLO_DRONE_REFERENCE_IMAGE_PATH.strip() +) +HELLO_DRONE_SIM_MIN = float( + os.environ.get( + "PROJECTAIRSIM_TEST_HELLO_DRONE_SIM_MIN", str(HELLO_DRONE_IMAGE_SIMILARITY_MIN) + ) +) +CONTEXT_TO_REF_STEP = { + "initial": "initial", + "Move-Up completed": "move_up", + "Move-North1 completed": "move_north1", + "Rotate-Z-60 completed": "rotate_z_60", + "Move-North2 completed": "move_north2", + "Rotate-Z-45a completed": "rotate_z_45a", + "Move-West completed": "move_west", + "Rotate-Z-45b completed": "rotate_z_45b", +} + + +def _resolve_context_reference_path( + base_reference_path: str, + context: str, + default_extension: str, + camera_suffix: str = "", +) -> str: + base = (base_reference_path or "").strip() + if not base: + return "" + + step = CONTEXT_TO_REF_STEP.get(context) + if not step: + return base + + if os.path.isdir(base): + candidate = os.path.join( + base, + f"hello_drone_{step}{camera_suffix}_ref{default_extension}", + ) + return candidate if os.path.isfile(candidate) else base + + root, ext = os.path.splitext(base) + candidate = f"{root}_{step}{ext or default_extension}" + if os.path.isfile(candidate): + return candidate + + parent = os.path.dirname(base) or "." + canonical = os.path.join( + parent, + f"hello_drone_{step}{camera_suffix}_ref{default_extension}", + ) + if os.path.isfile(canonical): + return canonical + + return base + + +def _normalized_cross_correlation(a: np.ndarray, b: np.ndarray) -> float: + if a.size == 0 or b.size == 0: + return 0.0 + b_h, b_w = b.shape[:2] + a_rs = cv2.resize(a, (b_w, b_h), interpolation=cv2.INTER_AREA).astype(np.float64) + b_f = b.astype(np.float64) + a_rs -= float(np.mean(a_rs)) + b_f -= float(np.mean(b_f)) + denom = float(np.linalg.norm(a_rs.ravel()) * np.linalg.norm(b_f.ravel())) + if denom < 1e-9: + return 0.0 + return float(np.dot(a_rs.ravel(), b_f.ravel()) / denom) + + +async def assert_scene_camera_similarity(drone, context: str) -> None: + await asyncio.sleep(5.0) + down_snaps = drone.get_images("DownCamera", [ImageType.SCENE]) + front_snaps = drone.get_images("FrontCamera", [ImageType.SCENE]) + + down_scene = down_snaps[ImageType.SCENE] + down_scene_ref_path = _resolve_context_reference_path( + HELLO_DRONE_REF_IMAGE, + context, + ".png", + "_down_camera", + ) + assert_rgb_scene_image_valid( + down_scene, + reference_image_path=down_scene_ref_path, + min_similarity_to_reference=HELLO_DRONE_SIM_MIN, + min_gray_std=2.0, + ) + + front_scene = front_snaps[ImageType.SCENE] + front_scene_ref_path = _resolve_context_reference_path( + HELLO_DRONE_REF_IMAGE, + context, + ".png", + "_front_camera", + ) + assert_rgb_scene_image_valid( + front_scene, + reference_image_path=front_scene_ref_path, + min_similarity_to_reference=HELLO_DRONE_SIM_MIN, + min_gray_std=2.0, + ) + + +def _assert_rgb_message(img_msg, context: str) -> None: + assert_rgb_scene_image_valid( + img_msg, + reference_image_path="", + min_similarity_to_reference=HELLO_DRONE_SIM_MIN, + min_gray_std=2.0, + ) + + +def check_image_rgb(img_msg): + _assert_rgb_message(img_msg, "rgb_stream") def check_imu(imu_msg): @@ -55,7 +180,7 @@ def multirotor(): class ProjectAirSimTestObject: client = ProjectAirSimClient() client.connect() - world = World(client, "scene_test_drone.jsonc", 1) + world = World(client, "scene_test_drone_hello_drone_refs.jsonc", 1) drone = Drone(client, world, "Drone1") robot_actual_pose = None @@ -74,6 +199,7 @@ async def main(self, multirotor): print("start") drone = multirotor.drone client = multirotor.client + world = multirotor.world client.subscribe( drone.robot_info["actual_pose"], multirotor.robot_actual_pose_callback @@ -86,81 +212,96 @@ async def main(self, multirotor): pytest.fail("Timeout waiting for a pose message update") await asyncio.sleep(0.1) + required_cameras = ["DownCamera", "FrontCamera"] + missing_cameras = [cam for cam in required_cameras if cam not in drone.sensors] + if missing_cameras: + pytest.fail( + "Missing required cameras in drone config: " + f"{missing_cameras}. Ensure scene_test_drone_hello_drone_refs.jsonc is used." + ) + client.subscribe( drone.sensors["DownCamera"]["scene_camera"], - lambda _, rgb: check_image(rgb), + lambda _, rgb: check_image_rgb(rgb), ) client.subscribe( - drone.sensors["DownCamera"]["depth_camera"], - lambda _, depth: check_image(depth), + drone.sensors["FrontCamera"]["scene_camera"], + lambda _, rgb: check_image_rgb(rgb), ) client.subscribe( drone.sensors["IMU1"]["imu_kinematics"], lambda _, imu: check_imu(imu), ) + await assert_scene_camera_similarity(drone, "initial") drone.enable_api_control() drone.arm() prev_pose = multirotor.robot_actual_pose + + # Move up move_up = await drone.move_by_velocity_async( - v_north=0.0, v_east=0.0, v_down=-2.0, duration=2.0 + v_north=0.0, v_east=0.0, v_down=-2.0, duration=5.0 ) projectairsim_log().info("Move-Up invoked") await move_up projectairsim_log().info("Move-Up completed") + await assert_scene_camera_similarity(drone, "Move-Up completed") new_pose = await wait_for_pose_change(multirotor, prev_pose) assert new_pose["position"]["z"] < prev_pose["position"]["z"] prev_pose = new_pose - move_north = await drone.move_by_velocity_async( - v_north=2.0, v_east=0.0, v_down=0.0, duration=2.0 + # Move north 1 + move_north1 = await drone.move_by_velocity_async( + v_north=2.0, v_east=0.0, v_down=0.0, duration=5.0 ) - projectairsim_log().info("Move-North invoked") - await move_north - projectairsim_log().info("Move-North completed") + projectairsim_log().info("Move-North1 invoked") + await move_north1 + projectairsim_log().info("Move-North1 completed") + await assert_scene_camera_similarity(drone, "Move-North1 completed") new_pose = await wait_for_pose_change(multirotor, prev_pose) assert new_pose["position"]["x"] > prev_pose["position"]["x"] prev_pose = new_pose - move_west = await drone.move_by_velocity_async( - v_north=0.0, v_east=-2.0, v_down=0.0, duration=2.0 - ) - projectairsim_log().info("Move-West invoked") - await move_west - projectairsim_log().info("Move-West completed") - new_pose = await wait_for_pose_change(multirotor, prev_pose) - assert new_pose["position"]["y"] < prev_pose["position"]["y"] + # Rotate Z 60 degrees (rate 30 deg/s for 2s) + import math + await drone.rotate_by_yaw_rate_async(math.radians(30.0), 2.0) + projectairsim_log().info("Rotate-Z-60 completed") + await assert_scene_camera_similarity(drone, "Rotate-Z-60 completed") - prev_pose = new_pose - move_south = await drone.move_by_velocity_async( - v_north=-2.0, v_east=0.0, v_down=0.0, duration=2.0 + # Move north 2 + move_north2 = await drone.move_by_velocity_async( + v_north=2.0, v_east=0.0, v_down=0.0, duration=5.0 ) - projectairsim_log().info("Move-South invoked") - await move_south - projectairsim_log().info("Move-South completed") + projectairsim_log().info("Move-North2 invoked") + await move_north2 + projectairsim_log().info("Move-North2 completed") + await assert_scene_camera_similarity(drone, "Move-North2 completed") new_pose = await wait_for_pose_change(multirotor, prev_pose) - assert new_pose["position"]["x"] < prev_pose["position"]["x"] + assert new_pose["position"]["x"] > prev_pose["position"]["x"] prev_pose = new_pose - move_east = await drone.move_by_velocity_async( - v_north=0.0, v_east=2.0, v_down=0.0, duration=2.0 + # Rotate Z 45 degrees (a) (rate 22.5 deg/s for 2s) + await drone.rotate_by_yaw_rate_async(math.radians(22.5), 2.0) + projectairsim_log().info("Rotate-Z-45a completed") + await assert_scene_camera_similarity(drone, "Rotate-Z-45a completed") + + # Move west + move_west = await drone.move_by_velocity_async( + v_north=0.0, v_east=-2.0, v_down=0.0, duration=5.0 ) - projectairsim_log().info("Move-East invoked") - await move_east - projectairsim_log().info("Move-East completed") + projectairsim_log().info("Move-West invoked") + await move_west + projectairsim_log().info("Move-West completed") + await assert_scene_camera_similarity(drone, "Move-West completed") new_pose = await wait_for_pose_change(multirotor, prev_pose) - assert new_pose["position"]["y"] > prev_pose["position"]["y"] + assert new_pose["position"]["y"] < prev_pose["position"]["y"] prev_pose = new_pose - move_down = await drone.move_by_velocity_async( - v_north=0.0, v_east=0.0, v_down=2.0, duration=4.0 - ) - projectairsim_log().info("Move-Down invoked") - await move_down - projectairsim_log().info("Move-Down completed") - new_pose = await wait_for_pose_change(multirotor, prev_pose) - assert new_pose["position"]["z"] > prev_pose["position"]["z"] + # Rotate Z 45 degrees (b) (rate 22.5 deg/s for 2s) + await drone.rotate_by_yaw_rate_async(math.radians(22.5), 2.0) + projectairsim_log().info("Rotate-Z-45b completed") + await assert_scene_camera_similarity(drone, "Rotate-Z-45b completed") drone.disarm() drone.disable_api_control() @@ -168,4 +309,4 @@ async def main(self, multirotor): client.disconnect() def test_hello_drone(self, multirotor): - asyncio.run(self.main(multirotor)) \ No newline at end of file + asyncio.run(self.main(multirotor)) diff --git a/client/python/projectairsim/tests/px4_test_sitl.py b/client/python/projectairsim/tests/test_px4_sitl.py similarity index 81% rename from client/python/projectairsim/tests/px4_test_sitl.py rename to client/python/projectairsim/tests/test_px4_sitl.py index 4e21618c..e92dc6ff 100644 --- a/client/python/projectairsim/tests/px4_test_sitl.py +++ b/client/python/projectairsim/tests/test_px4_sitl.py @@ -43,11 +43,23 @@ def actual_pose_callback(self, _, message) -> None: class TestPX4: def test_px4_hello_drone(self, robo_fixture): """Pytest test function entry point""" - asyncio.run( - self.fly_px4_drone( - robo_fixture.client, robo_fixture.world, robo_fixture.drone + try: + asyncio.run( + self.fly_px4_drone( + robo_fixture.client, robo_fixture.world, robo_fixture.drone + ) ) - ) + except RuntimeError as e: + # PX4 SITL integration can fail for various reasons (PX4 not running, connection issues, etc.) + # Provide more context in the error message + error_msg = str(e) + if "std::exception" in error_msg or not error_msg: + pytest.fail( + "PX4 SITL test failed. Ensure PX4 SITL is running and properly configured. " + f"Original error: {error_msg if error_msg else 'Unknown C++ exception'}" + ) + else: + raise async def fly_px4_drone( self, client: ProjectAirSimClient, world: World, drone: PX4Drone diff --git a/client/python/projectairsim/tests/test_sensor_apis.py b/client/python/projectairsim/tests/test_sensor_apis.py index b0b87833..9e2a41d3 100644 --- a/client/python/projectairsim/tests/test_sensor_apis.py +++ b/client/python/projectairsim/tests/test_sensor_apis.py @@ -6,8 +6,11 @@ """ from math import radians +import asyncio +import math import time +import numpy as np import pytest import projectairsim.utils as utils @@ -45,8 +48,30 @@ def drone(client, world): @pytest.fixture(scope="module") def world(client): - world = World(client, "scene_test_drone_sensors.jsonc", 1) - return world + # Create world with retries for scene loading + # Scene loading can timeout if the simulator is busy or the scene is complex + max_retries = 2 + last_error = None + for attempt in range(max_retries): + try: + print(f"\\nAttempt {attempt + 1}/{max_retries}: Loading scene_test_drone_sensors.jsonc...") + world = World(client, "scene_test_drone_sensors.jsonc", 1) + print("Scene loaded successfully") + return world + except RuntimeError as e: + last_error = e + error_msg = str(e) + if "Timeout" in error_msg or "timeout" in error_msg.lower(): + print(f"Scene load timeout (attempt {attempt + 1}/{max_retries}): {error_msg}") + if attempt < max_retries - 1: + print("Waiting 3 seconds before retry...") + time.sleep(3.0) + continue + else: + # Non-timeout error, raise immediately + raise + # If we get here, all retries failed + pytest.skip(f"Failed to load scene after {max_retries} attempts. Last error: {last_error}") def test_sensor_timestamp_validity(drone): @@ -71,8 +96,9 @@ def test_sensor_timestamp_validity(drone): raise Exception(str(err)) -def test_get_imu_data(drone): +def test_get_imu_data(drone, world): try: + world.pause() imu_data = drone.get_imu_data("IMU1") print(f"imu_data:{imu_data}") expected_fields = [ @@ -92,6 +118,16 @@ def test_get_imu_data(drone): assert sorted(["x", "y", "z"]) == sorted( utils.decode(imu_data["linear_acceleration"].keys()) ) + + kin = drone.get_ground_truth_kinematics() + q_imu = imu_data["orientation"] + q_gt = kin["pose"]["orientation"] + r_imu = quaternion_to_rpy(q_imu["w"], q_imu["x"], q_imu["y"], q_imu["z"]) + r_gt = quaternion_to_rpy(q_gt["w"], q_gt["x"], q_gt["y"], q_gt["z"]) + for a, b in zip(r_imu, r_gt): + assert a == pytest.approx(b, abs=0.08) + world.resume() + except NNGException as err: raise Exception(str(err)) @@ -118,18 +154,29 @@ def test_get_gps_data(drone): utils.decode(gps_data["velocity"].keys()) ) + loc = drone.get_ground_truth_geo_location() + assert gps_data["latitude"] == pytest.approx(loc["latitude"], abs=1e-4) + assert gps_data["longitude"] == pytest.approx(loc["longitude"], abs=1e-4) + assert gps_data["altitude"] == pytest.approx(loc["altitude"], abs=2.0) + except NNGException as err: raise Exception(str(err)) -def test_get_barometer_data(drone): +def test_get_barometer_data(drone, world): try: + world.pause() barometer_data = drone.get_barometer_data("Barometer") print(f"barometer_data:{barometer_data}") expected_fields = ["time_stamp", "altitude", "pressure", "qnh"] actual_fields = barometer_data.keys() assert sorted(expected_fields) == sorted(actual_fields) + loc = drone.get_ground_truth_geo_location() + assert barometer_data["altitude"] == pytest.approx(loc["altitude"], abs=15.0) + assert 50000.0 < barometer_data["pressure"] < 120000.0 + world.resume() + except NNGException as err: raise Exception(str(err)) @@ -148,12 +195,16 @@ def test_get_magnetometer_data(drone): assert sorted(["x", "y", "z"]) == sorted( utils.decode(magnetometer_data["magnetic_field_body"].keys()) ) + b = utils.decode(magnetometer_data["magnetic_field_body"]) + bn = math.hypot(float(b["x"]), float(b["y"]), float(b["z"])) + assert bn > 1e-6 except NNGException as err: raise Exception(str(err)) -def test_get_airspeed_data(drone): +def test_get_airspeed_data(drone, world): try: + world.pause() airspeed_data = drone.get_airspeed_data("Airspeed") print(f"airspeed_data:{airspeed_data}") expected_fields = [ @@ -162,6 +213,8 @@ def test_get_airspeed_data(drone): ] actual_fields = airspeed_data.keys() assert sorted(expected_fields) == sorted(actual_fields) + assert abs(airspeed_data["diff_pressure"]) < 500.0 + world.resume() except NNGException as err: raise Exception(str(err)) @@ -249,3 +302,271 @@ def test_camera_look_at_object(drone, world): except NNGException as err: raise Exception(str(err)) + + +# --------------------------------------------------------------------------- +# Flight helpers +# --------------------------------------------------------------------------- + + +async def _takeoff(drone): + """Arm and take off. Call _land() when done.""" + drone.enable_api_control() + drone.arm() + await (await drone.takeoff_async()) + + +async def _land(drone): + """Land and disarm.""" + await (await drone.land_async()) + drone.disarm() + drone.disable_api_control() + + +# --------------------------------------------------------------------------- +# IMU — gravity consistency +# --------------------------------------------------------------------------- + + +async def test_imu_gravity_on_ground(drone, world): + """While stationary on the ground the accelerometer norm should equal 9.81 m/s².""" + try: + world.pause() + imu_data = drone.get_imu_data("IMU1") + a = utils.decode(imu_data["linear_acceleration"]) + ax, ay, az = float(a["x"]), float(a["y"]), float(a["z"]) + norm = math.hypot(ax, ay, az) + print(f"accel norm on ground: {norm:.4f} m/s²") + assert abs(norm - 9.81) < 0.1, f"Expected ~9.81 m/s², got {norm:.4f}" + world.resume() + except NNGException as err: + raise Exception(str(err)) + + +async def test_imu_gravity_in_air(drone): + """While hovering the accelerometer norm should still equal 9.81 m/s².""" + try: + await _takeoff(drone) + imu_data = drone.get_imu_data("IMU1") + a = utils.decode(imu_data["linear_acceleration"]) + ax, ay, az = float(a["x"]), float(a["y"]), float(a["z"]) + norm = math.hypot(ax, ay, az) + print(f"accel norm in air: {norm:.4f} m/s²") + assert abs(norm - 9.81) < 0.1, f"Expected ~9.81 m/s², got {norm:.4f}" + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# IMU — pure yaw gyro response +# --------------------------------------------------------------------------- + + +async def test_imu_pure_yaw(drone): + """During a slow 360° yaw: gyro-Z non-zero, gyro-X/Y near zero.""" + try: + await _takeoff(drone) + # 2π/10 rad/s → full 360° in 10 s + yaw_rate = 2 * math.pi / 10 + rotate_task = await drone.rotate_by_yaw_rate_async( + yaw_rate=yaw_rate, duration=10.0 + ) + # Sample mid-rotation + await asyncio.sleep(5.0) + imu_data = drone.get_imu_data("IMU1") + omega = utils.decode(imu_data["angular_velocity"]) + ox = float(omega["x"]) + oy = float(omega["y"]) + oz = float(omega["z"]) + print(f"gyro x={ox:.4f}, y={oy:.4f}, z={oz:.4f} rad/s") + assert abs(oz) > 0.1, f"Expected non-zero gyro-Z during yaw, got {oz:.4f}" + assert abs(ox) < 0.3, f"Expected gyro-X near zero, got {ox:.4f}" + assert abs(oy) < 0.3, f"Expected gyro-Y near zero, got {oy:.4f}" + await rotate_task + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# Magnetometer — 360° heading monotonic +# --------------------------------------------------------------------------- + + +async def test_mag_heading_monotonic_during_yaw(drone): + """During a full 360° yaw the decoded heading should change monotonically.""" + try: + await _takeoff(drone) + yaw_rate = 2 * math.pi / 10 # rad/s → full 360° in 10 s + rotate_task = await drone.rotate_by_yaw_rate_async( + yaw_rate=yaw_rate, duration=10.0 + ) + headings = [] + start = time.monotonic() + while time.monotonic() - start < 10.0: + mag = drone.get_magnetometer_data("Magnetometer") + b = utils.decode(mag["magnetic_field_body"]) + heading = math.atan2(float(b["x"]), float(b["y"])) + headings.append(heading) + await asyncio.sleep(0.1) + await rotate_task + unwrapped = np.unwrap(headings) + diffs = np.diff(unwrapped) + total_sweep = abs(unwrapped[-1] - unwrapped[0]) + print(f"heading total sweep: {math.degrees(total_sweep):.1f}°") + assert total_sweep >= math.radians(350), ( + f"Expected ≥350° sweep, got {math.degrees(total_sweep):.1f}°" + ) + assert np.all(diffs > 0) or np.all(diffs < 0), ( + "Heading did not change monotonically during 360° yaw" + ) + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# GPS — straight-line motion accuracy +# --------------------------------------------------------------------------- + + +async def test_gps_straight_line_motion(drone): + """Move 10 m north; GPS north-delta should match ground-truth within 2 m.""" + try: + await _takeoff(drone) + gps0 = drone.get_gps_data("GPS") + gt0 = drone.get_ground_truth_kinematics() + await ( + await drone.move_to_position_async( + north=10.0, east=0.0, down=-5.0, velocity=2.0, timeout_sec=20.0 + ) + ) + gps1 = drone.get_gps_data("GPS") + gt1 = drone.get_ground_truth_kinematics() + gps_north_delta = (gps1["latitude"] - gps0["latitude"]) * 111111.0 + gt_north_delta = gt1["pose"]["position"]["x"] - gt0["pose"]["position"]["x"] + print( + f"GPS north Δ: {gps_north_delta:.3f} m, GT north Δ: {gt_north_delta:.3f} m" + ) + assert abs(gps_north_delta - gt_north_delta) < 2.0, ( + f"GPS/GT north delta mismatch: {abs(gps_north_delta - gt_north_delta):.3f} m" + ) + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# GPS — velocity sign/magnitude consistency +# --------------------------------------------------------------------------- + + +async def test_gps_velocity_consistency(drone): + """Command 3 m/s north for 20 s; finite-diff GPS lat should match commanded velocity.""" + try: + await _takeoff(drone) + # Start 20 s northward move and sample twice 1 s apart at early-steady state + move_task = await drone.move_by_velocity_async( + v_north=3.0, v_east=0.0, v_down=0.0, duration=20.0 + ) + await asyncio.sleep(0.5) + gps_t1 = drone.get_gps_data("GPS") + await asyncio.sleep(1.0) + gps_t2 = drone.get_gps_data("GPS") + # Finite-difference northward velocity from GPS latitude + v_fd = (gps_t2["latitude"] - gps_t1["latitude"]) * 111111.0 / 1.0 + print(f"GPS finite-diff v_north: {v_fd:.3f} m/s (commanded 3.0 m/s)") + assert v_fd > 0, "Expected positive (northward) finite-diff GPS velocity" + assert abs(v_fd - 3.0) < 2.0, ( + f"GPS velocity magnitude too far from commanded: {v_fd:.3f} vs 3.0 m/s" + ) + assert gps_t2["velocity"]["x"] > 0, "GPS velocity x field should be positive" + drone.cancel_last_task() + await move_task + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# Barometer — ascend/descend altitude tracking +# --------------------------------------------------------------------------- + + +async def test_barometer_altitude_ascend_descend(drone): + """Ascend from z=-5 m to z=-20 m; barometer altitude should track GT within 5 m.""" + try: + await _takeoff(drone) + # Settle at a low hover altitude + await ( + await drone.move_to_position_async( + north=0.0, east=0.0, down=-5.0, velocity=2.0, timeout_sec=15.0 + ) + ) + baro_low = drone.get_barometer_data("Barometer") + gt_low = drone.get_ground_truth_kinematics() + # Ascend + await ( + await drone.move_to_position_async( + north=0.0, east=0.0, down=-20.0, velocity=2.0, timeout_sec=20.0 + ) + ) + baro_high = drone.get_barometer_data("Barometer") + gt_high = drone.get_ground_truth_kinematics() + # NED: z is negative when above origin → altitude gain = -(z_high) - -(z_low) + gt_alt_change = ( + -gt_high["pose"]["position"]["z"] - (-gt_low["pose"]["position"]["z"]) + ) + baro_alt_change = baro_high["altitude"] - baro_low["altitude"] + print(f"baro Δalt: {baro_alt_change:.2f} m, GT Δalt: {gt_alt_change:.2f} m") + assert baro_high["altitude"] > baro_low["altitude"], ( + "Barometer altitude did not increase after ascending" + ) + assert abs(baro_alt_change - gt_alt_change) < 5.0, ( + f"Baro/GT altitude change mismatch: {abs(baro_alt_change - gt_alt_change):.2f} m" + ) + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone) + + +# --------------------------------------------------------------------------- +# Airspeed — hover near-zero + monotonic ramp +# --------------------------------------------------------------------------- + + +async def test_airspeed_ramp(drone): + """diff_pressure near zero at hover; increases strictly monotonically with forward speed.""" + try: + await _takeoff(drone) + await asyncio.sleep(1.0) # settle hover + dp_hover = drone.get_airspeed_data("Airspeed")["diff_pressure"] + print(f"diff_pressure at hover: {dp_hover:.4f} Pa") + assert abs(dp_hover) < 10.0, ( + f"Expected near-zero diff_pressure at hover, got {dp_hover:.4f} Pa" + ) + dp_values = [] + for v_fwd in [2.0, 4.0, 6.0]: + await ( + await drone.move_by_velocity_body_frame_async( + v_forward=v_fwd, v_right=0.0, v_down=0.0, duration=1.0 + ) + ) + dp = drone.get_airspeed_data("Airspeed")["diff_pressure"] + print(f"diff_pressure at {v_fwd} m/s: {dp:.4f} Pa") + dp_values.append(dp) + assert dp_hover < dp_values[0] < dp_values[1] < dp_values[2], ( + f"diff_pressure not strictly monotonic: hover={dp_hover:.4f}, " + f"2m/s={dp_values[0]:.4f}, 4m/s={dp_values[1]:.4f}, 6m/s={dp_values[2]:.4f}" + ) + except NNGException as err: + raise Exception(str(err)) + finally: + await _land(drone)