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/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/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_cpu_usage.py b/client/python/projectairsim/tests/test_cpu_usage.py new file mode 100644 index 00000000..7c298dd4 --- /dev/null +++ b/client/python/projectairsim/tests/test_cpu_usage.py @@ -0,0 +1,317 @@ +"""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 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 +""" + +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 + +STEP_3MS = 3_000_000 +STEP_10MS = 10_000_000 +STEP_20MS = 20_000_000 + +SIM_CONFIG_PATH = str( + Path(__file__).resolve().parent.parent + / "client" + / "python" + / "example_user_scripts" + / "sim_config" +) +SCENE_CONFIG = "scene_basic_drone.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() + c.connect() + yield c + c.disconnect() + + +@pytest.fixture(scope="module") +def world(client): + w = World( + client, + SCENE_CONFIG, + sim_config_path=SIM_CONFIG_PATH, + 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}) + 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 + + # 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.""" + 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/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/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.