From 876e0f4d7ded862202f5b021d1ba5ca58243b054 Mon Sep 17 00:00:00 2001 From: Zaffer <51871197+Zaffer@users.noreply.github.com> Date: Tue, 7 Apr 2026 00:24:41 -0400 Subject: [PATCH 1/5] add CPU usage profiling tests and multi-drone lift-land test cases --- .claude/plans/improve-resource-usage-plan.md | 89 ++++++ client/python/projectairsim/pyproject.toml | 4 + tests/e2e/test_debug_viz.py | 210 ++++++++++++ tests/test_cpu_usage.py | 316 +++++++++++++++++++ tests/test_multi_drone_lift_land.py | 185 +++++++++++ 5 files changed, 804 insertions(+) create mode 100644 .claude/plans/improve-resource-usage-plan.md create mode 100644 tests/e2e/test_debug_viz.py create mode 100644 tests/test_cpu_usage.py create mode 100644 tests/test_multi_drone_lift_land.py diff --git a/.claude/plans/improve-resource-usage-plan.md b/.claude/plans/improve-resource-usage-plan.md new file mode 100644 index 00000000..9b6b50c4 --- /dev/null +++ b/.claude/plans/improve-resource-usage-plan.md @@ -0,0 +1,89 @@ +# Fix Excessive CPU Usage — Step-API-Only Mode + +## Context + +When a Python client connects, CPU usage spikes to 103%+ even when idle. The root cause is a single Python thread spinning `recv(block=False)` in a zero-sleep loop. Server-side system CPU is only 3-6% — not a real problem. + +## Plan + +### PR1: Kill client-side recv spin +**File:** `client/python/projectairsim/src/projectairsim/client.py` + +1. Remove auto-start of `recv_topic_thread` from `connect()` (line 81-82) +2. Start it lazily on first `subscribe()` call +3. Switch recv loop from `recv(block=False)` to blocking recv with 100ms timeout +4. Keep thread non-daemon — the 100ms recv timeout guarantees bounded join on `disconnect()` + +**Note:** `get_topic_info()` calls `subscribe()` internally, so lazy start still works for topic discovery. + +**Acceptance:** Idle Python CPU < 5%. Step throughput within 5% of baseline. + +### PR2: Server topic manager — blocking recv +**File:** `core_sim/src/topic_manager.cpp` + +1. Set `NNG_OPT_RECVTIMEO` (100ms) on topic socket at startup +2. Remove `NNG_FLAG_NONBLOCK` from `nng_recv()` (line 422) +3. Remove `sleep_for(milliseconds(1))` (line 427) +4. Handle `NNG_ETIMEDOUT` same as `NNG_EAGAIN` (continue) + +**Acceptance:** Server stops waking 1000/sec while idle. + +### After PR1+PR2: Re-profile +Run full test matrix. If system CPU and step throughput look fine, stop here. + +## Files to Modify +- `client/python/projectairsim/src/projectairsim/client.py` +- `core_sim/src/topic_manager.cpp` + +## Verification +``` +uv run pytest tests/test_cpu_usage.py -v -s +``` +**Pass criteria:** +1. Idle Python CPU < 5% (from 103%) +2. No thread pinned near 100% when idle +3. Step throughput regression < 5% (baseline: 342.5 steps/sec at 3ms dt) + +--- + +## Future (only if post-fix profiling shows need) + +### Scene step yield loops → condition variable +`core_sim/src/scene.cpp` lines 695, 722, 746, 769, 789 — `while (!IsSimPaused()) { yield(); }`. Replace with CV wait + stop predicate. Requires lock-order validation against `ScheduledExecutor::mutex_` (clock.cpp:305). + +### ScheduledExecutor paused spin → 1ms sleep +`core_sim/src/clock.cpp:287-326` — loops at 333Hz when paused. Back off to 1ms sleep. Must verify < 5% throughput regression. + +### UnrealScene sleep(0) → sleep(100µs) +`UnrealScene.cpp:441-443` — replace zero-duration sleep with bounded microsecond sleep. + +--- + +## Measurements + +### Baseline (2026-04-05, pre-fix) + +#### Idle / Connected Cases + +| Case | Scenario | Py CPU | Sys CPU | Hottest Thread | +|------|----------------------------|------------|---------|----------------| +| 2 | Connected only | **103.5%** | 5.9% | 104.0% | +| 3 | Scene loaded, idle | **103.3%** | 3.7% | 103.6% | +| 9 | Idle after stepping | **103.6%** | 3.4% | 103.6% | + +#### Stepping Cases + +| Case | Scenario | Py CPU | Steps/sec | Avg Latency | +|------|---------------------------------|------------|-----------|-------------| +| 4 | Step 3ms, no images | **105.9%** | 342.5 | 2.92ms | +| 5 | Step 3ms + GetImages every step | **104.2%** | 3.2 | 314.57ms | +| 6 | Step 3ms + GetImages every 10 | **104.3%** | 31.2 | 32.06ms | +| 7 | Step 10ms, no images | **104.2%** | 85.8 | 11.66ms | +| 8 | Step 20ms, no images | **104.0%** | 48.9 | 20.44ms | + +#### Key Findings + +1. **One Python thread (recv_topic) burns an entire core** even when idle — the only real problem. +2. **System CPU is 3-6%** — server-side spins exist but aren't impactful. +3. **GetImages is 314ms/call** — separate issue, not a spin problem. +4. **Step latency scales linearly with dt** — RPC overhead is negligible. diff --git a/client/python/projectairsim/pyproject.toml b/client/python/projectairsim/pyproject.toml index 992c1928..68b7b743 100644 --- a/client/python/projectairsim/pyproject.toml +++ b/client/python/projectairsim/pyproject.toml @@ -63,6 +63,10 @@ bonsai = [ datacollection = [ "albumentations==1.3.0", ] +tests = [ + "pytest", + "psutil>=5.9.0", +] [tool.setuptools] # Source layout: packages live under src/ diff --git a/tests/e2e/test_debug_viz.py b/tests/e2e/test_debug_viz.py new file mode 100644 index 00000000..74630303 --- /dev/null +++ b/tests/e2e/test_debug_viz.py @@ -0,0 +1,210 @@ +""" +Test all debug visualization primitives. +Uses step API to fly the drone (acro mode + throttle), draws debug shapes with labels. +""" + +import os +import time + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log + +DURATION = 60.0 +SIM_ADDR = "172.23.240.1" +STEP_NS = 3_000_000 # 3ms per physics tick +SIM_CONFIG_PATH = os.path.join( + os.path.dirname(__file__), "..", "..", "client", "python", "example_user_scripts", "sim_config" +) + + +# Layout helper: NED coordinates +def pos(east_offset, z=-3.0): + return [0.0, east_offset, z] + + +def arm_drone(client, topic): + client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) + + +def set_controls(client, topic, throttle, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0): + client.request({ + "method": f"{topic}/SetAngleRatesThrottle", + "params": {"roll_rate": roll_rate, "pitch_rate": pitch_rate, + "yaw_rate": yaw_rate, "throttle": throttle}, + "version": 1.0, + }) + + +def main(): + client = ProjectAirSimClient(address=SIM_ADDR) + + try: + client.connect() + world = World(client, "scene_basic_drone.jsonc", delay_after_load_sec=2, sim_config_path=SIM_CONFIG_PATH) + + drone_topic = f"{world.parent_topic}/robots/Drone1" + + # --- Draw all debug primitives --- + projectairsim_log().info("Drawing debug visualization primitives...") + + spacing = 4.0 + z = -3.0 # 3m above ground (NED) + + label_positions = [] + label_names = [] + + def add_label(name, east): + label_positions.append([0.0, east, z]) + label_names.append(name) + + # 1) POINTS - cluster of red points + east = 0.0 + add_label("Points", east) + world.plot_debug_points( + points=[ + pos(east - 0.3, z), pos(east + 0.3, z), pos(east, z - 0.3), + pos(east, z + 0.3), pos(east - 0.2, z - 0.2), pos(east + 0.2, z + 0.2), + ], + color_rgba=[1.0, 0.0, 0.0, 1.0], + size=15.0, + duration=DURATION, + is_persistent=False, + ) + + # 2) SOLID LINE - green zigzag + east += spacing + add_label("Solid Line", east) + world.plot_debug_solid_line( + points=[ + pos(east - 1.0, z), + pos(east - 0.5, z - 0.8), + pos(east, z), + pos(east + 0.5, z - 0.8), + pos(east + 1.0, z), + ], + color_rgba=[0.0, 1.0, 0.0, 1.0], + thickness=3.0, + duration=DURATION, + is_persistent=False, + ) + + # 3) DASHED LINE - cyan segments (even number of points) + east += spacing + add_label("Dashed Line", east) + world.plot_debug_dashed_line( + points=[ + pos(east - 1.0, z), pos(east - 0.5, z), + pos(east - 0.3, z), pos(east + 0.3, z), + pos(east + 0.5, z), pos(east + 1.0, z), + ], + color_rgba=[0.0, 1.0, 1.0, 1.0], + thickness=3.0, + duration=DURATION, + is_persistent=False, + ) + + # 4) ARROWS - magenta arrows pointing in different directions + east += spacing + add_label("Arrows", east) + world.plot_debug_arrows( + points_start=[ + pos(east, z), + pos(east, z), + pos(east, z), + ], + points_end=[ + pos(east + 1.5, z), + pos(east, z - 1.5), + pos(east + 1.0, z - 1.0), + ], + color_rgba=[1.0, 0.0, 1.0, 1.0], + thickness=3.0, + arrow_size=30.0, + duration=DURATION, + is_persistent=False, + ) + + # 5) COLORED POINTS DEMO - heatmap gradient blue -> red + east += spacing + add_label("Color Gradient", east) + n = 10 + for i in range(n): + t = i / max(n - 1, 1) + world.plot_debug_points( + points=[pos(east - 1.5 + 3.0 * t, z)], + color_rgba=[t, 0.0, 1.0 - t, 1.0], + size=20.0, + duration=DURATION, + is_persistent=False, + ) + + # 6) Labels for everything + world.plot_debug_strings( + strings=label_names, + positions=label_positions, + scale=5.0, + color_rgba=[1.0, 1.0, 0.0, 1.0], + duration=DURATION, + ) + + # Big test string near origin + world.plot_debug_strings( + strings=["HELLO DEBUG"], + positions=[[0.0, 0.0, -1.0]], + scale=10.0, + color_rgba=[1.0, 1.0, 1.0, 1.0], + duration=DURATION, + ) + + projectairsim_log().info("All primitives drawn!") + + # --- Fly the drone using step API (acro mode) --- + projectairsim_log().info("Setting up trace and flying drone via step API...") + world.set_trace_line(color_rgba=[1.0, 0.5, 0.0, 1.0], thickness=4.0) + world.toggle_trace() + + arm_drone(client, drone_topic) + + # Step once to settle + world.step(STEP_NS) + + # Phase 1: Climb (~3s = 1000 steps at 3ms) + projectairsim_log().info("Climbing...") + set_controls(client, drone_topic, throttle=0.55) + for _ in range(1000): + world.step(STEP_NS) + + # Phase 2: Pitch forward (~2s) + projectairsim_log().info("Flying forward...") + set_controls(client, drone_topic, throttle=0.45, pitch_rate=0.5) + for _ in range(666): + world.step(STEP_NS) + + # Phase 3: Yaw spin (~2s) + projectairsim_log().info("Yawing...") + set_controls(client, drone_topic, throttle=0.45, yaw_rate=1.0) + for _ in range(666): + world.step(STEP_NS) + + # Phase 4: Cut throttle and descend (~3s) + projectairsim_log().info("Descending...") + set_controls(client, drone_topic, throttle=0.0) + for _ in range(1000): + world.step(STEP_NS) + + projectairsim_log().info("Flight done. Keeping connection alive for 30s...") + + # Keep alive so debug drawings stay visible + time.sleep(30) + + except Exception as err: + projectairsim_log().error(f"Exception: {err}", exc_info=True) + + finally: + client.disconnect() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/tests/test_cpu_usage.py b/tests/test_cpu_usage.py new file mode 100644 index 00000000..ced824f6 --- /dev/null +++ b/tests/test_cpu_usage.py @@ -0,0 +1,316 @@ +"""CPU usage profiling suite — baseline before busy-wait fixes. + +Reproducible workload: one scene, one drone, fixed dt, no randomization. +Each case runs for a measurement window and reports per-thread CPU. + +Cases: + 1. Sim running, no client connected (manual — observe before connect) + 2. Client connected only (no scene load) + 3. Client connected + scene loaded, no stepping + 4. Step loop only, no image calls (dt = 3ms) + 5. Step loop + GetImages every step (dt = 3ms) + 6. Step loop + GetImages every 10 steps (dt = 3ms) + 7. Step loop only (dt = 10ms) + 8. Step loop only (dt = 20ms) + 9. Idle after all stepping + +Run from: ProjectAirSim repo root +Requires: UE editor with NavGym map in Play mode. + +Usage: + uv run pytest tests/test_cpu_usage.py -v -s +""" + +import os +import threading +import time +from pathlib import Path + +import pytest + +psutil = pytest.importorskip("psutil") + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.types import ImageType + +SIM_ADDRESS = "172.23.240.1" +STEP_3MS = 3_000_000 +STEP_10MS = 10_000_000 +STEP_20MS = 20_000_000 + +NAV_JAX_SIM_CONFIG = str( + Path(__file__).resolve().parent.parent.parent + / "nav-jax" + / "sims" + / "airsim" + / "sim_config" +) +SCENE_CONFIG = "scene_gate_course_rl.jsonc" + +MEASURE_SEC = 5.0 # measurement window per case + + +# ── Helpers ────────────────────────────────────────────────────────── + + +def get_thread_cpu(proc: psutil.Process) -> list[dict]: + """Get per-thread CPU usage for a process. Returns list sorted by CPU% desc.""" + try: + threads = proc.threads() + except (psutil.NoSuchProcess, psutil.AccessDenied): + return [] + # threads() gives cumulative user+system time; we sample twice to get delta + return [ + {"tid": t.id, "user": t.user_time, "system": t.system_time} + for t in threads + ] + + +def measure_thread_cpu(proc: psutil.Process, duration: float) -> list[dict]: + """Measure per-thread CPU% over a duration. Returns top threads by CPU%.""" + before = {t["tid"]: t for t in get_thread_cpu(proc)} + time.sleep(duration) + after = {t["tid"]: t for t in get_thread_cpu(proc)} + + results = [] + for tid, a in after.items(): + b = before.get(tid) + if b is None: + continue + user_delta = a["user"] - b["user"] + sys_delta = a["system"] - b["system"] + total_pct = (user_delta + sys_delta) / duration * 100 + results.append({"tid": tid, "cpu_pct": total_pct, + "user_pct": user_delta / duration * 100, + "sys_pct": sys_delta / duration * 100}) + results.sort(key=lambda x: x["cpu_pct"], reverse=True) + return results + + +def measure_case(duration: float) -> dict: + """Measure process + system + per-thread CPU over a time window.""" + proc = psutil.Process(os.getpid()) + proc.cpu_percent() + psutil.cpu_percent(percpu=True) + + thread_cpu = measure_thread_cpu(proc, duration) + + return { + "process_cpu_pct": proc.cpu_percent(), + "system_cpu_pct": psutil.cpu_percent(), + "per_core_pct": psutil.cpu_percent(percpu=True), + "threads": thread_cpu, + "num_python_threads": threading.active_count(), + } + + +def run_step_loop(world, dt_ns, duration_sec, get_images_fn=None, + image_every_n=0): + """Run steps for a fixed duration. Returns step count and elapsed time.""" + proc = psutil.Process(os.getpid()) + proc.cpu_percent() + psutil.cpu_percent(percpu=True) + + t0 = time.perf_counter() + n_steps = 0 + + # Run for at least duration_sec + while time.perf_counter() - t0 < duration_sec: + world.step(dt_ns) + n_steps += 1 + if get_images_fn and image_every_n > 0 and n_steps % image_every_n == 0: + get_images_fn() + + elapsed = time.perf_counter() - t0 + thread_cpu = measure_thread_cpu(proc, 0.01) # snapshot + + return { + "process_cpu_pct": proc.cpu_percent(), + "system_cpu_pct": psutil.cpu_percent(), + "per_core_pct": psutil.cpu_percent(percpu=True), + "threads": thread_cpu, + "num_python_threads": threading.active_count(), + "steps": n_steps, + "elapsed_sec": elapsed, + "steps_per_sec": n_steps / elapsed, + "avg_step_ms": elapsed / n_steps * 1000, + } + + +def print_report(label: str, stats: dict): + """Pretty-print a case report.""" + print(f"\n{'='*70}") + print(f" {label}") + print(f"{'='*70}") + print(f" Python process CPU: {stats['process_cpu_pct']:.1f}%") + print(f" System CPU (total): {stats['system_cpu_pct']:.1f}%") + print(f" Python threads: {stats['num_python_threads']}") + + # Hot cores + cores = stats["per_core_pct"] + hot = [(i, p) for i, p in enumerate(cores) if p > 50] + if hot: + print(f" Hot cores (>50%): {len(hot)}") + for i, p in hot[:5]: + print(f" Core {i:2d}: {p:.0f}%") + + # Top threads by CPU + threads = stats.get("threads", []) + top = [t for t in threads if t["cpu_pct"] > 1.0] + if top: + print(f" Top threads by CPU:") + for t in top[:5]: + print(f" TID {t['tid']:>6}: {t['cpu_pct']:5.1f}% " + f"(user {t['user_pct']:.1f}%, sys {t['sys_pct']:.1f}%)") + + # Step metrics if present + if "steps" in stats: + print(f" Steps: {stats['steps']}") + print(f" Elapsed: {stats['elapsed_sec']:.2f}s") + print(f" Steps/sec: {stats['steps_per_sec']:.1f}") + print(f" Avg step latency: {stats['avg_step_ms']:.2f}ms") + print() + + +# ── Fixtures ───────────────────────────────────────────────────────── + + +@pytest.fixture(scope="module") +def client(): + c = ProjectAirSimClient(address=SIM_ADDRESS) + c.connect() + yield c + c.disconnect() + + +@pytest.fixture(scope="module") +def world(client): + w = World( + client, + SCENE_CONFIG, + sim_config_path=NAV_JAX_SIM_CONFIG, + delay_after_load_sec=2, + ) + return w + + +@pytest.fixture(scope="module") +def drone(client, world): + """Create Drone object and arm for step API.""" + d = Drone(client, world, "Drone1") + topic = f"{world.parent_topic}/robots/Drone1" + client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) + return d + + +# ── Tests ──────────────────────────────────────────────────────────── + + +ALL_RESULTS = {} + + +class TestCPUProfile: + """Systematic CPU profiling across cases.""" + + def test_case2_connected_only(self, client): + """Case 2: Client connected, no scene loaded yet.""" + time.sleep(1) + stats = measure_case(MEASURE_SEC) + print_report("CASE 2: Client connected only", stats) + ALL_RESULTS["2_connected"] = stats + + def test_case3_scene_loaded_idle(self, client, world): + """Case 3: Scene loaded, no stepping.""" + time.sleep(1) + stats = measure_case(MEASURE_SEC) + print_report("CASE 3: Scene loaded, idle (no stepping)", stats) + ALL_RESULTS["3_scene_idle"] = stats + + def test_case4_step_3ms(self, client, world, drone): + """Case 4: Step loop, dt=3ms, no images.""" + # Warm up + for _ in range(20): + world.step(STEP_3MS) + stats = run_step_loop(world, STEP_3MS, MEASURE_SEC) + print_report("CASE 4: Step only, dt=3ms", stats) + ALL_RESULTS["4_step_3ms"] = stats + + def test_case5_step_3ms_images_every(self, client, world, drone): + """Case 5: Step loop, dt=3ms, GetImages every step.""" + get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) + stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, + get_images_fn=get_img, image_every_n=1) + print_report("CASE 5: Step + GetImages every step, dt=3ms", stats) + ALL_RESULTS["5_step_3ms_img1"] = stats + + def test_case6_step_3ms_images_every10(self, client, world, drone): + """Case 6: Step loop, dt=3ms, GetImages every 10 steps.""" + get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) + stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, + get_images_fn=get_img, image_every_n=10) + print_report("CASE 6: Step + GetImages every 10 steps, dt=3ms", stats) + ALL_RESULTS["6_step_3ms_img10"] = stats + + def test_case7_step_10ms(self, client, world, drone): + """Case 7: Step loop, dt=10ms, no images.""" + stats = run_step_loop(world, STEP_10MS, MEASURE_SEC) + print_report("CASE 7: Step only, dt=10ms", stats) + ALL_RESULTS["7_step_10ms"] = stats + + def test_case8_step_20ms(self, client, world, drone): + """Case 8: Step loop, dt=20ms, no images.""" + stats = run_step_loop(world, STEP_20MS, MEASURE_SEC) + print_report("CASE 8: Step only, dt=20ms", stats) + ALL_RESULTS["8_step_20ms"] = stats + + def test_case9_idle_after_stepping(self, client, world, drone): + """Case 9: Idle after all stepping.""" + time.sleep(1) + stats = measure_case(MEASURE_SEC) + print_report("CASE 9: Idle after stepping", stats) + ALL_RESULTS["9_idle_after"] = stats + + def test_summary(self): + """Print compact summary table.""" + r = ALL_RESULTS + print(f"\n{'='*70}") + print(f" PROFILING SUMMARY") + print(f"{'='*70}") + + # Idle/connected cases + print(f"\n {'Case':<40} {'Py CPU':>8} {'Sys CPU':>9} {'Threads':>8}") + print(f" {'-'*40} {'-'*8} {'-'*9} {'-'*8}") + for key, label in [ + ("2_connected", "2. Connected only"), + ("3_scene_idle", "3. Scene loaded, idle"), + ("9_idle_after", "9. Idle after stepping"), + ]: + if key in r: + s = r[key] + print(f" {label:<40} {s['process_cpu_pct']:>6.1f}% " + f"{s['system_cpu_pct']:>7.1f}% {s['num_python_threads']:>7}") + + # Stepping cases + print(f"\n {'Case':<40} {'Py CPU':>8} {'Steps/s':>9} {'Lat ms':>8}") + print(f" {'-'*40} {'-'*8} {'-'*9} {'-'*8}") + for key, label in [ + ("4_step_3ms", "4. Step 3ms, no images"), + ("5_step_3ms_img1", "5. Step 3ms + img every step"), + ("6_step_3ms_img10", "6. Step 3ms + img every 10"), + ("7_step_10ms", "7. Step 10ms, no images"), + ("8_step_20ms", "8. Step 20ms, no images"), + ]: + if key in r: + s = r[key] + print(f" {label:<40} {s['process_cpu_pct']:>6.1f}% " + f"{s['steps_per_sec']:>8.1f} {s['avg_step_ms']:>7.2f}") + + # Top thread from worst idle case + for key in ["2_connected", "3_scene_idle"]: + if key in r and r[key]["threads"]: + top = r[key]["threads"][0] + print(f"\n Hottest thread ({key}): " + f"TID {top['tid']} at {top['cpu_pct']:.1f}%") + print() diff --git a/tests/test_multi_drone_lift_land.py b/tests/test_multi_drone_lift_land.py new file mode 100644 index 00000000..4f4b4bb9 --- /dev/null +++ b/tests/test_multi_drone_lift_land.py @@ -0,0 +1,185 @@ +"""Multi-drone lift-land test using nav-jax RL robot config (3 cameras per drone). + +Loads the multi-drone scene with the RL robot config (Chase, World, FPV cameras), +arms N drones, lifts them, verifies altitude, then lands. + +Run from: ProjectAirSim repo root +Requires: UE editor with NavGym map in Play mode. + +Usage: + uv run pytest tests/test_multi_drone_lift_land.py -v + uv run pytest tests/test_multi_drone_lift_land.py -v -k "test_lift_land_3" +""" + +import sys +import time +from pathlib import Path + +import commentjson +import pytest +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.types import ImageType +from projectairsim.types import Pose +from projectairsim.utils import load_scene_config_as_dict + +SIM_ADDRESS = "172.23.240.1" +STEP_NS = 3_000_000 # 3ms per physics tick + +# nav-jax sim_config directory (has robot + scene configs) +NAV_JAX_SIM_CONFIG = str(Path(__file__).resolve().parent.parent.parent / "nav-jax" / "sims" / "airsim" / "sim_config") +SCENE_CONFIG = "scene_gate_course_rl_multi.jsonc" + + +# ── Helpers ────────────────────────────────────────────────────────── + + +def load_scene(client, num_drones): + """Load multi-drone scene and return World + list of drone topics.""" + world = World( + client, + SCENE_CONFIG, + sim_config_path=NAV_JAX_SIM_CONFIG, + delay_after_load_sec=2, + ) + + drone_topics = [f"{world.parent_topic}/robots/Drone{i+1}" for i in range(num_drones)] + return world, drone_topics + + +def arm_drone(client, topic): + """EnableApiControl + Arm + SetAcroMode.""" + client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) + client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) + + +def set_controls(client, topic, throttle, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0): + client.request({ + "method": f"{topic}/SetAngleRatesThrottle", + "params": {"roll_rate": roll_rate, "pitch_rate": pitch_rate, + "yaw_rate": yaw_rate, "throttle": throttle}, + "version": 1.0, + }) + + +# ── Tests ──────────────────────────────────────────────────────────── + + +def _run_lift_land(num_drones): + """Core lift-land test for N drones.""" + client = ProjectAirSimClient(address=SIM_ADDRESS) + client.connect() + + try: + world, drone_topics = load_scene(client, num_drones) + drone_names = [f"Drone{i+1}" for i in range(num_drones)] + + # Arm all drones + for topic in drone_topics: + arm_drone(client, topic) + + # Step once to settle + resp = world.step(STEP_NS) + assert "drones" in resp + for name in drone_names: + assert name in resp["drones"], f"{name} not in step response" + + # Record starting z for each drone + start_z = {} + for name in drone_names: + start_z[name] = resp["drones"][name]["state"]["position"]["z"] + print(f" {name} start z={start_z[name]:.2f}") + + # Phase 1: Gentle climb for ~3s + for topic in drone_topics: + set_controls(client, topic, throttle=0.5) + + for _ in range(1000): + resp = world.step(STEP_NS) + + print("\nAfter climb:") + for name in drone_names: + z = resp["drones"][name]["state"]["position"]["z"] + print(f" {name} z={z:.2f}") + assert z < start_z[name] - 0.5, ( + f"{name} didn't climb: start={start_z[name]:.2f}, now={z:.2f}" + ) + + # Phase 2: Pitch forward for ~2s + print("\nPitching forward...") + for topic in drone_topics: + set_controls(client, topic, throttle=0.4, pitch_rate=0.5) + + for _ in range(666): + resp = world.step(STEP_NS) + + # Phase 3: Yaw spin for ~2s + print("Yawing...") + for topic in drone_topics: + set_controls(client, topic, throttle=0.4, yaw_rate=1.0) + + for _ in range(666): + resp = world.step(STEP_NS) + + # Phase 4: Roll tilt for ~2s + print("Rolling...") + for topic in drone_topics: + set_controls(client, topic, throttle=0.4, roll_rate=0.5) + + for _ in range(666): + resp = world.step(STEP_NS) + + # Phase 5: Hover for ~2s to settle + print("Hovering...") + for topic in drone_topics: + set_controls(client, topic, throttle=0.35) + + for _ in range(666): + resp = world.step(STEP_NS) + + # Phase 6: Cut throttle — drones descend for ~3s + print("Descending...") + for topic in drone_topics: + set_controls(client, topic, throttle=0.0) + + climb_z = {name: resp["drones"][name]["state"]["position"]["z"] for name in drone_names} + + for _ in range(1000): + resp = world.step(STEP_NS) + + print("\nAfter descend:") + for name in drone_names: + z = resp["drones"][name]["state"]["position"]["z"] + print(f" {name} z={z:.2f}") + assert z > climb_z[name], ( + f"{name} didn't descend: climb={climb_z[name]:.2f}, now={z:.2f}" + ) + + # Phase 3: Get images from FPV camera on each drone + print("\nFPV images:") + for i, name in enumerate(drone_names): + drone = Drone(client, world, name) + images = drone.get_images(camera_id="FPV", image_type_ids=[ImageType.SCENE]) + img = images.get(ImageType.SCENE) + assert img is not None, f"{name}: FPV image is None" + assert img["width"] == 64, f"{name}: expected 64px, got {img['width']}" + assert img["height"] == 64, f"{name}: expected 64px, got {img['height']}" + assert len(img["data"]) == 64 * 64 * 3, f"{name}: wrong data size" + print(f" {name}: FPV {img['width']}x{img['height']} OK") + + print(f"\n✓ {num_drones}-drone lift-land + FPV capture passed") + + finally: + client.disconnect() + + +def test_lift_land_1(): + _run_lift_land(1) + + +def test_lift_land_3(): + _run_lift_land(3) + + +def test_lift_land_10(): + _run_lift_land(10) From f2d4e147531fcf9a5376005db188056f84765898 Mon Sep 17 00:00:00 2001 From: Zaffer <51871197+Zaffer@users.noreply.github.com> Date: Tue, 7 Apr 2026 00:58:25 -0400 Subject: [PATCH 2/5] refactor: optimize CPU usage by eliminating non-blocking recv spin and implementing recv timeout --- .claude/plans/improve-resource-usage-plan.md | 27 +++++++++++++++++++ .../projectairsim/src/projectairsim/client.py | 16 ++++++----- core_sim/src/topic_manager.cpp | 20 ++++++++++---- .../ProjectAirSim/Private/UnrealScene.cpp | 2 +- 4 files changed, 53 insertions(+), 12 deletions(-) diff --git a/.claude/plans/improve-resource-usage-plan.md b/.claude/plans/improve-resource-usage-plan.md index 9b6b50c4..78227f55 100644 --- a/.claude/plans/improve-resource-usage-plan.md +++ b/.claude/plans/improve-resource-usage-plan.md @@ -87,3 +87,30 @@ uv run pytest tests/test_cpu_usage.py -v -s 2. **System CPU is 3-6%** — server-side spins exist but aren't impactful. 3. **GetImages is 314ms/call** — separate issue, not a spin problem. 4. **Step latency scales linearly with dt** — RPC overhead is negligible. + +### After PR1 (2026-04-07) + +#### Idle / Connected Cases + +| Case | Scenario | Py CPU | Sys CPU | Threads | Hottest Thread | +|------|----------------------------|----------|---------|---------|----------------| +| 2 | Connected only | **0.0%** | 2.8% | 1 | 0.0% | +| 3 | Scene loaded, idle | **1.4%** | 0.5% | 2 | 0.2% | +| 9 | Idle after stepping | **1.2%** | 0.5% | 2 | — | + +#### Stepping Cases + +| Case | Scenario | Py CPU | Steps/sec | Avg Latency | +|------|---------------------------------|----------|-----------|-------------| +| 4 | Step 3ms, no images | **5.0%** | 331.5 | 3.02ms | +| 5 | Step 3ms + GetImages every step | **5.5%** | 3.1 | 317.97ms | +| 6 | Step 3ms + GetImages every 10 | **5.6%** | 32.8 | 30.50ms | +| 7 | Step 10ms, no images | **2.4%** | 90.5 | 11.05ms | +| 8 | Step 20ms, no images | **2.2%** | 50.4 | 19.84ms | + +#### Results + +- **Idle CPU: 103% → 0-1.4%.** Target met. +- **No thread pinned at 100% when idle.** Target met. +- **Step throughput: 342.5 → 331.5 steps/sec (3.2% drop).** Within 5% budget. +- **Step latency: 2.92ms → 3.02ms.** Negligible change. diff --git a/client/python/projectairsim/src/projectairsim/client.py b/client/python/projectairsim/src/projectairsim/client.py index 89f537ff..c9c64da2 100644 --- a/client/python/projectairsim/src/projectairsim/client.py +++ b/client/python/projectairsim/src/projectairsim/client.py @@ -78,9 +78,6 @@ def connect(self): ) projectairsim_log().info("Connection opened.") self.state = True - self.recv_topic_thread = threading.Thread(target=self.__recv_topic) - self.recv_topic_thread.start() - projectairsim_log().info("Started the pub-sub topic receiving thread.") def get_topic_info(self): """This is used by World to get the list of topic info on reload scene.""" @@ -125,6 +122,12 @@ def subscribe(self, topic, callback, reliability=1.0): topic (str): the name of the topic callback (callable): function to call when data is received """ + # Lazily start the recv thread on first subscribe call + if self.recv_topic_thread is None: + self.socket_topics.recv_timeout = 100 # ms — bounded block for clean shutdown + self.recv_topic_thread = threading.Thread(target=self.__recv_topic) + self.recv_topic_thread.start() + if topic in self.subs: self.subs[topic]["callbacks"].append(callback) self.subs[topic]["reliability"] = reliability @@ -479,9 +482,10 @@ def __make_message_frame(self, topic, message): def __recv_topic_frame(self): try: - # Check if new data is ready to be received with non-blocking recv() call - frame_packed = self.socket_topics.recv(block=False) - except pynng.exceptions.TryAgain: + # Blocking recv with timeout (set on socket in subscribe()). + # Returns None on timeout so the loop can check self.state. + frame_packed = self.socket_topics.recv() + except (pynng.exceptions.TryAgain, pynng.exceptions.Timeout): return # If data was ready to be received, process and return it. diff --git a/core_sim/src/topic_manager.cpp b/core_sim/src/topic_manager.cpp index 0e868237..9f4e9816 100644 --- a/core_sim/src/topic_manager.cpp +++ b/core_sim/src/topic_manager.cpp @@ -268,6 +268,18 @@ void TopicManager::Impl::Start() { throw Error("Error setting send timeout on server socket."); } + // Recv timeout so RecvLoop blocks instead of polling with NNG_FLAG_NONBLOCK. + // 100ms is short enough for responsive shutdown (state_ check) while + // eliminating the 1ms poll wakeup overhead. + const int recv_timeout = 100; + rv = nng_setopt_ms(topic_socket_, NNG_OPT_RECVTIMEO, recv_timeout); + if (rv != 0) { + auto errno_str = nng_strerror(rv); + log_.LogError(Constant::Component::topic_manager, + "nng_setopt_ms failed with '%s'.", errno_str); + throw Error("Error setting recv timeout on server socket."); + } + // Previously with Nanomsg the send buffer size NN_SNDBUF was set to 4 MB, // but NNG's send buffer size parameter NNG_OPT_SENDBUF is in number of // messages rather than bytes, so instead of choosing some arbitrary number @@ -419,13 +431,11 @@ void TopicManager::Impl::UnregisterTopic(const Topic& topic) { void TopicManager::Impl::RecvLoop() { while (state_.load()) { int rv = nng_recv(topic_socket_, &recv_buffer_, &recv_buffer_size_, - NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK); + NNG_FLAG_ALLOC); if (rv != 0) { - if (rv == NNG_EAGAIN) { - // No data is ready yet, spin to retry - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; + if (rv == NNG_ETIMEDOUT) { + continue; // recv timeout — re-check state_ and retry } else { auto errno_str = nng_strerror(rv); log_.LogWarning(name_, "nng_recv failed with '%s'.", errno_str); diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealScene.cpp b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealScene.cpp index e7678a42..0a7358d5 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealScene.cpp +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/UnrealScene.cpp @@ -471,7 +471,7 @@ void AUnrealScene::Tick(float DeltaTime) { // Wait for sim to catch up processing new sim time while (projectairsim::SimClock::Get()->NowSimNanos() < cur_sim_time) { - std::this_thread::sleep_for(std::chrono::duration(0)); + std::this_thread::sleep_for(std::chrono::microseconds(100)); } // If cur_sim_time is now at the time for simclock to pause, pause // Unreal now so physics will not advance again at the next tick. From 20e73764abbb2ab934ed47b7d7068c29d9aeef93 Mon Sep 17 00:00:00 2001 From: Zaffer <51871197+Zaffer@users.noreply.github.com> Date: Tue, 24 Mar 2026 23:54:21 -0400 Subject: [PATCH 3/5] Add step API --- .../projectairsim/src/projectairsim/world.py | 29 ++- .../projectairsim/tests/test_step_api.py | 168 ++++++++++++++ core_sim/include/core_sim/actor/robot.hpp | 4 + core_sim/src/actor/robot.cpp | 41 ++++ core_sim/src/scene.cpp | 57 +++++ tests/e2e/test_debug_viz.py | 210 ------------------ tests/test_cpu_usage.py | 21 +- 7 files changed, 308 insertions(+), 222 deletions(-) create mode 100644 client/python/projectairsim/tests/test_step_api.py delete mode 100644 tests/e2e/test_debug_viz.py diff --git a/client/python/projectairsim/src/projectairsim/world.py b/client/python/projectairsim/src/projectairsim/world.py index affe11cb..9ec937b3 100644 --- a/client/python/projectairsim/src/projectairsim/world.py +++ b/client/python/projectairsim/src/projectairsim/world.py @@ -334,7 +334,34 @@ def continue_for_single_step(self, wait_until_complete: bool = True) -> int: single_step_result: str = self.client.request(single_step_req) return single_step_result - def list_actors(self) -> List[str]: + def step(self, dt_ns: int) -> dict: + """Advance sim by dt_ns nanoseconds and return bundled state + events. + + This is the pull API entry point. The sim advances time, then returns + per-drone kinematics and any events (collisions) that + occurred during the step. + + Actions should be sent to individual drones via their existing move + methods before calling step(). + + Args: + dt_ns: simulation time to advance, in nanoseconds + + Returns: + dict with keys: + sim_time_ns (int): current sim time after the step + drones (dict): per-drone data keyed by drone ID, each containing: + state (dict): position, orientation, linear_velocity, angular_velocity + events (list): collision and gate_pass events with sim_time_ns ordering + """ + step_req: dict = { + "method": f"{self.parent_topic}/Step", + "params": {"dt_ns": dt_ns}, + "version": 1.0, + } + return self.client.request(step_req) + + def list_actors(self) -> list[str]: """List actor names in the scene Returns: diff --git a/client/python/projectairsim/tests/test_step_api.py b/client/python/projectairsim/tests/test_step_api.py new file mode 100644 index 00000000..fad3ebdc --- /dev/null +++ b/client/python/projectairsim/tests/test_step_api.py @@ -0,0 +1,168 @@ +""" +Tests for the pull API Step() method on World. +""" + +from unittest.mock import MagicMock +from projectairsim.world import World + + +def _make_world(): + """Create a World with a mocked client, bypassing __init__ side effects.""" + world = World.__new__(World) + world.client = MagicMock() + world.parent_topic = "/Sim/TestScene" + return world + + +def test_step_sends_correct_request(): + world = _make_world() + world.client.request.return_value = { + "sim_time_ns": 100_000_000, + "drones": {}, + } + + result = world.step(dt_ns=100_000_000) + + world.client.request.assert_called_once() + req = world.client.request.call_args[0][0] + assert req["method"] == "/Sim/TestScene/Step" + assert req["params"]["dt_ns"] == 100_000_000 + assert req["version"] == 1.0 + + +def test_step_returns_sim_time_and_drones(): + world = _make_world() + expected = { + "sim_time_ns": 200_000_000, + "drones": { + "Drone1": { + "state": { + "position": {"x": 1.0, "y": 2.0, "z": -3.0}, + "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "linear_velocity": {"x": 0.5, "y": 0.0, "z": 0.0}, + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.1}, + }, + "events": [], + } + }, + } + world.client.request.return_value = expected + + result = world.step(dt_ns=100_000_000) + + assert result["sim_time_ns"] == 200_000_000 + assert "Drone1" in result["drones"] + assert result["drones"]["Drone1"]["state"]["position"]["x"] == 1.0 + assert result["drones"]["Drone1"]["events"] == [] + + +def test_step_returns_collision_events(): + world = _make_world() + expected = { + "sim_time_ns": 300_000_000, + "drones": { + "Drone1": { + "state": { + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "linear_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + }, + "events": [ + { + "type": "collision", + "sim_time_ns": 299_000_000, + "object_name": "gate_frame", + "impact_point": {"x": 10.0, "y": 0.0, "z": -2.0}, + "normal": {"x": -1.0, "y": 0.0, "z": 0.0}, + } + ], + } + }, + } + world.client.request.return_value = expected + + result = world.step(dt_ns=100_000_000) + + events = result["drones"]["Drone1"]["events"] + assert len(events) == 1 + assert events[0]["type"] == "collision" + assert events[0]["object_name"] == "gate_frame" + + +def test_step_returns_gate_pass_events(): + world = _make_world() + expected = { + "sim_time_ns": 400_000_000, + "drones": { + "Drone1": { + "state": { + "position": {"x": 20.0, "y": 0.0, "z": -2.0}, + "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "linear_velocity": {"x": 8.0, "y": 0.0, "z": 0.0}, + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + }, + "events": [ + { + "type": "gate_pass", + "sim_time_ns": 398_000_000, + "gate_index": 3, + "lap_count": 1, + "is_correct_order": True, + } + ], + } + }, + } + world.client.request.return_value = expected + + result = world.step(dt_ns=100_000_000) + + events = result["drones"]["Drone1"]["events"] + assert len(events) == 1 + assert events[0]["type"] == "gate_pass" + assert events[0]["gate_index"] == 3 + assert events[0]["is_correct_order"] is True + + +def test_step_preserves_event_ordering(): + """Events within a step should preserve their sim_time_ns ordering.""" + world = _make_world() + expected = { + "sim_time_ns": 500_000_000, + "drones": { + "Drone1": { + "state": { + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "linear_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + }, + "events": [ + { + "type": "gate_pass", + "sim_time_ns": 498_000_000, + "gate_index": 2, + "lap_count": 1, + "is_correct_order": True, + }, + { + "type": "collision", + "sim_time_ns": 499_000_000, + "object_name": "gate_frame", + "impact_point": {"x": 10.0, "y": 0.0, "z": -2.0}, + "normal": {"x": -1.0, "y": 0.0, "z": 0.0}, + }, + ], + } + }, + } + world.client.request.return_value = expected + + result = world.step(dt_ns=100_000_000) + + events = result["drones"]["Drone1"]["events"] + assert len(events) == 2 + assert events[0]["sim_time_ns"] < events[1]["sim_time_ns"] + assert events[0]["type"] == "gate_pass" + assert events[1]["type"] == "collision" diff --git a/core_sim/include/core_sim/actor/robot.hpp b/core_sim/include/core_sim/actor/robot.hpp index 59b4aeb4..022ef3c3 100644 --- a/core_sim/include/core_sim/actor/robot.hpp +++ b/core_sim/include/core_sim/actor/robot.hpp @@ -103,6 +103,10 @@ class Robot : public Actor { const CollisionInfo& GetCollisionInfo() const; const Vector3& GetExternalForce() const; + // Pull API event accumulation: events are buffered during sim steps and + // drained into the Step() response. + json DrainStepEvents(); + // Manually sets actuated rotations on the robot links (such as spinning // propeller link meshes) that are not moved through the physics model void SetActuatedRotations(const ActuatedRotations& actuated_rots, diff --git a/core_sim/src/actor/robot.cpp b/core_sim/src/actor/robot.cpp index 33eeb12a..bc8c2b53 100644 --- a/core_sim/src/actor/robot.cpp +++ b/core_sim/src/actor/robot.cpp @@ -126,6 +126,10 @@ class Robot::Impl : public ActorImpl { const CollisionInfo& GetCollisionInfo() const; const Vector3& GetExternalForce() const; + // Pull API event accumulation + void AccumulateStepEvent(const json& event); + json DrainStepEvents(); + void SetActuatedRotations(const ActuatedRotations& actuated_rots, TimeNano external_time_stamp = -1); @@ -208,6 +212,10 @@ class Robot::Impl : public ActorImpl { Topic rotor_info_topic_; std::vector> topics_; + // Pull API event accumulation buffer + std::vector step_event_buffer_; + std::mutex step_event_mutex_; + KinematicsCallback callback_kinematics_updated_; ActuatedTransformsCallback callback_actuated_transforms_updated_; @@ -414,6 +422,10 @@ const CollisionInfo& Robot::GetCollisionInfo() const { return static_cast(pimpl_.get())->GetCollisionInfo(); } +json Robot::DrainStepEvents() { + return static_cast(pimpl_.get())->DrainStepEvents(); +} + void Robot::SetActuatedTransforms(const ActuatedTransforms& actuated_transforms, TimeNano external_time_stamp) { static_cast(pimpl_.get()) @@ -1034,7 +1046,36 @@ void Robot::Impl::UpdateCollisionInfo(const CollisionInfo& collision_info) { if (collision_info_.has_collided) { CollisionInfoMessage collision_info_msg(collision_info_); topic_manager_.PublishTopic(collision_info_topic_, collision_info_msg); + + // Accumulate collision event for pull API Step() response + AccumulateStepEvent(json{ + {"type", "collision"}, + {"sim_time_ns", collision_info_.time_stamp}, + {"object_name", collision_info_.object_name}, + {"impact_point", + {{"x", collision_info_.impact_point.x()}, + {"y", collision_info_.impact_point.y()}, + {"z", collision_info_.impact_point.z()}}}, + {"normal", + {{"x", collision_info_.normal.x()}, + {"y", collision_info_.normal.y()}, + {"z", collision_info_.normal.z()}}}}); + } +} + +void Robot::Impl::AccumulateStepEvent(const json& event) { + std::lock_guard lock(step_event_mutex_); + step_event_buffer_.push_back(event); +} + +json Robot::Impl::DrainStepEvents() { + std::lock_guard lock(step_event_mutex_); + json events = json::array(); + for (auto& event : step_event_buffer_) { + events.push_back(std::move(event)); } + step_event_buffer_.clear(); + return events; } void Robot::Impl::SetHasCollided(bool has_collided) { diff --git a/core_sim/src/scene.cpp b/core_sim/src/scene.cpp index 11d5ba1d..a3227d90 100644 --- a/core_sim/src/scene.cpp +++ b/core_sim/src/scene.cpp @@ -169,6 +169,9 @@ class Scene::Impl : public ComponentWithTopicsAndServiceMethods { TimeNano ContinueForSingleStep(bool wait_until_complete = false); std::vector SimGetActors(); + // Pull API: advance sim and return bundled state + events + json Step(TimeNano dt_ns); + bool SetWindVelocity(float v_x, float v_y, float v_z); Vector3 GetWindVelocity(); void UpdateWindVelocity(); @@ -786,6 +789,55 @@ TimeNano Scene::Impl::ContinueForSingleStep(bool wait_until_complete) { return SimClock::Get()->NowSimNanos(); } +json Scene::Impl::Step(TimeNano dt_ns) { + if (clock_settings_.type != ClockType::kSteppable) { + logger_.LogError(name_, "This clock type doesn't support Step."); + throw Error("This clock type doesn't support Step."); + } + + // Advance sim time and wait for completion + SimClock::Get()->ContinueForSimTime(dt_ns); + while (!IsSimPaused()) { + std::this_thread::yield(); + } + + TimeNano current_sim_time = SimClock::Get()->NowSimNanos(); + + // Build response with per-drone state and events + json drones = json::object(); + for (auto& actor : actors_) { + if (actor->GetType() == ActorType::kRobot) { + auto& robot = static_cast(*actor); + const auto& kin = robot.GetKinematics(); + + json state = json{ + {"position", + {{"x", kin.pose.position.x()}, + {"y", kin.pose.position.y()}, + {"z", kin.pose.position.z()}}}, + {"orientation", + {{"w", kin.pose.orientation.w()}, + {"x", kin.pose.orientation.x()}, + {"y", kin.pose.orientation.y()}, + {"z", kin.pose.orientation.z()}}}, + {"linear_velocity", + {{"x", kin.twist.linear.x()}, + {"y", kin.twist.linear.y()}, + {"z", kin.twist.linear.z()}}}, + {"angular_velocity", + {{"x", kin.twist.angular.x()}, + {"y", kin.twist.angular.y()}, + {"z", kin.twist.angular.z()}}}}; + + json events = robot.DrainStepEvents(); + + drones[robot.GetID()] = json{{"state", state}, {"events", events}}; + } + } + + return json{{"sim_time_ns", current_sim_time}, {"drones", drones}}; +} + std::vector Scene::Impl::SimGetActors() { std::vector actor_ids = {}; for (const auto& actor : actors_) { @@ -884,6 +936,11 @@ void Scene::Impl::RegisterServiceMethods() { auto get_wind_vel_handler = get_wind_vel.CreateMethodHandler(&Scene::Impl::GetWindVelocity, *this); RegisterServiceMethod(get_wind_vel, get_wind_vel_handler); + + // Pull API: Step advances sim and returns bundled state + events + auto step = ServiceMethod("Step", {"dt_ns"}); + auto step_handler = step.CreateMethodHandler(&Scene::Impl::Step, *this); + RegisterServiceMethod(step, step_handler); } void Scene::Impl::UnregisterAllServiceMethods() { diff --git a/tests/e2e/test_debug_viz.py b/tests/e2e/test_debug_viz.py deleted file mode 100644 index 74630303..00000000 --- a/tests/e2e/test_debug_viz.py +++ /dev/null @@ -1,210 +0,0 @@ -""" -Test all debug visualization primitives. -Uses step API to fly the drone (acro mode + throttle), draws debug shapes with labels. -""" - -import os -import time - -from projectairsim import ProjectAirSimClient, Drone, World -from projectairsim.utils import projectairsim_log - -DURATION = 60.0 -SIM_ADDR = "172.23.240.1" -STEP_NS = 3_000_000 # 3ms per physics tick -SIM_CONFIG_PATH = os.path.join( - os.path.dirname(__file__), "..", "..", "client", "python", "example_user_scripts", "sim_config" -) - - -# Layout helper: NED coordinates -def pos(east_offset, z=-3.0): - return [0.0, east_offset, z] - - -def arm_drone(client, topic): - client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) - client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) - client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) - - -def set_controls(client, topic, throttle, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0): - client.request({ - "method": f"{topic}/SetAngleRatesThrottle", - "params": {"roll_rate": roll_rate, "pitch_rate": pitch_rate, - "yaw_rate": yaw_rate, "throttle": throttle}, - "version": 1.0, - }) - - -def main(): - client = ProjectAirSimClient(address=SIM_ADDR) - - try: - client.connect() - world = World(client, "scene_basic_drone.jsonc", delay_after_load_sec=2, sim_config_path=SIM_CONFIG_PATH) - - drone_topic = f"{world.parent_topic}/robots/Drone1" - - # --- Draw all debug primitives --- - projectairsim_log().info("Drawing debug visualization primitives...") - - spacing = 4.0 - z = -3.0 # 3m above ground (NED) - - label_positions = [] - label_names = [] - - def add_label(name, east): - label_positions.append([0.0, east, z]) - label_names.append(name) - - # 1) POINTS - cluster of red points - east = 0.0 - add_label("Points", east) - world.plot_debug_points( - points=[ - pos(east - 0.3, z), pos(east + 0.3, z), pos(east, z - 0.3), - pos(east, z + 0.3), pos(east - 0.2, z - 0.2), pos(east + 0.2, z + 0.2), - ], - color_rgba=[1.0, 0.0, 0.0, 1.0], - size=15.0, - duration=DURATION, - is_persistent=False, - ) - - # 2) SOLID LINE - green zigzag - east += spacing - add_label("Solid Line", east) - world.plot_debug_solid_line( - points=[ - pos(east - 1.0, z), - pos(east - 0.5, z - 0.8), - pos(east, z), - pos(east + 0.5, z - 0.8), - pos(east + 1.0, z), - ], - color_rgba=[0.0, 1.0, 0.0, 1.0], - thickness=3.0, - duration=DURATION, - is_persistent=False, - ) - - # 3) DASHED LINE - cyan segments (even number of points) - east += spacing - add_label("Dashed Line", east) - world.plot_debug_dashed_line( - points=[ - pos(east - 1.0, z), pos(east - 0.5, z), - pos(east - 0.3, z), pos(east + 0.3, z), - pos(east + 0.5, z), pos(east + 1.0, z), - ], - color_rgba=[0.0, 1.0, 1.0, 1.0], - thickness=3.0, - duration=DURATION, - is_persistent=False, - ) - - # 4) ARROWS - magenta arrows pointing in different directions - east += spacing - add_label("Arrows", east) - world.plot_debug_arrows( - points_start=[ - pos(east, z), - pos(east, z), - pos(east, z), - ], - points_end=[ - pos(east + 1.5, z), - pos(east, z - 1.5), - pos(east + 1.0, z - 1.0), - ], - color_rgba=[1.0, 0.0, 1.0, 1.0], - thickness=3.0, - arrow_size=30.0, - duration=DURATION, - is_persistent=False, - ) - - # 5) COLORED POINTS DEMO - heatmap gradient blue -> red - east += spacing - add_label("Color Gradient", east) - n = 10 - for i in range(n): - t = i / max(n - 1, 1) - world.plot_debug_points( - points=[pos(east - 1.5 + 3.0 * t, z)], - color_rgba=[t, 0.0, 1.0 - t, 1.0], - size=20.0, - duration=DURATION, - is_persistent=False, - ) - - # 6) Labels for everything - world.plot_debug_strings( - strings=label_names, - positions=label_positions, - scale=5.0, - color_rgba=[1.0, 1.0, 0.0, 1.0], - duration=DURATION, - ) - - # Big test string near origin - world.plot_debug_strings( - strings=["HELLO DEBUG"], - positions=[[0.0, 0.0, -1.0]], - scale=10.0, - color_rgba=[1.0, 1.0, 1.0, 1.0], - duration=DURATION, - ) - - projectairsim_log().info("All primitives drawn!") - - # --- Fly the drone using step API (acro mode) --- - projectairsim_log().info("Setting up trace and flying drone via step API...") - world.set_trace_line(color_rgba=[1.0, 0.5, 0.0, 1.0], thickness=4.0) - world.toggle_trace() - - arm_drone(client, drone_topic) - - # Step once to settle - world.step(STEP_NS) - - # Phase 1: Climb (~3s = 1000 steps at 3ms) - projectairsim_log().info("Climbing...") - set_controls(client, drone_topic, throttle=0.55) - for _ in range(1000): - world.step(STEP_NS) - - # Phase 2: Pitch forward (~2s) - projectairsim_log().info("Flying forward...") - set_controls(client, drone_topic, throttle=0.45, pitch_rate=0.5) - for _ in range(666): - world.step(STEP_NS) - - # Phase 3: Yaw spin (~2s) - projectairsim_log().info("Yawing...") - set_controls(client, drone_topic, throttle=0.45, yaw_rate=1.0) - for _ in range(666): - world.step(STEP_NS) - - # Phase 4: Cut throttle and descend (~3s) - projectairsim_log().info("Descending...") - set_controls(client, drone_topic, throttle=0.0) - for _ in range(1000): - world.step(STEP_NS) - - projectairsim_log().info("Flight done. Keeping connection alive for 30s...") - - # Keep alive so debug drawings stay visible - time.sleep(30) - - except Exception as err: - projectairsim_log().error(f"Exception: {err}", exc_info=True) - - finally: - client.disconnect() - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/tests/test_cpu_usage.py b/tests/test_cpu_usage.py index ced824f6..100fe9f2 100644 --- a/tests/test_cpu_usage.py +++ b/tests/test_cpu_usage.py @@ -15,7 +15,8 @@ 9. Idle after all stepping Run from: ProjectAirSim repo root -Requires: UE editor with NavGym map in Play mode. +Requires: UE editor or packaged sim in Play mode (Blocks map is fine). +Uses bundled scene_basic_drone.jsonc from example_user_scripts/sim_config. Usage: uv run pytest tests/test_cpu_usage.py -v -s @@ -33,19 +34,18 @@ from projectairsim import ProjectAirSimClient, Drone, World from projectairsim.types import ImageType -SIM_ADDRESS = "172.23.240.1" STEP_3MS = 3_000_000 STEP_10MS = 10_000_000 STEP_20MS = 20_000_000 -NAV_JAX_SIM_CONFIG = str( - Path(__file__).resolve().parent.parent.parent - / "nav-jax" - / "sims" - / "airsim" +SIM_CONFIG_PATH = str( + Path(__file__).resolve().parent.parent + / "client" + / "python" + / "example_user_scripts" / "sim_config" ) -SCENE_CONFIG = "scene_gate_course_rl.jsonc" +SCENE_CONFIG = "scene_basic_drone.jsonc" MEASURE_SEC = 5.0 # measurement window per case @@ -177,7 +177,7 @@ def print_report(label: str, stats: dict): @pytest.fixture(scope="module") def client(): - c = ProjectAirSimClient(address=SIM_ADDRESS) + c = ProjectAirSimClient() c.connect() yield c c.disconnect() @@ -188,7 +188,7 @@ def world(client): w = World( client, SCENE_CONFIG, - sim_config_path=NAV_JAX_SIM_CONFIG, + sim_config_path=SIM_CONFIG_PATH, delay_after_load_sec=2, ) return w @@ -201,7 +201,6 @@ def drone(client, world): topic = f"{world.parent_topic}/robots/Drone1" client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) - client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) return d From da7409d4d1a426da9256203403693021388347a3 Mon Sep 17 00:00:00 2001 From: Lucas Scheinkerman Date: Wed, 24 Jun 2026 12:03:36 -0300 Subject: [PATCH 4/5] Comment GetImages() based cpu tests --- tests/test_cpu_usage.py | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/tests/test_cpu_usage.py b/tests/test_cpu_usage.py index 100fe9f2..7c298dd4 100644 --- a/tests/test_cpu_usage.py +++ b/tests/test_cpu_usage.py @@ -236,21 +236,23 @@ def test_case4_step_3ms(self, client, world, drone): print_report("CASE 4: Step only, dt=3ms", stats) ALL_RESULTS["4_step_3ms"] = stats - def test_case5_step_3ms_images_every(self, client, world, drone): - """Case 5: Step loop, dt=3ms, GetImages every step.""" - get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) - stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, - get_images_fn=get_img, image_every_n=1) - print_report("CASE 5: Step + GetImages every step, dt=3ms", stats) - ALL_RESULTS["5_step_3ms_img1"] = stats - - def test_case6_step_3ms_images_every10(self, client, world, drone): - """Case 6: Step loop, dt=3ms, GetImages every 10 steps.""" - get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) - stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, - get_images_fn=get_img, image_every_n=10) - print_report("CASE 6: Step + GetImages every 10 steps, dt=3ms", stats) - ALL_RESULTS["6_step_3ms_img10"] = stats + # Disabled until GetImages() timeout is fixed + # def test_case5_step_3ms_images_every(self, client, world, drone): + # """Case 5: Step loop, dt=3ms, GetImages every step.""" + # get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) + # stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, + # get_images_fn=get_img, image_every_n=1) + # print_report("CASE 5: Step + GetImages every step, dt=3ms", stats) + # ALL_RESULTS["5_step_3ms_img1"] = stats + + # Disabled until GetImages() timeout is fixed + # def test_case6_step_3ms_images_every10(self, client, world, drone): + # """Case 6: Step loop, dt=3ms, GetImages every 10 steps.""" + # get_img = lambda: drone.get_images("Chase", [ImageType.SCENE]) + # stats = run_step_loop(world, STEP_3MS, MEASURE_SEC, + # get_images_fn=get_img, image_every_n=10) + # print_report("CASE 6: Step + GetImages every 10 steps, dt=3ms", stats) + # ALL_RESULTS["6_step_3ms_img10"] = stats def test_case7_step_10ms(self, client, world, drone): """Case 7: Step loop, dt=10ms, no images.""" From f94a1ba73918bbe14adb90557684739866f37831 Mon Sep 17 00:00:00 2001 From: Lucas Scheinkerman Date: Wed, 24 Jun 2026 12:09:36 -0300 Subject: [PATCH 5/5] Remove unnecessary files --- .claude/plans/improve-resource-usage-plan.md | 116 ----------- .../projectairsim/tests}/test_cpu_usage.py | 0 tests/test_multi_drone_lift_land.py | 185 ------------------ 3 files changed, 301 deletions(-) delete mode 100644 .claude/plans/improve-resource-usage-plan.md rename {tests => client/python/projectairsim/tests}/test_cpu_usage.py (100%) delete mode 100644 tests/test_multi_drone_lift_land.py diff --git a/.claude/plans/improve-resource-usage-plan.md b/.claude/plans/improve-resource-usage-plan.md deleted file mode 100644 index 78227f55..00000000 --- a/.claude/plans/improve-resource-usage-plan.md +++ /dev/null @@ -1,116 +0,0 @@ -# Fix Excessive CPU Usage — Step-API-Only Mode - -## Context - -When a Python client connects, CPU usage spikes to 103%+ even when idle. The root cause is a single Python thread spinning `recv(block=False)` in a zero-sleep loop. Server-side system CPU is only 3-6% — not a real problem. - -## Plan - -### PR1: Kill client-side recv spin -**File:** `client/python/projectairsim/src/projectairsim/client.py` - -1. Remove auto-start of `recv_topic_thread` from `connect()` (line 81-82) -2. Start it lazily on first `subscribe()` call -3. Switch recv loop from `recv(block=False)` to blocking recv with 100ms timeout -4. Keep thread non-daemon — the 100ms recv timeout guarantees bounded join on `disconnect()` - -**Note:** `get_topic_info()` calls `subscribe()` internally, so lazy start still works for topic discovery. - -**Acceptance:** Idle Python CPU < 5%. Step throughput within 5% of baseline. - -### PR2: Server topic manager — blocking recv -**File:** `core_sim/src/topic_manager.cpp` - -1. Set `NNG_OPT_RECVTIMEO` (100ms) on topic socket at startup -2. Remove `NNG_FLAG_NONBLOCK` from `nng_recv()` (line 422) -3. Remove `sleep_for(milliseconds(1))` (line 427) -4. Handle `NNG_ETIMEDOUT` same as `NNG_EAGAIN` (continue) - -**Acceptance:** Server stops waking 1000/sec while idle. - -### After PR1+PR2: Re-profile -Run full test matrix. If system CPU and step throughput look fine, stop here. - -## Files to Modify -- `client/python/projectairsim/src/projectairsim/client.py` -- `core_sim/src/topic_manager.cpp` - -## Verification -``` -uv run pytest tests/test_cpu_usage.py -v -s -``` -**Pass criteria:** -1. Idle Python CPU < 5% (from 103%) -2. No thread pinned near 100% when idle -3. Step throughput regression < 5% (baseline: 342.5 steps/sec at 3ms dt) - ---- - -## Future (only if post-fix profiling shows need) - -### Scene step yield loops → condition variable -`core_sim/src/scene.cpp` lines 695, 722, 746, 769, 789 — `while (!IsSimPaused()) { yield(); }`. Replace with CV wait + stop predicate. Requires lock-order validation against `ScheduledExecutor::mutex_` (clock.cpp:305). - -### ScheduledExecutor paused spin → 1ms sleep -`core_sim/src/clock.cpp:287-326` — loops at 333Hz when paused. Back off to 1ms sleep. Must verify < 5% throughput regression. - -### UnrealScene sleep(0) → sleep(100µs) -`UnrealScene.cpp:441-443` — replace zero-duration sleep with bounded microsecond sleep. - ---- - -## Measurements - -### Baseline (2026-04-05, pre-fix) - -#### Idle / Connected Cases - -| Case | Scenario | Py CPU | Sys CPU | Hottest Thread | -|------|----------------------------|------------|---------|----------------| -| 2 | Connected only | **103.5%** | 5.9% | 104.0% | -| 3 | Scene loaded, idle | **103.3%** | 3.7% | 103.6% | -| 9 | Idle after stepping | **103.6%** | 3.4% | 103.6% | - -#### Stepping Cases - -| Case | Scenario | Py CPU | Steps/sec | Avg Latency | -|------|---------------------------------|------------|-----------|-------------| -| 4 | Step 3ms, no images | **105.9%** | 342.5 | 2.92ms | -| 5 | Step 3ms + GetImages every step | **104.2%** | 3.2 | 314.57ms | -| 6 | Step 3ms + GetImages every 10 | **104.3%** | 31.2 | 32.06ms | -| 7 | Step 10ms, no images | **104.2%** | 85.8 | 11.66ms | -| 8 | Step 20ms, no images | **104.0%** | 48.9 | 20.44ms | - -#### Key Findings - -1. **One Python thread (recv_topic) burns an entire core** even when idle — the only real problem. -2. **System CPU is 3-6%** — server-side spins exist but aren't impactful. -3. **GetImages is 314ms/call** — separate issue, not a spin problem. -4. **Step latency scales linearly with dt** — RPC overhead is negligible. - -### After PR1 (2026-04-07) - -#### Idle / Connected Cases - -| Case | Scenario | Py CPU | Sys CPU | Threads | Hottest Thread | -|------|----------------------------|----------|---------|---------|----------------| -| 2 | Connected only | **0.0%** | 2.8% | 1 | 0.0% | -| 3 | Scene loaded, idle | **1.4%** | 0.5% | 2 | 0.2% | -| 9 | Idle after stepping | **1.2%** | 0.5% | 2 | — | - -#### Stepping Cases - -| Case | Scenario | Py CPU | Steps/sec | Avg Latency | -|------|---------------------------------|----------|-----------|-------------| -| 4 | Step 3ms, no images | **5.0%** | 331.5 | 3.02ms | -| 5 | Step 3ms + GetImages every step | **5.5%** | 3.1 | 317.97ms | -| 6 | Step 3ms + GetImages every 10 | **5.6%** | 32.8 | 30.50ms | -| 7 | Step 10ms, no images | **2.4%** | 90.5 | 11.05ms | -| 8 | Step 20ms, no images | **2.2%** | 50.4 | 19.84ms | - -#### Results - -- **Idle CPU: 103% → 0-1.4%.** Target met. -- **No thread pinned at 100% when idle.** Target met. -- **Step throughput: 342.5 → 331.5 steps/sec (3.2% drop).** Within 5% budget. -- **Step latency: 2.92ms → 3.02ms.** Negligible change. diff --git a/tests/test_cpu_usage.py b/client/python/projectairsim/tests/test_cpu_usage.py similarity index 100% rename from tests/test_cpu_usage.py rename to client/python/projectairsim/tests/test_cpu_usage.py diff --git a/tests/test_multi_drone_lift_land.py b/tests/test_multi_drone_lift_land.py deleted file mode 100644 index 4f4b4bb9..00000000 --- a/tests/test_multi_drone_lift_land.py +++ /dev/null @@ -1,185 +0,0 @@ -"""Multi-drone lift-land test using nav-jax RL robot config (3 cameras per drone). - -Loads the multi-drone scene with the RL robot config (Chase, World, FPV cameras), -arms N drones, lifts them, verifies altitude, then lands. - -Run from: ProjectAirSim repo root -Requires: UE editor with NavGym map in Play mode. - -Usage: - uv run pytest tests/test_multi_drone_lift_land.py -v - uv run pytest tests/test_multi_drone_lift_land.py -v -k "test_lift_land_3" -""" - -import sys -import time -from pathlib import Path - -import commentjson -import pytest -from projectairsim import ProjectAirSimClient, Drone, World -from projectairsim.types import ImageType -from projectairsim.types import Pose -from projectairsim.utils import load_scene_config_as_dict - -SIM_ADDRESS = "172.23.240.1" -STEP_NS = 3_000_000 # 3ms per physics tick - -# nav-jax sim_config directory (has robot + scene configs) -NAV_JAX_SIM_CONFIG = str(Path(__file__).resolve().parent.parent.parent / "nav-jax" / "sims" / "airsim" / "sim_config") -SCENE_CONFIG = "scene_gate_course_rl_multi.jsonc" - - -# ── Helpers ────────────────────────────────────────────────────────── - - -def load_scene(client, num_drones): - """Load multi-drone scene and return World + list of drone topics.""" - world = World( - client, - SCENE_CONFIG, - sim_config_path=NAV_JAX_SIM_CONFIG, - delay_after_load_sec=2, - ) - - drone_topics = [f"{world.parent_topic}/robots/Drone{i+1}" for i in range(num_drones)] - return world, drone_topics - - -def arm_drone(client, topic): - """EnableApiControl + Arm + SetAcroMode.""" - client.request({"method": f"{topic}/EnableApiControl", "params": {}, "version": 1.0}) - client.request({"method": f"{topic}/Arm", "params": {}, "version": 1.0}) - client.request({"method": f"{topic}/SetAcroMode", "params": {"enabled": True}, "version": 1.0}) - - -def set_controls(client, topic, throttle, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0): - client.request({ - "method": f"{topic}/SetAngleRatesThrottle", - "params": {"roll_rate": roll_rate, "pitch_rate": pitch_rate, - "yaw_rate": yaw_rate, "throttle": throttle}, - "version": 1.0, - }) - - -# ── Tests ──────────────────────────────────────────────────────────── - - -def _run_lift_land(num_drones): - """Core lift-land test for N drones.""" - client = ProjectAirSimClient(address=SIM_ADDRESS) - client.connect() - - try: - world, drone_topics = load_scene(client, num_drones) - drone_names = [f"Drone{i+1}" for i in range(num_drones)] - - # Arm all drones - for topic in drone_topics: - arm_drone(client, topic) - - # Step once to settle - resp = world.step(STEP_NS) - assert "drones" in resp - for name in drone_names: - assert name in resp["drones"], f"{name} not in step response" - - # Record starting z for each drone - start_z = {} - for name in drone_names: - start_z[name] = resp["drones"][name]["state"]["position"]["z"] - print(f" {name} start z={start_z[name]:.2f}") - - # Phase 1: Gentle climb for ~3s - for topic in drone_topics: - set_controls(client, topic, throttle=0.5) - - for _ in range(1000): - resp = world.step(STEP_NS) - - print("\nAfter climb:") - for name in drone_names: - z = resp["drones"][name]["state"]["position"]["z"] - print(f" {name} z={z:.2f}") - assert z < start_z[name] - 0.5, ( - f"{name} didn't climb: start={start_z[name]:.2f}, now={z:.2f}" - ) - - # Phase 2: Pitch forward for ~2s - print("\nPitching forward...") - for topic in drone_topics: - set_controls(client, topic, throttle=0.4, pitch_rate=0.5) - - for _ in range(666): - resp = world.step(STEP_NS) - - # Phase 3: Yaw spin for ~2s - print("Yawing...") - for topic in drone_topics: - set_controls(client, topic, throttle=0.4, yaw_rate=1.0) - - for _ in range(666): - resp = world.step(STEP_NS) - - # Phase 4: Roll tilt for ~2s - print("Rolling...") - for topic in drone_topics: - set_controls(client, topic, throttle=0.4, roll_rate=0.5) - - for _ in range(666): - resp = world.step(STEP_NS) - - # Phase 5: Hover for ~2s to settle - print("Hovering...") - for topic in drone_topics: - set_controls(client, topic, throttle=0.35) - - for _ in range(666): - resp = world.step(STEP_NS) - - # Phase 6: Cut throttle — drones descend for ~3s - print("Descending...") - for topic in drone_topics: - set_controls(client, topic, throttle=0.0) - - climb_z = {name: resp["drones"][name]["state"]["position"]["z"] for name in drone_names} - - for _ in range(1000): - resp = world.step(STEP_NS) - - print("\nAfter descend:") - for name in drone_names: - z = resp["drones"][name]["state"]["position"]["z"] - print(f" {name} z={z:.2f}") - assert z > climb_z[name], ( - f"{name} didn't descend: climb={climb_z[name]:.2f}, now={z:.2f}" - ) - - # Phase 3: Get images from FPV camera on each drone - print("\nFPV images:") - for i, name in enumerate(drone_names): - drone = Drone(client, world, name) - images = drone.get_images(camera_id="FPV", image_type_ids=[ImageType.SCENE]) - img = images.get(ImageType.SCENE) - assert img is not None, f"{name}: FPV image is None" - assert img["width"] == 64, f"{name}: expected 64px, got {img['width']}" - assert img["height"] == 64, f"{name}: expected 64px, got {img['height']}" - assert len(img["data"]) == 64 * 64 * 3, f"{name}: wrong data size" - print(f" {name}: FPV {img['width']}x{img['height']} OK") - - print(f"\n✓ {num_drones}-drone lift-land + FPV capture passed") - - finally: - client.disconnect() - - -def test_lift_land_1(): - _run_lift_land(1) - - -def test_lift_land_3(): - _run_lift_land(3) - - -def test_lift_land_10(): - _run_lift_land(10)