diff --git a/.github/workflows/dimsim-check.yml b/.github/workflows/dimsim-check.yml index be1ca9690d..0ae5dae6a0 100644 --- a/.github/workflows/dimsim-check.yml +++ b/.github/workflows/dimsim-check.yml @@ -47,3 +47,6 @@ jobs: - name: Type-check CLI run: cd cli && deno check cli.ts + + - name: Run Deno tests + run: deno test -A --unstable-net --config cli/deno.json cli evals diff --git a/dimos/robot/unitree/dimsim_connection.py b/dimos/robot/unitree/dimsim_connection.py index bca815ccfb..fcb8d17158 100644 --- a/dimos/robot/unitree/dimsim_connection.py +++ b/dimos/robot/unitree/dimsim_connection.py @@ -16,7 +16,7 @@ import functools from typing import Any -from reactivex import Observable, Subject +from reactivex import Subject from dimos.core.global_config import GlobalConfig from dimos.core.transport import PubSubTransport @@ -51,36 +51,58 @@ class DimSimConnection: def __init__(self, global_config: GlobalConfig) -> None: self._dimsim_process: DimSimProcess = DimSimProcess(global_config) self._odom_transport: PubSubTransport[PoseStamped] = make_transport("/odom", PoseStamped) - self._unsubscribe_odom: Callable[[], None] | None = None + self._lidar_transport: PubSubTransport[PointCloud2] = make_transport("/lidar", PointCloud2) + self._video_transport: PubSubTransport[Image] = make_transport("/color_image", Image) + self._unsubscribes: list[Callable[[], None]] = [] + self._latest_sensor_ts = { + "odom": float("-inf"), + "lidar": float("-inf"), + "video": float("-inf"), + } self._tf = tf_backend()() def start(self) -> None: self._dimsim_process.start() - self._odom_transport.start() - self._unsubscribe_odom = self._odom_transport.subscribe(self._handle_odom) + for transport in ( + self._odom_transport, + self._lidar_transport, + self._video_transport, + ): + transport.start() + self._unsubscribes = [ + self._odom_transport.subscribe(self._handle_odom), + self._lidar_transport.subscribe(self._handle_lidar), + self._video_transport.subscribe(self._handle_video), + ] self._tf.start() def stop(self) -> None: self._tf.stop() - if self._unsubscribe_odom is not None: - self._unsubscribe_odom() - self._odom_transport.stop() + for unsubscribe in self._unsubscribes: + unsubscribe() + self._unsubscribes.clear() + for transport in ( + self._video_transport, + self._lidar_transport, + self._odom_transport, + ): + transport.stop() self._dimsim_process.stop() @functools.cache - def lidar_stream(self) -> Observable[PointCloud2]: + def lidar_stream(self) -> Subject[PointCloud2]: return Subject() @functools.cache - def odom_stream(self) -> Observable[PoseStamped]: + def odom_stream(self) -> Subject[PoseStamped]: return Subject() @functools.cache - def video_stream(self) -> Observable[Image]: + def video_stream(self) -> Subject[Image]: return Subject() @functools.cache - def lowstate_stream(self) -> Observable[Any]: + def lowstate_stream(self) -> Subject[Any]: return Subject() def move(self, twist: Twist, duration: float = 0.0) -> bool: @@ -118,7 +140,25 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: return {} def _handle_odom(self, msg: PoseStamped) -> None: + if not self._is_new_sensor_sample("odom", msg.ts): + return self._tf.publish(*_odom_to_tf(msg)) + self.odom_stream().on_next(msg) + + def _handle_lidar(self, msg: PointCloud2) -> None: + if self._is_new_sensor_sample("lidar", msg.ts): + self.lidar_stream().on_next(msg) + + def _handle_video(self, msg: Image) -> None: + if self._is_new_sensor_sample("video", msg.ts): + self.video_stream().on_next(msg) + + def _is_new_sensor_sample(self, stream: str, timestamp: float) -> bool: + """Reject the same packet when GO2 republishes it on the bridge topic.""" + if timestamp <= self._latest_sensor_ts[stream]: + return False + self._latest_sensor_ts[stream] = timestamp + return True def _odom_to_tf(odom: PoseStamped) -> list[Transform]: diff --git a/dimos/robot/unitree/test_dimsim_connection.py b/dimos/robot/unitree/test_dimsim_connection.py new file mode 100644 index 0000000000..79f8108ba7 --- /dev/null +++ b/dimos/robot/unitree/test_dimsim_connection.py @@ -0,0 +1,115 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from unittest.mock import MagicMock + +import pytest + +from dimos.core.global_config import GlobalConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.robot.unitree.dimsim_connection import DimSimConnection + + +def test_dimsim_camera_info_matches_browser_sensor_frames(): + camera_info = DimSimConnection.camera_info_static + + assert camera_info.width == 640 + assert camera_info.height == 288 + + +@pytest.fixture +def dimsim_connection(mocker): + process = mocker.patch("dimos.robot.unitree.dimsim_connection.DimSimProcess").return_value + transports = [MagicMock(), MagicMock(), MagicMock()] + mocker.patch( + "dimos.robot.unitree.dimsim_connection.make_transport", + side_effect=transports, + ) + tf = MagicMock() + mocker.patch("dimos.robot.unitree.dimsim_connection.tf_backend").return_value.return_value = tf + + callbacks = [] + unsubscribes = [] + for transport in transports: + unsubscribe = MagicMock() + unsubscribes.append(unsubscribe) + + def subscribe(callback, *, _unsubscribe=unsubscribe): + callbacks.append(callback) + return _unsubscribe + + transport.subscribe.side_effect = subscribe + + connection = DimSimConnection(GlobalConfig(simulation="dimsim")) + connection.start() + try: + yield connection, process, transports, tf, callbacks, unsubscribes + finally: + connection.stop() + + +def test_dimsim_connection_relays_sensor_topics(dimsim_connection): + connection, _, _, tf, callbacks, _ = dimsim_connection + received_odom = [] + received_lidar = [] + received_video = [] + connection.odom_stream().subscribe(received_odom.append) + connection.lidar_stream().subscribe(received_lidar.append) + connection.video_stream().subscribe(received_video.append) + + odom = PoseStamped(ts=1.0) + lidar = PointCloud2(ts=1.0) + image = Image(ts=1.0) + callbacks[0](odom) + callbacks[1](lidar) + callbacks[2](image) + + assert received_odom == [odom] + assert received_lidar == [lidar] + assert received_video == [image] + assert tf.publish.call_count == 1 + + +def test_dimsim_connection_drops_republished_sensor_packets(dimsim_connection): + connection, _, _, _, callbacks, _ = dimsim_connection + received_odom = [] + received_lidar = [] + received_video = [] + connection.odom_stream().subscribe(received_odom.append) + connection.lidar_stream().subscribe(received_lidar.append) + connection.video_stream().subscribe(received_video.append) + + odom = PoseStamped(ts=1.0) + lidar = PointCloud2(ts=1.0) + image = Image(ts=1.0) + for callback, message in zip(callbacks, (odom, lidar, image), strict=True): + callback(message) + callback(message) + + assert received_odom == [odom] + assert received_lidar == [lidar] + assert received_video == [image] + + +def test_dimsim_connection_stops_all_resources(dimsim_connection): + connection, process, transports, tf, _, unsubscribes = dimsim_connection + + connection.stop() + + assert [unsubscribe.call_count for unsubscribe in unsubscribes] == [1, 1, 1] + assert [transport.stop.call_count for transport in transports] == [1, 1, 1] + tf.stop.assert_called_once_with() + process.stop.assert_called_once_with() diff --git a/dimos/simulation/dimsim/agent_output_sidecar.py b/dimos/simulation/dimsim/agent_output_sidecar.py new file mode 100644 index 0000000000..db2d161394 --- /dev/null +++ b/dimos/simulation/dimsim/agent_output_sidecar.py @@ -0,0 +1,177 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Read-only NDJSON bridge from the DimOS ``/agent`` stream to DimSim evals.""" + +from __future__ import annotations + +from collections.abc import Callable +import json +import signal +import sys +from threading import Event, RLock +import time +from typing import Any, Protocol, TextIO + +from langchain_core.messages import AIMessage +from langchain_core.messages.base import BaseMessage + +from dimos.core.transport_factory import apply_transport_arg, make_transport + + +class AgentTransport(Protocol): + """Transport behavior used by the sidecar.""" + + def start(self) -> None: ... + + def subscribe(self, callback: Callable[[Any], Any]) -> Callable[[], None]: ... + + def stop(self) -> None: ... + + +def _text_content(content: Any) -> str: + if isinstance(content, str): + return content + if not isinstance(content, list): + return "" + + parts: list[str] = [] + for block in content: + if isinstance(block, str): + parts.append(block) + continue + if not isinstance(block, dict): + continue + block_type = block.get("type") + text = block.get("text") + if block_type in {"text", "output_text"} and isinstance(text, str): + parts.append(text) + return "".join(parts) + + +def serialize_agent_message( + message: BaseMessage, + *, + timestamp: float | None = None, +) -> dict[str, Any] | None: + """Return the eval-safe representation of an AI message.""" + if not isinstance(message, AIMessage): + return None + return { + "type": "agent_output", + "text": _text_content(message.content), + "hasToolCalls": bool(message.tool_calls), + "timestampMs": round((time.time() if timestamp is None else timestamp) * 1000), + } + + +def serialize_agent_idle( + idle: Any, + *, + timestamp: float | None = None, +) -> dict[str, Any] | None: + """Return a typed idle-state event for the eval lifecycle.""" + if not isinstance(idle, bool): + return None + return { + "type": "agent_idle", + "idle": idle, + "timestampMs": round((time.time() if timestamp is None else timestamp) * 1000), + } + + +def run_sidecar( + transport: AgentTransport, + output: TextIO, + stop_event: Event, + idle_transport: AgentTransport | None = None, +) -> None: + """Stream AI messages and optional idle state, owning all cleanup.""" + lock = RLock() + ready = False + pending: list[dict[str, Any]] = [] + + def emit(payload: dict[str, Any]) -> None: + try: + output.write(json.dumps(payload, separators=(",", ":")) + "\n") + output.flush() + except BrokenPipeError: + stop_event.set() + + def on_message(message: Any) -> None: + event = serialize_agent_message(message) + if event is None: + return + with lock: + if ready: + emit(event) + else: + pending.append(event) + + def on_idle(idle: Any) -> None: + event = serialize_agent_idle(idle) + if event is None: + return + with lock: + if ready: + emit(event) + else: + pending.append(event) + + transport.start() + unsubscribe: Callable[[], None] | None = None + unsubscribe_idle: Callable[[], None] | None = None + idle_started = False + try: + unsubscribe = transport.subscribe(on_message) + if idle_transport is not None: + idle_transport.start() + idle_started = True + unsubscribe_idle = idle_transport.subscribe(on_idle) + with lock: + emit({"type": "ready"}) + ready = True + for event in pending: + emit(event) + pending.clear() + stop_event.wait() + finally: + if unsubscribe_idle is not None: + unsubscribe_idle() + if unsubscribe is not None: + unsubscribe() + if idle_started and idle_transport is not None: + idle_transport.stop() + transport.stop() + + +def main() -> None: + apply_transport_arg(sys.argv) + stop_event = Event() + + def request_stop(_signum: int, _frame: Any) -> None: + stop_event.set() + + signal.signal(signal.SIGINT, request_stop) + signal.signal(signal.SIGTERM, request_stop) + run_sidecar( + make_transport("/agent"), + sys.stdout, + stop_event, + make_transport("/agent_idle"), + ) + + +if __name__ == "__main__": + main() diff --git a/dimos/simulation/dimsim/agent_turn_control.py b/dimos/simulation/dimsim/agent_turn_control.py new file mode 100644 index 0000000000..08b398f3ea --- /dev/null +++ b/dimos/simulation/dimsim/agent_turn_control.py @@ -0,0 +1,78 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Private DimSim control publisher for terminating an active eval turn.""" + +from __future__ import annotations + +import argparse +import sys +from typing import Any, Protocol + +from dimos.core.transport_factory import apply_transport_arg, make_transport + +EVAL_TURN_CONTROL_TOPIC = "/dimsim_eval_turn_control" +CANCEL_ACTIVE_TURN = "cancel_active_turn" + + +class ControlTransport(Protocol): + """Transport behavior needed by the one-shot control publisher.""" + + def start(self) -> None: ... + + def publish(self, message: Any) -> None: ... + + def stop(self) -> None: ... + + +def publish_turn_cancellation( + transport: ControlTransport, + run_id: str, +) -> None: + """Publish one run-correlated request and own the transport lifecycle.""" + if not run_id: + raise ValueError("run_id must not be empty") + + transport.start() + try: + transport.publish( + { + "type": CANCEL_ACTIVE_TURN, + "runId": run_id, + } + ) + finally: + transport.stop() + + +def main() -> None: + apply_transport_arg(sys.argv) + parser = argparse.ArgumentParser( + description="Terminate the currently active DimSim evaluation turn.", + ) + parser.add_argument("run_id") + parser.add_argument( + "--transport", + choices=("lcm", "zenoh"), + help=argparse.SUPPRESS, + ) + args = parser.parse_args() + publish_turn_cancellation( + make_transport(EVAL_TURN_CONTROL_TOPIC), + args.run_id, + ) + + +if __name__ == "__main__": + main() diff --git a/dimos/simulation/dimsim/agentic_blueprint.py b/dimos/simulation/dimsim/agentic_blueprint.py new file mode 100644 index 0000000000..bd86c59096 --- /dev/null +++ b/dimos/simulation/dimsim/agentic_blueprint.py @@ -0,0 +1,78 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DimSim-specific Go2 agent composition. + +Run it through the normal DimOS CLI without adding a generated core registry +entry: + + uv run python -m dimos.simulation.dimsim.agentic_blueprint \ + --simulation dimsim run unitree-go2-agentic-dimsim +""" + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.core.coordination.blueprints import autoconnect +from dimos.perception.perceive_loop_skill import PerceiveLoopSkill +from dimos.perception.spatial_perception import SpatialMemory +from dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic import ( + unitree_go2_agentic, +) +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.unitree_skill_container import UnitreeSkillContainer +from dimos.simulation.dimsim.go2_connection import DimSimGO2Connection +from dimos.simulation.dimsim.mcp_client import DimSimMcpClient +from dimos.simulation.dimsim.navigation_skill_container import ( + DimSimNavigationSkillContainer, +) +from dimos.simulation.dimsim.perceive_loop_skill import DimSimPerceiveLoopSkill +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemory +from dimos.simulation.dimsim.unitree_skill_container import DimSimUnitreeSkillContainer + +unitree_go2_agentic_dimsim = autoconnect( + unitree_go2_agentic.disabled_modules( + UnitreeSkillContainer, + McpClient, + NavigationSkillContainer, + PerceiveLoopSkill, + SpatialMemory, + GO2Connection, + ), + DimSimGO2Connection.blueprint(), + DimSimMcpClient.blueprint(), + DimSimNavigationSkillContainer.blueprint(), + DimSimPerceiveLoopSkill.blueprint(), + DimSimSpatialMemory.blueprint(), + DimSimUnitreeSkillContainer.blueprint(), +).global_config(simulation="dimsim") + +_BLUEPRINT_NAME = "unitree-go2-agentic-dimsim" + + +def main() -> None: + from dimos.robot.all_blueprints import all_blueprints + + all_blueprints[_BLUEPRINT_NAME] = ( + "dimos.simulation.dimsim.agentic_blueprint:unitree_go2_agentic_dimsim" + ) + + # Import after registering so get_all_blueprints computes suggestions from + # the augmented registry and the standard CLI still owns lifecycle/logging. + from dimos.robot.cli.dimos import cli_main + + cli_main() + + +if __name__ == "__main__": + main() diff --git a/dimos/simulation/dimsim/go2_connection.py b/dimos/simulation/dimsim/go2_connection.py new file mode 100644 index 0000000000..fea7517347 --- /dev/null +++ b/dimos/simulation/dimsim/go2_connection.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DimSim-specific Go2 camera observation semantics.""" + +from __future__ import annotations + +from threading import Condition +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.unitree.go2.connection import GO2Connection + +_FRESH_FRAME_TIMEOUT_SEC = 5.0 + + +class DimSimGO2Connection(GO2Connection): + """Go2 connection whose observation waits for a post-request DimSim frame.""" + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._init_fresh_frame_state() + + def _init_fresh_frame_state(self) -> None: + self._fresh_frame_condition = Condition() + self._latest_dimsim_frame: Image | None = None + + @rpc + def start(self) -> None: + super().start() + self.register_disposable( + Disposable(self.color_image.subscribe(self._on_dimsim_frame)), + ) + + def _on_dimsim_frame(self, image: Image) -> None: + with self._fresh_frame_condition: + if self._latest_dimsim_frame is not None and image.ts <= self._latest_dimsim_frame.ts: + return + self._latest_dimsim_frame = image + self._fresh_frame_condition.notify_all() + + def _wait_for_frame_after(self, timestamp: float, timeout: float) -> Image | None: + deadline = time.monotonic() + timeout + with self._fresh_frame_condition: + while self._latest_dimsim_frame is None or self._latest_dimsim_frame.ts <= timestamp: + remaining = deadline - time.monotonic() + if remaining <= 0: + return None + self._fresh_frame_condition.wait(remaining) + return self._latest_dimsim_frame + + @skill + def observe(self) -> Image | None: + """Return a camera frame captured after this observation request. + + DimSim rendering and perception can lag behind robot movement. Waiting + for a frame with a newer sensor timestamp prevents the agent from + steering from a camera pose that predates its latest movement. + Returns None if no fresh frame arrives within five seconds. + """ + return self._wait_for_frame_after( + time.time(), + _FRESH_FRAME_TIMEOUT_SEC, + ) diff --git a/dimos/simulation/dimsim/mcp_client.py b/dimos/simulation/dimsim/mcp_client.py new file mode 100644 index 0000000000..29024ae748 --- /dev/null +++ b/dimos/simulation/dimsim/mcp_client.py @@ -0,0 +1,414 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DimSim-specific MCP client behavior.""" + +from queue import Empty +from threading import Event, Lock, Thread +from typing import Any + +from langchain.agents import create_agent +from langchain.tools import ToolRuntime +from langchain_core.messages import HumanMessage, ToolMessage +from langchain_core.tools import StructuredTool +from langchain_openai import ChatOpenAI +from langgraph.types import Command +from reactivex.disposable import Disposable + +from dimos.agents.mcp import tool_stream +from dimos.agents.mcp.mcp_client import McpClient, _init_model +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.rpc_client import RPCClient +from dimos.core.transport_factory import make_transport +from dimos.simulation.dimsim.agent_turn_control import ( + CANCEL_ACTIVE_TURN, + EVAL_TURN_CONTROL_TOPIC, +) +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemorySpec + +_EVAL_IDLE_HEARTBEAT_SEC = 0.25 +_EVAL_TURN_ENDED_TEXT = "The DimSim evaluation has ended. End this agent turn now." + + +class DimSimMcpClient(McpClient): + """Keep camera observations in the tool call that requested them.""" + + _spatial_memory: DimSimSpatialMemorySpec + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._eval_idle_lock = Lock() + self._eval_idle = True + self._live_update_lock = Lock() + self._processing_turn = False + self._live_updates: list[HumanMessage] = [] + self._eval_turn_cancel = Event() + self._eval_agent_tools: list[StructuredTool] = [] + self._eval_control_transport: Any | None = None + self._eval_control_unsubscribe: Any | None = None + self._eval_idle_stop = Event() + self._eval_idle_thread = Thread( + target=self._eval_idle_heartbeat, + name=f"{self.__class__.__name__}-idle-heartbeat", + daemon=True, + ) + + @rpc + def start(self) -> None: + Module.start(self) + self.register_disposable( + Disposable(self.human_input.subscribe(self._queue_human_input)), + ) + self._tool_stream_cleanup = tool_stream.subscribe(self._on_tool_stream_message) + self._eval_control_transport = make_transport(EVAL_TURN_CONTROL_TOPIC) + self._eval_control_unsubscribe = self._eval_control_transport.subscribe( + self._on_eval_turn_control, + ) + if not self._eval_idle_thread.is_alive(): + self._eval_idle_thread.start() + + def _set_eval_idle(self, idle: bool) -> None: + with self._eval_idle_lock: + self._eval_idle = idle + self.agent_idle.publish(idle) + + def _eval_idle_heartbeat(self) -> None: + while not self._eval_idle_stop.wait(_EVAL_IDLE_HEARTBEAT_SEC): + with self._eval_idle_lock: + idle = self._eval_idle + self.agent_idle.publish(idle) + + def _process_message(self, state_graph: Any, message: Any) -> None: + with self._live_update_lock: + self._eval_turn_cancel.clear() + self._processing_turn = True + self._set_eval_idle(False) + try: + super()._process_message(state_graph, message) + finally: + with self._live_update_lock: + self._processing_turn = False + self._eval_turn_cancel.clear() + remaining_updates = self._live_updates + self._live_updates = [] + for tool in self._eval_agent_tools: + tool.return_direct = False + for update in remaining_updates: + self._message_queue.put(update) + self._set_eval_idle(self._message_queue.empty()) + + @rpc + def stop(self) -> None: + if self._eval_control_unsubscribe is not None: + self._eval_control_unsubscribe() + self._eval_control_unsubscribe = None + if self._eval_control_transport is not None: + self._eval_control_transport.stop() + self._eval_control_transport = None + self._eval_idle_stop.set() + if self._eval_idle_thread.is_alive(): + self._eval_idle_thread.join(timeout=1.0) + super().stop() + + def _on_eval_turn_control(self, message: Any) -> None: + if ( + not isinstance(message, dict) + or message.get("type") != CANCEL_ACTIVE_TURN + or not isinstance(message.get("runId"), str) + or not message["runId"] + ): + return + with self._live_update_lock: + # A late packet from a completed run must not poison the next task. + if self._processing_turn: + self._eval_turn_cancel.set() + + def _cancelled_tool_result( + self, + tool: StructuredTool, + result: str = "", + ) -> str | None: + if not self._eval_turn_cancel.is_set(): + return None + tool.return_direct = True + return self._append_update_text(result, _EVAL_TURN_ENDED_TEXT) + + def _queue_human_input(self, string: str) -> None: + # Each DimSim task follows an authoritative simulator reset and must + # not inherit model messages, queued tool updates, or semantic-map + # viewpoints from a previous run. The lock also prevents resetting + # history while the prior agent turn is still streaming. + with self._lock: + self._spatial_memory.clear_eval_memory() + self._history.clear() + with self._live_update_lock: + self._live_updates.clear() + while True: + try: + self._message_queue.get_nowait() + except Empty: + break + self._message_queue.put(HumanMessage(content=string)) + + def _enqueue_agent_update(self, message: HumanMessage) -> None: + """Deliver background events into the active tool loop when possible. + + LangGraph runs one agent turn until the model stops calling tools. + Upstream background notifications are queued for the *next* turn, which + means a moving robot can execute several more actions before learning + that a lookout matched. A DimSim evaluation needs the match to be + visible at the next tool boundary so perception and pose stay + correlated. + """ + with self._live_update_lock: + if self._processing_turn: + self._live_updates.append(message) + return + self._message_queue.put(message) + + def _take_live_update_text(self) -> str: + with self._live_update_lock: + updates = self._live_updates + self._live_updates = [] + return "\n".join(str(update.content) for update in updates) + + @staticmethod + def _append_update_text(text: str, update: str) -> str: + if not update: + return text + if not text: + return update + return f"{text}\n\n{update}" + + def _on_tool_stream_message(self, msg: dict[str, Any]) -> None: + method = msg.get("method") + params = msg.get("params") or {} + if method == tool_stream.NOTIFICATIONS_PROGRESS_METHOD: + text = params.get("message") or "" + tool_name = (params.get("_meta") or {}).get("tool_name") or "tool" + elif method == tool_stream.NOTIFICATIONS_MESSAGE_METHOD: + text = params.get("data") or "" + tool_name = params.get("logger") or "tool" + else: + return + if text: + self._enqueue_agent_update( + HumanMessage(content=f"[tool:{tool_name}] {text}"), + ) + + @rpc + def dispatch_continuation( + self, + continuation: dict[str, Any], + continuation_context: dict[str, Any], + ) -> None: + """Execute a lookout continuation and surface its result immediately.""" + tool_name = continuation.get("tool") + if not tool_name: + self._enqueue_agent_update( + HumanMessage( + content=f"Continuation failed: missing 'tool' key in {continuation}", + ), + ) + return + + if tool_name not in self._tool_registry: + self._enqueue_agent_update( + HumanMessage(content=f"Continuation failed: tool '{tool_name}' not found"), + ) + return + + tool_args: dict[str, Any] = dict(continuation.get("args", {})) + for key, value in tool_args.items(): + if isinstance(value, str) and value.startswith("$"): + context_key = value[1:] + if context_key in continuation_context: + tool_args[key] = continuation_context[context_key] + + try: + result = self._mcp_tool_call(tool_name, tool_args) + content = result.get("content", []) + parts = [item.get("text", "") for item in content if item.get("type") == "text"] + text = "\n".join(parts) + except Exception as exc: + self._enqueue_agent_update( + HumanMessage( + content=f"Continuation '{tool_name}' failed with error: {exc}", + ), + ) + return + + label = continuation_context.get("label", "unknown") + self._enqueue_agent_update( + HumanMessage( + content=( + f"Automatically executed '{tool_name}' as a continuation of " + f"lookout detection (detected: {label}). Result: " + f"{text or 'started'}" + ), + ), + ) + + @rpc + def on_system_modules(self, _modules: list[RPCClient]) -> None: + self._eval_agent_tools = [] + tools = self._fetch_tools() + model: Any + + if self.config.model_fixture is not None: + from dimos.agents.testing.mock_model import MockModel + + model = MockModel(json_path=self.config.model_fixture) + else: + model = _init_model(self.config.model) + # The configured coding-plan proxy exposes the Responses API as an + # event stream. Its non-streaming response is not OpenAI-compatible, + # while LangChain correctly merges the streamed text and tool-call + # events into complete AIMessages. Keep this compatibility setting + # local to the DimSim adapter. + if isinstance(model, ChatOpenAI) and model.use_responses_api: + model = model.model_copy(update={"streaming": True}) + + with self._lock: + self._state_graph = self._create_eval_agent(model, tools) + if not self._thread.is_alive(): + self._thread.start() + self._set_eval_idle(self._message_queue.empty()) + + def _create_eval_agent( + self, + model: Any, + tools: list[StructuredTool], + ) -> Any: + # create_agent only compiles its tools -> END destination when at least + # one tool is direct-return at construction time. Include that dormant + # edge, then restore normal looping before any turn can execute. A + # cancellation request activates it for exactly one tool boundary. + direct_edge_tool = tools[0] if tools else None + if direct_edge_tool is not None: + direct_edge_tool.return_direct = True + try: + return create_agent( + model=model, + tools=tools, + system_prompt=self.config.system_prompt, + ) + finally: + if direct_edge_tool is not None: + direct_edge_tool.return_direct = False + + def _mcp_tool_to_langchain(self, mcp_tool: dict[str, Any]) -> StructuredTool: + name = mcp_tool["name"] + description = mcp_tool.get("description", "") + input_schema = mcp_tool.get( + "inputSchema", + {"type": "object", "properties": {}}, + ) + + if name != "observe": + upstream_tool = super()._mcp_tool_to_langchain(mcp_tool) + + def call_tool(**kwargs: Any) -> str: + cancelled = self._cancelled_tool_result(wrapped_tool) + if cancelled is not None: + return cancelled + if upstream_tool.func is None: + raise RuntimeError(f"MCP tool '{name}' has no callable function") + text = upstream_tool.func(**kwargs) + text = self._append_update_text( + text, + self._take_live_update_text(), + ) + return self._cancelled_tool_result(wrapped_tool, text) or text + + wrapped_tool = StructuredTool( + name=name, + description=description, + func=call_tool, + args_schema=input_schema, + ) + self._eval_agent_tools.append(wrapped_tool) + return wrapped_tool + + def call_observe(runtime: ToolRuntime, **kwargs: Any) -> str | Command[Any]: + cancelled = self._cancelled_tool_result(wrapped_observe) + if cancelled is not None: + if runtime.tool_call_id is None: + raise RuntimeError("observe requires an active tool call") + return Command( + update={ + "messages": [ + ToolMessage( + content=cancelled, + tool_call_id=runtime.tool_call_id, + ) + ] + } + ) + result = self._mcp_tool_call(name, kwargs) + content = result.get("content", []) + text = "\n".join(item.get("text", "") for item in content if item.get("type") == "text") + images = [item for item in content if item.get("type") != "text"] + text = self._append_update_text( + text, + self._take_live_update_text(), + ) + if runtime.tool_call_id is None: + raise RuntimeError("observe requires an active tool call") + + cancelled = self._cancelled_tool_result(wrapped_observe, text) + if cancelled is not None: + return Command( + update={ + "messages": [ + ToolMessage( + content=cancelled, + tool_call_id=runtime.tool_call_id, + ) + ] + } + ) + + if not images: + return text + + return Command( + update={ + "messages": [ + ToolMessage( + content=text or "Current camera frame attached.", + tool_call_id=runtime.tool_call_id, + ), + HumanMessage( + content=[ + { + "type": "text", + "text": "This is the current camera frame returned by observe.", + }, + *images, + ] + ), + ] + } + ) + + wrapped_observe = StructuredTool( + name=name, + description=description, + func=call_observe, + args_schema=input_schema, + ) + self._eval_agent_tools.append(wrapped_observe) + return wrapped_observe diff --git a/dimos/simulation/dimsim/navigation_skill_container.py b/dimos/simulation/dimsim/navigation_skill_container.py new file mode 100644 index 0000000000..d6b5dfaf6e --- /dev/null +++ b/dimos/simulation/dimsim/navigation_skill_container.py @@ -0,0 +1,81 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DimSim-specific semantic-viewpoint navigation semantics.""" + +import math + +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemorySpec + +_CURRENT_VIEWPOINT_THRESHOLD_M = 0.75 + + +class DimSimNavigationSkillContainer(NavigationSkillContainer): + """Describe semantic-map hits as viewpoints rather than object poses.""" + + _spatial_memory: DimSimSpatialMemorySpec + + def _navigate_using_semantic_map(self, query: str) -> str: + detection_viewpoint = self._spatial_memory.query_detection_viewpoint(query) + if detection_viewpoint is not None: + return self._navigate_to_viewpoint( + detection_viewpoint, + current_message=( + f"The camera viewpoint where the lookout detected '{query}' " + "is the current camera viewpoint" + ), + found_message=(f"Found the camera viewpoint where the lookout detected '{query}'"), + ) + + results = self._spatial_memory.query_by_text(query) + if not results: + return f"No matching location found in semantic map for '{query}'" + + goal_pose = self._get_goal_pose_from_result(results[0]) + if goal_pose is None: + return f"Found a result for '{query}' but it didn't have a reliable position." + + return self._navigate_to_viewpoint( + goal_pose, + current_message=( + f"The semantic-map match for '{query}' is the current camera viewpoint" + ), + found_message=f"Found a prior camera viewpoint matching '{query}'", + ) + + def _navigate_to_viewpoint( + self, + goal_pose: PoseStamped, + *, + current_message: str, + found_message: str, + ) -> str: + if self._latest_odom is not None: + viewpoint_distance = math.hypot( + goal_pose.position.x - self._latest_odom.position.x, + goal_pose.position.y - self._latest_odom.position.y, + ) + if viewpoint_distance <= _CURRENT_VIEWPOINT_THRESHOLD_M: + return ( + f"{current_message}, not the object's position. No navigation " + "was started. Observe the current frame and approach the visible " + "object with bounded local movement." + ) + + return self._navigate_to( + goal_pose, + f"{found_message}, not a confirmed object position", + ) diff --git a/dimos/simulation/dimsim/perceive_loop_skill.py b/dimos/simulation/dimsim/perceive_loop_skill.py new file mode 100644 index 0000000000..eb66c022fc --- /dev/null +++ b/dimos/simulation/dimsim/perceive_loop_skill.py @@ -0,0 +1,204 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DimSim-specific continuous visual detection semantics.""" + +from collections import deque +import json +import os +from typing import Any + +import cv2 +from dimos_lcm.std_msgs import Bool +import numpy as np +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.perception.perceive_loop_skill import ( + PerceiveLoopSkill, + _write_debug_image, + logger, +) +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemorySpec + +_ODOM_HISTORY_SIZE = 2048 +_MAX_FRAME_POSE_DELTA_SEC = 0.5 +_MIN_VISIBLE_LUMA = 24 +_MIN_VISIBLE_PIXEL_FRACTION = 0.3 + + +class DimSimPerceiveLoopSkill(PerceiveLoopSkill): + """Pass individual object descriptions to the configured detector. + + ``look_out_for`` accepts a list because one lookout can watch for several + things. Moondream's detection API accepts one object description at a + time. The upstream implementation serializes the complete Python tuple as + JSON and sends text such as ``["bathtub"]`` as the object name. That query + is ambiguous and produced false matches during natural DimSim searches. + + This replacement preserves the existing agent-visible tools and + notification behavior while adapting the list-valued tool input to the + detector's scalar query contract. + """ + + odom: In[PoseStamped] + stop_movement: Out[Bool] + + _spatial_memory: DimSimSpatialMemorySpec + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._odom_history: deque[PoseStamped] = deque(maxlen=_ODOM_HISTORY_SIZE) + + @rpc + def start(self) -> None: + super().start() + self.register_disposable( + Disposable(self.odom.subscribe(self._on_odom)), + ) + + def _on_odom(self, pose: PoseStamped) -> None: + with self._lock: + if self._odom_history and pose.ts <= self._odom_history[-1].ts: + return + self._odom_history.append(pose) + + def _pose_for_frame(self, frame_ts: float) -> PoseStamped | None: + with self._lock: + if not self._odom_history: + return None + pose = min( + self._odom_history, + key=lambda candidate: abs(candidate.ts - frame_ts), + ) + if abs(pose.ts - frame_ts) > _MAX_FRAME_POSE_DELTA_SEC: + return None + return pose + + def _query_active_lookout( + self, + image: Image, + descriptions: tuple[str, ...], + ) -> ImageDetections2D[Detection2DBBox]: + combined: ImageDetections2D[Detection2DBBox] = ImageDetections2D(image) + for description in descriptions: + result = self._vl_model.query_detections(image, description) + combined.detections.extend(result.detections) + return combined + + @staticmethod + def _visible_pixel_fraction(image: Image) -> float: + frame = image.to_opencv() + gray = frame if frame.ndim == 2 else cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + if gray.size == 0: + return 0.0 + visible = np.count_nonzero(gray > _MIN_VISIBLE_LUMA) + return visible / gray.size + + def _on_image(self, image: Image) -> None: + with self._lock: + if not self._active_lookout: + return + active_lookout = self._active_lookout + active_lookout_str = json.dumps(active_lookout) + + # The Go2 camera is mounted ahead of the robot's body collider. When + # the body stops close to a thin wall, the camera near plane can cross + # that wall and render a mostly black frame. Open-vocabulary detectors + # can hallucinate confident boxes on such invalid input. Keep the + # lookout active and wait for a usable view instead of turning a render + # artifact into an object discovery. + visible_fraction = self._visible_pixel_fraction(image) + if visible_fraction < _MIN_VISIBLE_PIXEL_FRACTION: + logger.info( + "Skipping low-visibility lookout frame", + lookout=active_lookout_str, + frame_ts=image.ts, + visible_fraction=visible_fraction, + ) + return + + detections = self._query_active_lookout(image, active_lookout) + if not detections: + return + + capture_pose = self._pose_for_frame(image.ts) + if os.environ.get("DEBUG"): + _write_debug_image(image, detections) + + with self._lock: + if not self._active_lookout: + return + if self._lookout_subscription is not None: + self._lookout_subscription.dispose() + self._lookout_subscription = None + self._active_lookout = () + then = self._then + self._then = None + self._vl_model.stop() + self._model_started = False + + # The detector can take several seconds while frontier exploration + # keeps moving. Stop all navigation immediately, then remember the + # odometry pose belonging to the matched frame rather than the pose at + # detector completion. A later navigate_with_text call can therefore + # return to the real observed viewpoint. + self.stop_movement.publish(Bool(data=True)) + if capture_pose is not None: + self._spatial_memory.record_detection_viewpoint( + list(active_lookout), + capture_pose, + image.ts, + ) + logger.info( + "Recorded lookout capture viewpoint", + lookout=active_lookout_str, + frame_ts=image.ts, + pose=capture_pose, + ) + else: + logger.warning( + "Lookout matched without correlated odometry", + lookout=active_lookout_str, + frame_ts=image.ts, + ) + + if then is None: + self.tool_update( + "look_out_for", + f"Found a match for {active_lookout_str}. Please announce audibly.", + ) + self.stop_tool("look_out_for") + return + + self.stop_tool("look_out_for") + + best = max(detections.detections, key=lambda detection: detection.bbox_2d_volume()) + continuation_context: dict[str, Any] = { + "bbox": list(best.bbox), + "label": best.name, + "image": image.to_base64(quality=70), + } + logger.info( + "Lookout matched, dispatching continuation", + lookout=active_lookout_str, + continuation=then, + detection=continuation_context, + ) + self._agent_spec.dispatch_continuation(then, continuation_context) diff --git a/dimos/simulation/dimsim/spatial_memory.py b/dimos/simulation/dimsim/spatial_memory.py new file mode 100644 index 0000000000..83382eddc0 --- /dev/null +++ b/dimos/simulation/dimsim/spatial_memory.py @@ -0,0 +1,129 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Cold spatial memory for independently reset DimSim evaluation tasks.""" + +import re +from threading import RLock +from typing import Any, Protocol + +from dimos.core.core import rpc +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.perception.spatial_memory_spec import SpatialMemorySpec +from dimos.perception.spatial_perception import SpatialMemory + + +class DimSimSpatialMemorySpec(SpatialMemorySpec, Protocol): + def clear_eval_memory(self) -> int: ... + + def eval_memory_generation(self) -> int: ... + + def record_detection_viewpoint( + self, + descriptions: list[str], + pose: PoseStamped, + frame_ts: float, + ) -> None: ... + + def query_detection_viewpoint(self, query: str) -> PoseStamped | None: ... + + +class DimSimSpatialMemory(SpatialMemory): + """Spatial memory that can be cleared at a DimSim task boundary.""" + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._eval_memory_lock = RLock() + self._eval_memory_generation = 0 + self._detection_viewpoints: dict[str, tuple[float, PoseStamped]] = {} + + def _process_frame(self) -> None: + # A periodic embedding already in flight must finish before the reset + # deletes its vector, otherwise a pre-reset viewpoint can be inserted + # after clear_eval_memory() returns. + with self._eval_memory_lock: + super()._process_frame() + + @rpc + def clear_eval_memory(self) -> int: + """Remove observations and tags left by an earlier simulator pose. + + DimSim resets the robot without restarting the process. Semantic-map + entries are camera viewpoints, so retaining entries across a reset can + send a new task toward an unrelated pose from the previous run. + """ + with self._eval_memory_lock: + image_collection = self.vector_db.image_collection + image_ids = image_collection.get(include=[]).get("ids", []) + if image_ids: + image_collection.delete(ids=image_ids) + + location_collection = self.vector_db.location_collection + location_ids = location_collection.get(include=[]).get("ids", []) + if location_ids: + location_collection.delete(ids=location_ids) + + if self._visual_memory is not None: + self._visual_memory.clear() + + self.robot_locations.clear() + self._latest_video_frame = None + self.last_position = None + self.last_record_time = None + self.frame_count = 0 + self.stored_frame_count = 0 + self._detection_viewpoints.clear() + self._eval_memory_generation += 1 + return len(image_ids) + len(location_ids) + + @rpc + def eval_memory_generation(self) -> int: + """Return the current independently-reset eval memory generation.""" + with self._eval_memory_lock: + return self._eval_memory_generation + + @rpc + def record_detection_viewpoint( + self, + descriptions: list[str], + pose: PoseStamped, + frame_ts: float, + ) -> None: + """Remember where a positive lookout frame was actually captured.""" + with self._eval_memory_lock: + for description in descriptions: + key = _normalize_detection_query(description) + if key: + self._detection_viewpoints[key] = (frame_ts, pose) + + @rpc + def query_detection_viewpoint(self, query: str) -> PoseStamped | None: + """Return the newest exact lookout viewpoint matching ``query``.""" + normalized_query = _normalize_detection_query(query) + if not normalized_query: + return None + + with self._eval_memory_lock: + matches = [ + value + for description, value in self._detection_viewpoints.items() + if description in normalized_query or normalized_query in description + ] + if not matches: + return None + return max(matches, key=lambda match: match[0])[1] + + +def _normalize_detection_query(query: str) -> str: + return " ".join(re.findall(r"[a-z0-9]+", query.lower())) diff --git a/dimos/simulation/dimsim/test_agent_output_sidecar.py b/dimos/simulation/dimsim/test_agent_output_sidecar.py new file mode 100644 index 0000000000..9f74bd16b8 --- /dev/null +++ b/dimos/simulation/dimsim/test_agent_output_sidecar.py @@ -0,0 +1,155 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from io import StringIO +import json +from threading import Event +from typing import Any + +from langchain_core.messages import AIMessage, HumanMessage + +from dimos.simulation.dimsim.agent_output_sidecar import ( + run_sidecar, + serialize_agent_idle, + serialize_agent_message, +) + + +class FakeTransport: + def __init__(self, messages: list[Any]) -> None: + self.messages = messages + self.started = False + self.unsubscribed = False + self.stopped = False + + def start(self) -> None: + self.started = True + + def subscribe(self, callback: Any) -> Any: + for message in self.messages: + callback(message) + + def unsubscribe() -> None: + self.unsubscribed = True + + return unsubscribe + + def stop(self) -> None: + self.stopped = True + + +def test_serialize_agent_message_ignores_non_ai_messages() -> None: + result = serialize_agent_message(HumanMessage(content="FOUND_BATHTUB")) + + assert result is None + + +def test_serialize_agent_message_extracts_responses_text_and_tool_metadata() -> None: + message = AIMessage( + content=[ + {"type": "reasoning", "summary": "private"}, + {"type": "output_text", "text": "FOUND_"}, + {"type": "text", "text": "BATHTUB"}, + ], + tool_calls=[ + { + "name": "speak", + "args": {"text": "FOUND_BATHTUB"}, + "id": "tool-1", + "type": "tool_call", + } + ], + ) + + result = serialize_agent_message(message, timestamp=12.345) + + assert result == { + "type": "agent_output", + "text": "FOUND_BATHTUB", + "hasToolCalls": True, + "timestampMs": 12345, + } + + +def test_serialize_agent_idle_accepts_only_boolean_state() -> None: + assert serialize_agent_idle(True, timestamp=12.345) == { + "type": "agent_idle", + "idle": True, + "timestampMs": 12345, + } + assert serialize_agent_idle("true", timestamp=12.345) is None + + +def test_run_sidecar_signals_ready_before_buffered_output_and_cleans_up() -> None: + transport = FakeTransport( + [ + HumanMessage(content="ignored"), + AIMessage(content="FOUND_BATHTUB"), + ] + ) + output = StringIO() + stop_event = Event() + stop_event.set() + + run_sidecar(transport, output, stop_event) + + lines = [json.loads(line) for line in output.getvalue().splitlines()] + assert lines == [ + {"type": "ready"}, + { + "type": "agent_output", + "text": "FOUND_BATHTUB", + "hasToolCalls": False, + "timestampMs": lines[1]["timestampMs"], + }, + ] + assert transport.started + assert transport.unsubscribed + assert transport.stopped + + +def test_run_sidecar_streams_idle_state_and_cleans_up_both_transports() -> None: + agent_transport = FakeTransport([]) + idle_transport = FakeTransport([False, True]) + output = StringIO() + stop_event = Event() + stop_event.set() + + run_sidecar( + agent_transport, + output, + stop_event, + idle_transport=idle_transport, + ) + + lines = [json.loads(line) for line in output.getvalue().splitlines()] + assert lines == [ + {"type": "ready"}, + { + "type": "agent_idle", + "idle": False, + "timestampMs": lines[1]["timestampMs"], + }, + { + "type": "agent_idle", + "idle": True, + "timestampMs": lines[2]["timestampMs"], + }, + ] + assert agent_transport.unsubscribed + assert agent_transport.stopped + assert idle_transport.unsubscribed + assert idle_transport.stopped diff --git a/dimos/simulation/dimsim/test_agent_turn_control.py b/dimos/simulation/dimsim/test_agent_turn_control.py new file mode 100644 index 0000000000..75d4fcd83d --- /dev/null +++ b/dimos/simulation/dimsim/test_agent_turn_control.py @@ -0,0 +1,74 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import Any + +import pytest + +from dimos.simulation.dimsim.agent_turn_control import ( + CANCEL_ACTIVE_TURN, + publish_turn_cancellation, +) + + +class FakeControlTransport: + def __init__(self, publish_error: Exception | None = None) -> None: + self.publish_error = publish_error + self.started = False + self.stopped = False + self.messages: list[Any] = [] + + def start(self) -> None: + self.started = True + + def publish(self, message: Any) -> None: + self.messages.append(message) + if self.publish_error is not None: + raise self.publish_error + + def stop(self) -> None: + self.stopped = True + + +def test_publish_turn_cancellation_is_correlated_and_cleans_up() -> None: + transport = FakeControlTransport() + + publish_turn_cancellation(transport, "run-123") + + assert transport.messages == [ + { + "type": CANCEL_ACTIVE_TURN, + "runId": "run-123", + } + ] + assert transport.started + assert transport.stopped + + +def test_publish_turn_cancellation_cleans_up_after_publish_failure() -> None: + transport = FakeControlTransport(RuntimeError("publish failed")) + + with pytest.raises(RuntimeError, match="publish failed"): + publish_turn_cancellation(transport, "run-123") + + assert transport.stopped + + +def test_publish_turn_cancellation_rejects_empty_run_id() -> None: + transport = FakeControlTransport() + + with pytest.raises(ValueError, match="run_id"): + publish_turn_cancellation(transport, "") + + assert not transport.started diff --git a/dimos/simulation/dimsim/test_go2_connection.py b/dimos/simulation/dimsim/test_go2_connection.py new file mode 100644 index 0000000000..8a6d327731 --- /dev/null +++ b/dimos/simulation/dimsim/test_go2_connection.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Thread +import time + +from dimos.msgs.sensor_msgs.Image import Image +from dimos.simulation.dimsim.go2_connection import DimSimGO2Connection + + +def _connection_without_runtime() -> DimSimGO2Connection: + connection = object.__new__(DimSimGO2Connection) + connection._init_fresh_frame_state() + return connection + + +def test_wait_for_frame_rejects_pre_request_image() -> None: + connection = _connection_without_runtime() + stale = Image(ts=99.0) + fresh = Image(ts=101.0) + connection._on_dimsim_frame(stale) + + publisher = Thread( + target=lambda: ( + time.sleep(0.02), + connection._on_dimsim_frame(fresh), + ) + ) + publisher.start() + try: + assert connection._wait_for_frame_after(100.0, timeout=0.5) is fresh + finally: + publisher.join() + + +def test_wait_for_frame_rejects_late_delivery_with_old_sensor_timestamp() -> None: + connection = _connection_without_runtime() + delayed_stale = Image(ts=99.0) + fresh = Image(ts=101.0) + + def publish_frames() -> None: + time.sleep(0.01) + connection._on_dimsim_frame(delayed_stale) + time.sleep(0.01) + connection._on_dimsim_frame(fresh) + + publisher = Thread(target=publish_frames) + publisher.start() + try: + assert connection._wait_for_frame_after(100.0, timeout=0.5) is fresh + finally: + publisher.join() + + +def test_wait_for_frame_times_out_without_post_request_image() -> None: + connection = _connection_without_runtime() + connection._on_dimsim_frame(Image(ts=99.0)) + + assert connection._wait_for_frame_after(100.0, timeout=0.01) is None diff --git a/dimos/simulation/dimsim/test_mcp_client.py b/dimos/simulation/dimsim/test_mcp_client.py new file mode 100644 index 0000000000..b8968979dd --- /dev/null +++ b/dimos/simulation/dimsim/test_mcp_client.py @@ -0,0 +1,333 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from langchain_core.messages import AIMessage, HumanMessage, ToolMessage +from langchain_openai import ChatOpenAI +from langgraph.graph import END, START, MessagesState, StateGraph +from langgraph.prebuilt import ToolNode +import pytest + +from dimos.agents.testing.mock_model import MockModel +from dimos.simulation.dimsim.agent_turn_control import CANCEL_ACTIVE_TURN +from dimos.simulation.dimsim.mcp_client import DimSimMcpClient + + +def _mcp_tool(name: str) -> dict: + return { + "name": name, + "description": f"{name} description", + "inputSchema": {"type": "object", "properties": {}}, + } + + +@pytest.fixture +def client() -> DimSimMcpClient: + mcp_client = DimSimMcpClient() + try: + yield mcp_client + finally: + mcp_client.stop() + + +def test_observe_delivers_image_in_same_graph_turn(mocker, client) -> None: + image = { + "type": "image_url", + "image_url": {"url": "data:image/jpeg;base64,AA=="}, + } + mocker.patch.object( + client, + "_mcp_tool_call", + return_value={"content": [image]}, + ) + node = ToolNode([client._mcp_tool_to_langchain(_mcp_tool("observe"))]) + graph = StateGraph(MessagesState) + graph.add_node("tools", node) + graph.add_edge(START, "tools") + graph.add_edge("tools", END) + request = AIMessage( + content="", + tool_calls=[ + { + "name": "observe", + "args": {}, + "id": "observe-call", + "type": "tool_call", + } + ], + ) + + result = graph.compile().invoke({"messages": [request]}) + + tool_result, image_result = result["messages"][-2:] + assert isinstance(tool_result, ToolMessage) + assert tool_result.content == "Current camera frame attached." + assert tool_result.name == "observe" + assert tool_result.tool_call_id == "observe-call" + assert isinstance(image_result, HumanMessage) + assert image_result.content == [ + { + "type": "text", + "text": "This is the current camera frame returned by observe.", + }, + image, + ] + assert client._message_queue.empty() + + +def test_non_image_tools_keep_upstream_text_result(mocker, client) -> None: + mocker.patch.object( + client, + "_mcp_tool_call", + return_value={"content": [{"type": "text", "text": "done"}]}, + ) + tool = client._mcp_tool_to_langchain(_mcp_tool("wait")) + + result = tool.invoke({}) + + assert result == "done" + + +def test_background_update_is_injected_at_active_tool_boundary(mocker, client) -> None: + def call_tool(_name, _args): + client._on_tool_stream_message( + { + "method": "notifications/progress", + "params": { + "message": 'Found a match for ["bathtub"].', + "_meta": {"tool_name": "look_out_for"}, + }, + }, + ) + return {"content": [{"type": "text", "text": "wait complete"}]} + + mocker.patch.object(client, "_mcp_tool_call", side_effect=call_tool) + with client._live_update_lock: + client._processing_turn = True + + result = client._mcp_tool_to_langchain(_mcp_tool("wait")).invoke({}) + + assert result == ('wait complete\n\n[tool:look_out_for] Found a match for ["bathtub"].') + assert client._message_queue.empty() + assert client._take_live_update_text() == "" + + +def test_lookout_continuation_result_is_injected_into_active_turn(mocker, client) -> None: + client._tool_registry = {"stop_navigation": _mcp_tool("stop_navigation")} + mocker.patch.object( + client, + "_mcp_tool_call", + side_effect=[ + {"content": [{"type": "text", "text": "Stopped"}]}, + {"content": [{"type": "text", "text": "wait complete"}]}, + ], + ) + with client._live_update_lock: + client._processing_turn = True + + client.dispatch_continuation( + {"tool": "stop_navigation", "args": {}}, + {"label": "bathtub"}, + ) + result = client._mcp_tool_to_langchain(_mcp_tool("wait")).invoke({}) + + assert result == ( + "wait complete\n\n" + "Automatically executed 'stop_navigation' as a continuation of lookout " + "detection (detected: bathtub). Result: Stopped" + ) + assert client._message_queue.empty() + + +def test_background_update_queues_normally_while_agent_is_idle(client) -> None: + client._on_tool_stream_message( + { + "method": "notifications/message", + "params": { + "data": "exploration finished", + "logger": "begin_exploration", + }, + }, + ) + + message = client._message_queue.get_nowait() + assert message == HumanMessage( + content="[tool:begin_exploration] exploration finished", + ) + + +def test_turn_cancellation_skips_the_next_tool_and_exits_create_agent(mocker, client) -> None: + call_tool = mocker.patch.object(client, "_mcp_tool_call") + tool = client._mcp_tool_to_langchain(_mcp_tool("relative_move")) + model = MockModel( + responses=[ + AIMessage( + content="", + tool_calls=[ + { + "name": "relative_move", + "args": {}, + "id": "move-call", + "type": "tool_call", + } + ], + ), + AIMessage(content="model should not run again"), + ] + ) + graph = client._create_eval_agent(model, [tool]) + with client._live_update_lock: + client._processing_turn = True + client._on_eval_turn_control( + { + "type": CANCEL_ACTIVE_TURN, + "runId": "run-current", + } + ) + + updates = list(graph.stream({"messages": []}, stream_mode="updates")) + + assert model.i == 1 + assert call_tool.call_count == 0 + assert tool.return_direct is True + tool_messages = [ + message + for update in updates + for node in update.values() + for message in node.get("messages", []) + if isinstance(message, ToolMessage) + ] + assert [message.content for message in tool_messages] == [ + "The DimSim evaluation has ended. End this agent turn now." + ] + + +def test_turn_cancellation_after_active_tool_ends_at_that_boundary(mocker, client) -> None: + def call_tool(_name, _args): + client._on_eval_turn_control( + { + "type": CANCEL_ACTIVE_TURN, + "runId": "run-current", + } + ) + return {"content": [{"type": "text", "text": "movement stopped"}]} + + call = mocker.patch.object(client, "_mcp_tool_call", side_effect=call_tool) + tool = client._mcp_tool_to_langchain(_mcp_tool("relative_move")) + with client._live_update_lock: + client._processing_turn = True + + result = tool.invoke({}) + + call.assert_called_once_with("relative_move", {}) + assert result == ( + "movement stopped\n\nThe DimSim evaluation has ended. End this agent turn now." + ) + assert tool.return_direct is True + + +def test_turn_cancellation_is_ignored_while_idle(client) -> None: + client._on_eval_turn_control( + { + "type": CANCEL_ACTIVE_TURN, + "runId": "stale-run", + } + ) + + assert not client._eval_turn_cancel.is_set() + + +def test_agent_enables_streaming_for_responses_model(mocker, client) -> None: + model = ChatOpenAI( + api_key="test-key", + model="gpt-5.6-luna", + use_responses_api=True, + ) + graph = mocker.Mock() + mocker.patch.object(client, "_fetch_tools", return_value=[]) + init_model = mocker.patch( + "dimos.simulation.dimsim.mcp_client._init_model", + return_value=model, + ) + create_agent = mocker.patch( + "dimos.simulation.dimsim.mcp_client.create_agent", + return_value=graph, + ) + mocker.patch.object(client._thread, "start") + + client.on_system_modules([]) + + init_model.assert_called_once_with(client.config.model) + configured_model = create_agent.call_args.kwargs["model"] + assert isinstance(configured_model, ChatOpenAI) + assert configured_model is not model + assert configured_model.use_responses_api is True + assert configured_model.streaming is True + create_agent.assert_called_once_with( + model=configured_model, + tools=[], + system_prompt=client.config.system_prompt, + ) + assert client._state_graph is graph + + +def test_human_task_clears_spatial_memory_before_queueing(mocker, client) -> None: + spatial_memory = mocker.patch.object( + client, + "_spatial_memory", + create=True, + ) + + client._history.append(HumanMessage(content="previous task")) + client._message_queue.put(HumanMessage(content="stale tool update")) + + client._queue_human_input("find the bathtub") + + spatial_memory.clear_eval_memory.assert_called_once_with() + assert client._history == [] + message = client._message_queue.get_nowait() + assert message == HumanMessage(content="find the bathtub") + assert client._message_queue.empty() + + +def test_agent_turn_publishes_busy_then_idle_for_eval_barrier(mocker, client) -> None: + graph = mocker.Mock() + graph.stream.return_value = [] + publish_idle = mocker.patch.object(client.agent_idle, "publish") + mocker.patch.object(client.agent, "publish") + tool = client._mcp_tool_to_langchain(_mcp_tool("relative_move")) + tool.return_direct = True + client._eval_turn_cancel.set() + + client._process_message(graph, HumanMessage(content="go to the couch")) + + idle_states = [call.args[0] for call in publish_idle.call_args_list] + assert idle_states[0] is False + assert idle_states[-1] is True + assert tool.return_direct is False + assert not client._eval_turn_cancel.is_set() + + +def test_eval_idle_heartbeat_republishes_busy_state(mocker, client) -> None: + publish_idle = mocker.patch.object(client.agent_idle, "publish") + mocker.patch.object( + client._eval_idle_stop, + "wait", + side_effect=[False, True], + ) + client._set_eval_idle(False) + publish_idle.reset_mock() + + client._eval_idle_heartbeat() + + publish_idle.assert_called_once_with(False) diff --git a/dimos/simulation/dimsim/test_navigation_skill_container.py b/dimos/simulation/dimsim/test_navigation_skill_container.py new file mode 100644 index 0000000000..3233ae3631 --- /dev/null +++ b/dimos/simulation/dimsim/test_navigation_skill_container.py @@ -0,0 +1,119 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.simulation.dimsim.navigation_skill_container import ( + DimSimNavigationSkillContainer, +) + + +@pytest.fixture +def container(mocker) -> DimSimNavigationSkillContainer: + mocker.patch("dimos.models.vl.qwen.QwenVlModel") + navigation = DimSimNavigationSkillContainer() + try: + yield navigation + finally: + navigation.stop() + + +def test_semantic_match_at_current_viewpoint_does_not_start_navigation( + mocker, + container, +) -> None: + spatial_memory = mocker.patch.object( + container, + "_spatial_memory", + create=True, + ) + spatial_memory.query_detection_viewpoint.return_value = None + spatial_memory.query_by_text.return_value = [{"distance": 0.1}] + spatial_memory.eval_memory_generation.return_value = 1 + mocker.patch.object( + container, + "_get_goal_pose_from_result", + return_value=PoseStamped(position=Vector3(2.1, 3.1, 0)), + ) + navigate = mocker.patch.object(container, "_navigate_to") + container._latest_odom = PoseStamped(position=Vector3(2.0, 3.0, 0.5)) + + result = container._navigate_using_semantic_map("bathtub") + + assert result == ( + "The semantic-map match for 'bathtub' is the current camera viewpoint, " + "not the object's position. No navigation was started. Observe the current " + "frame and approach the visible object with bounded local movement." + ) + navigate.assert_not_called() + + +def test_semantic_viewpoint_can_be_revisited_after_moving_away( + mocker, + container, +) -> None: + spatial_memory = mocker.patch.object( + container, + "_spatial_memory", + create=True, + ) + spatial_memory.query_detection_viewpoint.return_value = None + spatial_memory.eval_memory_generation.return_value = 1 + spatial_memory.query_by_text.return_value = [{"id": "frame-1", "distance": 0.1}] + goal = PoseStamped(position=Vector3(2.1, 3.1, 0)) + mocker.patch.object(container, "_get_goal_pose_from_result", return_value=goal) + navigate = mocker.patch.object(container, "_navigate_to", return_value="started") + container._latest_odom = PoseStamped(position=Vector3(2.0, 3.0, 0.5)) + + container._navigate_using_semantic_map("bathtub") + container._latest_odom = PoseStamped(position=Vector3(8.0, 8.0, 0.5)) + result = container._navigate_using_semantic_map("gray soaking tub") + + assert result == "started" + navigate.assert_called_once_with( + goal, + ( + "Found a prior camera viewpoint matching 'gray soaking tub', " + "not a confirmed object position" + ), + ) + + +def test_exact_lookout_viewpoint_precedes_semantic_embedding_match( + mocker, + container, +) -> None: + spatial_memory = mocker.patch.object( + container, + "_spatial_memory", + create=True, + ) + detected = PoseStamped(position=Vector3(2.1, 3.1, 0)) + spatial_memory.query_detection_viewpoint.return_value = detected + navigate = mocker.patch.object(container, "_navigate_to", return_value="started") + container._latest_odom = PoseStamped(position=Vector3(8.0, 8.0, 0.5)) + + result = container._navigate_using_semantic_map("the bathtub") + + assert result == "started" + navigate.assert_called_once_with( + detected, + ( + "Found the camera viewpoint where the lookout detected 'the bathtub', " + "not a confirmed object position" + ), + ) + spatial_memory.query_by_text.assert_not_called() diff --git a/dimos/simulation/dimsim/test_perceive_loop_skill.py b/dimos/simulation/dimsim/test_perceive_loop_skill.py new file mode 100644 index 0000000000..de1db998b6 --- /dev/null +++ b/dimos/simulation/dimsim/test_perceive_loop_skill.py @@ -0,0 +1,201 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections.abc import Iterator +from typing import Any + +from dimos_lcm.std_msgs import Bool +import numpy as np +import pytest + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.Image import Image +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.simulation.dimsim.perceive_loop_skill import DimSimPerceiveLoopSkill + + +@pytest.fixture +def perceive_loop_skill(mocker: Any) -> Iterator[tuple[DimSimPerceiveLoopSkill, Any]]: + model = mocker.Mock() + mocker.patch("dimos.perception.perceive_loop_skill.create", return_value=model) + skill = DimSimPerceiveLoopSkill() + try: + yield skill, model + finally: + skill.stop() + + +def _detections(image: Image, name: str) -> ImageDetections2D[Detection2DBBox]: + result: ImageDetections2D[Detection2DBBox] = ImageDetections2D(image) + result.detections.append( + Detection2DBBox( + bbox=(10.0, 10.0, 20.0, 20.0), + track_id=0, + class_id=-1, + confidence=1.0, + name=name, + ts=image.ts, + image=image, + ) + ) + return result + + +def _usable_image(*, ts: float | None = None) -> Image: + kwargs = {"data": np.full((40, 40, 3), 128, dtype=np.uint8)} + if ts is not None: + kwargs["ts"] = ts + return Image(**kwargs) + + +def test_lookout_queries_each_description_as_plain_detector_text( + mocker: Any, + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + image = _usable_image() + empty: ImageDetections2D[Detection2DBBox] = ImageDetections2D(image) + model.query_detections.side_effect = [empty, _detections(image, "shower")] + tool_update = mocker.patch.object(skill, "tool_update") + stop_tool = mocker.patch.object(skill, "stop_tool") + stop_movement = mocker.patch.object(skill.stop_movement, "publish") + spatial_memory = mocker.patch.object(skill, "_spatial_memory", create=True) + skill._on_odom( + PoseStamped( + ts=image.ts - 0.05, + position=Vector3(1.0, 2.0, 0.5), + ) + ) + skill._active_lookout = ("bathtub", "shower") + + skill._on_image(image) + + assert model.query_detections.call_args_list == [ + mocker.call(image, "bathtub"), + mocker.call(image, "shower"), + ] + tool_update.assert_called_once_with( + "look_out_for", + 'Found a match for ["bathtub", "shower"]. Please announce audibly.', + ) + stop_tool.assert_called_once_with("look_out_for") + stop_movement.assert_called_once() + assert stop_movement.call_args.args[0].data is True + spatial_memory.record_detection_viewpoint.assert_called_once() + assert spatial_memory.record_detection_viewpoint.call_args.args == ( + ["bathtub", "shower"], + skill._odom_history[-1], + image.ts, + ) + + +def test_lookout_does_not_notify_when_plain_queries_do_not_match( + mocker: Any, + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + image = _usable_image() + model.query_detections.return_value = ImageDetections2D(image) + tool_update = mocker.patch.object(skill, "tool_update") + stop_tool = mocker.patch.object(skill, "stop_tool") + stop_movement = mocker.patch.object(skill.stop_movement, "publish") + skill._active_lookout = ("bathtub",) + + skill._on_image(image) + + model.query_detections.assert_called_once_with(image, "bathtub") + tool_update.assert_not_called() + stop_tool.assert_not_called() + stop_movement.assert_not_called() + assert skill._active_lookout == ("bathtub",) + + +def test_lookout_correlates_detection_with_frame_pose_not_completion_pose( + mocker: Any, + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + image = _usable_image(ts=100.0) + capture_pose = PoseStamped(ts=99.95, position=Vector3(1.0, 2.0, 0.5)) + completion_pose = PoseStamped(ts=104.0, position=Vector3(8.0, 9.0, 0.5)) + skill._on_odom(capture_pose) + skill._on_odom(completion_pose) + model.query_detections.return_value = _detections(image, "bathtub") + mocker.patch.object(skill.stop_movement, "publish") + spatial_memory = mocker.patch.object(skill, "_spatial_memory", create=True) + skill._active_lookout = ("bathtub",) + + skill._on_image(image) + + spatial_memory.record_detection_viewpoint.assert_called_once_with( + ["bathtub"], + capture_pose, + image.ts, + ) + + +def test_lookout_stops_motion_even_without_correlated_odometry( + mocker: Any, + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + image = _usable_image(ts=100.0) + model.query_detections.return_value = _detections(image, "bathtub") + stop_movement = mocker.patch.object(skill.stop_movement, "publish") + spatial_memory = mocker.patch.object(skill, "_spatial_memory", create=True) + skill._active_lookout = ("bathtub",) + + skill._on_image(image) + + stop_movement.assert_called_once() + assert isinstance(stop_movement.call_args.args[0], Bool) + spatial_memory.record_detection_viewpoint.assert_not_called() + + +def test_lookout_ignores_mostly_black_camera_frame( + mocker: Any, + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + data = np.zeros((100, 100, 3), dtype=np.uint8) + data[80:, :, :] = 128 + image = Image(data=data) + skill._active_lookout = ("bathtub",) + tool_update = mocker.patch.object(skill, "tool_update") + stop_movement = mocker.patch.object(skill.stop_movement, "publish") + + skill._on_image(image) + + model.query_detections.assert_not_called() + tool_update.assert_not_called() + stop_movement.assert_not_called() + assert skill._active_lookout == ("bathtub",) + + +def test_lookout_queries_detector_when_enough_of_frame_is_visible( + perceive_loop_skill: tuple[DimSimPerceiveLoopSkill, Any], +) -> None: + skill, model = perceive_loop_skill + data = np.zeros((100, 100, 3), dtype=np.uint8) + data[60:, :, :] = 128 + image = Image(data=data) + model.query_detections.return_value = ImageDetections2D(image) + skill._active_lookout = ("bathtub",) + + skill._on_image(image) + + model.query_detections.assert_called_once_with(image, "bathtub") + assert skill._active_lookout == ("bathtub",) diff --git a/dimos/simulation/dimsim/test_spatial_memory.py b/dimos/simulation/dimsim/test_spatial_memory.py new file mode 100644 index 0000000000..1f141490bf --- /dev/null +++ b/dimos/simulation/dimsim/test_spatial_memory.py @@ -0,0 +1,114 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.perception.visual_memory import VisualMemory +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemory + + +@pytest.fixture +def memory_setup( + mocker, + tmp_path, +): + vector_db = mocker.patch("dimos.perception.spatial_perception.SpatialVectorDB").return_value + mocker.patch("dimos.perception.spatial_perception.ImageEmbeddingProvider") + visual_memory = VisualMemory(output_dir=str(tmp_path)) + clear_visual_memory = mocker.spy(visual_memory, "clear") + memory = DimSimSpatialMemory( + db_path=None, + visual_memory=visual_memory, + visual_memory_path=None, + ) + try: + yield memory, vector_db, clear_visual_memory + finally: + memory.stop() + + +def test_clear_eval_memory_removes_observations_tags_and_sampling_state( + mocker, + memory_setup, +) -> None: + memory, vector_db, clear_visual_memory = memory_setup + vector_db.image_collection.get.return_value = {"ids": ["frame-1", "frame-2"]} + vector_db.location_collection.get.return_value = {"ids": ["tag-1"]} + memory.robot_locations.append(mocker.Mock()) + memory.last_position = mocker.Mock() + memory.last_record_time = 123.0 + memory.frame_count = 9 + memory.stored_frame_count = 4 + memory._latest_video_frame = mocker.Mock() + + assert memory.eval_memory_generation() == 0 + cleared = memory.clear_eval_memory() + + assert cleared == 3 + assert memory.eval_memory_generation() == 1 + vector_db.image_collection.delete.assert_called_once_with( + ids=["frame-1", "frame-2"], + ) + vector_db.location_collection.delete.assert_called_once_with(ids=["tag-1"]) + clear_visual_memory.assert_called_once_with() + assert memory.robot_locations == [] + assert memory._latest_video_frame is None + assert memory.last_position is None + assert memory.last_record_time is None + assert memory.frame_count == 0 + assert memory.stored_frame_count == 0 + assert memory.query_detection_viewpoint("bathtub") is None + + +def test_clear_eval_memory_advances_generation_even_when_empty( + memory_setup, +) -> None: + memory, vector_db, _clear_visual_memory = memory_setup + vector_db.image_collection.get.return_value = {"ids": []} + vector_db.location_collection.get.return_value = {"ids": []} + + memory.clear_eval_memory() + memory.clear_eval_memory() + + assert memory.eval_memory_generation() == 2 + + +def test_detection_viewpoint_uses_latest_matching_frame(memory_setup) -> None: + memory, _vector_db, _clear_visual_memory = memory_setup + old_pose = PoseStamped(position=Vector3(1.0, 2.0, 0.5)) + new_pose = PoseStamped(position=Vector3(3.0, 4.0, 0.5)) + + memory.record_detection_viewpoint(["Bathtub"], old_pose, 10.0) + memory.record_detection_viewpoint(["gray bathtub"], new_pose, 20.0) + + assert memory.query_detection_viewpoint("the gray bathtub in the bathroom") is new_pose + assert memory.query_detection_viewpoint("bathtub") is new_pose + assert memory.query_detection_viewpoint("couch") is None + + +def test_clear_eval_memory_forgets_detection_viewpoints(memory_setup) -> None: + memory, vector_db, _clear_visual_memory = memory_setup + vector_db.image_collection.get.return_value = {"ids": []} + vector_db.location_collection.get.return_value = {"ids": []} + memory.record_detection_viewpoint( + ["bathtub"], + PoseStamped(position=Vector3(1.0, 2.0, 0.5)), + 10.0, + ) + + memory.clear_eval_memory() + + assert memory.query_detection_viewpoint("bathtub") is None diff --git a/dimos/simulation/dimsim/test_unitree_skill_container.py b/dimos/simulation/dimsim/test_unitree_skill_container.py new file mode 100644 index 0000000000..c17197142d --- /dev/null +++ b/dimos/simulation/dimsim/test_unitree_skill_container.py @@ -0,0 +1,293 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import math + +import pytest + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.perception.perceive_loop_skill import PerceiveLoopSkill +from dimos.perception.spatial_perception import SpatialMemory +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.unitree_skill_container import UnitreeSkillContainer +from dimos.simulation.dimsim.agentic_blueprint import unitree_go2_agentic_dimsim +from dimos.simulation.dimsim.go2_connection import DimSimGO2Connection +from dimos.simulation.dimsim.mcp_client import DimSimMcpClient +from dimos.simulation.dimsim.navigation_skill_container import ( + DimSimNavigationSkillContainer, +) +from dimos.simulation.dimsim.perceive_loop_skill import DimSimPerceiveLoopSkill +from dimos.simulation.dimsim.spatial_memory import DimSimSpatialMemory +from dimos.simulation.dimsim.unitree_skill_container import DimSimUnitreeSkillContainer + + +def _pose(ts: float, x: float = 0, y: float = 0, yaw_deg: float = 0) -> PoseStamped: + return PoseStamped( + ts=ts, + position=Vector3(x, y, 0.5), + orientation=Quaternion.from_euler(Vector3(0, 0, math.radians(yaw_deg))), + ) + + +@pytest.fixture +def container() -> DimSimUnitreeSkillContainer: + skill_container = DimSimUnitreeSkillContainer() + try: + yield skill_container + finally: + skill_container.stop() + + +def test_dimsim_blueprint_preserves_full_upstream_agent_with_eval_adapters() -> None: + active_classes = {atom.module for atom in unitree_go2_agentic_dimsim.active_blueprints} + + assert UnitreeSkillContainer not in active_classes + assert DimSimUnitreeSkillContainer in active_classes + assert McpClient not in active_classes + assert DimSimMcpClient in active_classes + assert NavigationSkillContainer not in active_classes + assert DimSimNavigationSkillContainer in active_classes + assert PerceiveLoopSkill not in active_classes + assert DimSimPerceiveLoopSkill in active_classes + assert SpatialMemory not in active_classes + assert DimSimSpatialMemory in active_classes + assert GO2Connection not in active_classes + assert DimSimGO2Connection in active_classes + + mcp_client = next( + atom + for atom in unitree_go2_agentic_dimsim.active_blueprints + if atom.module is DimSimMcpClient + ) + assert "system_prompt" not in mcp_client.kwargs + assert "allowed_tools" not in mcp_client.kwargs + + +def test_relative_move_schema_explains_camera_left_right_sign(container) -> None: + skill = next(skill for skill in container.get_skills() if skill.func_name == "relative_move") + description = json.loads(skill.args_schema)["description"] + + assert "object on the left side" in description + assert "positive `left`" in description + assert "object on the right side" in description + assert "negative `left`" in description + + +def test_start_disposes_odometry_subscription(mocker, container) -> None: + mocker.patch.object(UnitreeSkillContainer, "start") + unsubscribe = mocker.Mock() + mocker.patch.object(container.odom, "subscribe", return_value=unsubscribe) + + container.start() + container.stop() + + unsubscribe.assert_called_once_with() + + +def test_relative_move_reports_measured_forward_progress(mocker, container) -> None: + container._on_odom(_pose(1)) + mocker.patch.object( + container, + "_next_odom", + side_effect=[ + _pose(2, x=0.2), + _pose(3, x=0.4), + _pose(4, x=0.6), + ], + ) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + + result = container.relative_move(forward=0.6) + + assert result == ( + "Movement completed: travelled 0.60m of the requested 0.60m along the " + "requested direction (lateral drift 0.00m). Observe to verify the camera view." + ) + assert publish.call_args_list[-1].args == (Twist.zero(),) + assert all(call.args[0].linear.x > 0 for call in publish.call_args_list[:-5]) + assert all(call.args == (Twist.zero(),) for call in publish.call_args_list[-5:]) + + +def test_translation_rate_limits_commands_during_queued_odometry( + mocker, + container, +) -> None: + start = _pose(1) + mocker.patch.object( + container, + "_next_odom", + side_effect=[ + _pose(2, x=0.2), + _pose(3, x=0.4), + _pose(4, x=0.6), + ], + ) + fake_time = mocker.Mock() + fake_time.monotonic.side_effect = [0, 0, 0, 0.02, 0.02, 0.04, 0.04] + mocker.patch( + "dimos.simulation.dimsim.unitree_skill_container.time", + fake_time, + ) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + + result = container._run_translation(start, 0.6, 0, 0.6) + + assert result.status == "completed" + assert result.along_m == pytest.approx(0.6) + publish.assert_called_once() + + +def test_relative_move_reports_collision_sliding_as_blocked(mocker, container) -> None: + container._on_odom(_pose(1)) + mocker.patch.object( + container, + "_next_odom", + side_effect=[_pose(ts, y=(ts - 1) * 0.02) for ts in range(2, 27)], + ) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + + result = container.relative_move(forward=1.0) + + assert result == ( + "Movement was blocked or only partially completed: travelled 0.00m of the " + "requested 1.00m along the requested direction (lateral drift 0.50m). " + "Rotate toward another open route, then observe." + ) + assert publish.call_args_list[-1].args == (Twist.zero(),) + + +def test_relative_rotation_tracks_progress_across_yaw_wrap(mocker, container) -> None: + container._on_odom(_pose(1, yaw_deg=170)) + mocker.patch.object( + container, + "_next_odom", + side_effect=[ + _pose(2, yaw_deg=179), + _pose(3, yaw_deg=-175), + _pose(4, yaw_deg=-160), + ], + ) + mocker.patch.object( + container, + "_settled_odom", + return_value=_pose(5, yaw_deg=-160), + ) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + + result = container.relative_move(degrees=30) + + assert result == ( + "Movement completed: turned 30.0 degrees of the requested 30.0 degrees. " + "Observe to verify the camera view." + ) + assert publish.call_args_list[-1].args == (Twist.zero(),) + assert all(call.args[0].angular.z > 0 for call in publish.call_args_list[:-10]) + assert all(call.args == (Twist.zero(),) for call in publish.call_args_list[-10:]) + + +def test_precise_rotation_corrects_settled_overshoot(mocker, container) -> None: + start = _pose(1, yaw_deg=0) + coarse = mocker.patch.object( + container, + "_run_rotation", + return_value=object(), + ) + mocker.patch.object(container, "_clear_velocity") + mocker.patch.object( + container, + "_settled_odom", + side_effect=[ + _pose(2, yaw_deg=40), + _pose(3, yaw_deg=30), + ], + ) + + result = container._run_precise_rotation(start, math.radians(30)) + + assert result.status == "completed" + assert math.degrees(result.radians) == pytest.approx(30) + assert coarse.call_count == 2 + first, correction = coarse.call_args_list + assert first.args == (start, pytest.approx(math.radians(30)), 0.35) + assert correction.args[0] == _pose(2, yaw_deg=40) + assert correction.args[1] == pytest.approx(math.radians(-10)) + assert correction.args[2] == 0.08 + + +def test_relative_move_stops_velocity_when_motion_raises(mocker, container) -> None: + container._on_odom(_pose(1)) + mocker.patch.object(container, "_run_translation", side_effect=RuntimeError("failed")) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + mocker.patch("dimos.simulation.dimsim.unitree_skill_container.time.sleep") + + with pytest.raises(RuntimeError, match="failed"): + container.relative_move(forward=0.5) + + assert len(publish.call_args_list) == 5 + assert all(call.args == (Twist.zero(),) for call in publish.call_args_list) + + +def test_relative_move_requires_fresh_odometry(mocker, container) -> None: + mocker.patch.object(container, "_fresh_odom", return_value=None) + publish = mocker.patch.object(container.tele_cmd_vel, "publish") + + result = container.relative_move(forward=0.5) + + assert result == "Movement not started: no fresh DimSim odometry is available." + publish.assert_not_called() + + +@pytest.mark.parametrize( + ("forward", "degrees", "expected"), + [ + ( + 1.01, + 0, + "Movement rejected: request 1.00m or less per call, then observe.", + ), + ( + 0, + 61, + "Movement rejected: request at most 60 degrees per call, then observe.", + ), + ], +) +def test_relative_move_rejects_unbounded_commands( + container, + forward, + degrees, + expected, +) -> None: + assert container.relative_move(forward=forward, degrees=degrees) == expected + + +def test_odom_cache_rejects_out_of_order_samples(container) -> None: + newest = _pose(2, x=1) + stale = _pose(1, x=9) + + container._on_odom(newest) + container._on_odom(stale) + + assert container._latest_odom is newest + + +def test_relative_move_rejects_non_finite_values(container) -> None: + with pytest.raises(ValueError, match="must be finite"): + container.relative_move(forward=float("nan")) diff --git a/dimos/simulation/dimsim/unitree_skill_container.py b/dimos/simulation/dimsim/unitree_skill_container.py new file mode 100644 index 0000000000..6575a3fdb5 --- /dev/null +++ b/dimos/simulation/dimsim/unitree_skill_container.py @@ -0,0 +1,417 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from dataclasses import dataclass +import math +from threading import Condition +import time +from typing import Any, Literal + +from reactivex.disposable import Disposable + +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.unitree.unitree_skill_container import UnitreeSkillContainer + +_CONTROL_PERIOD_SEC = 0.1 +_STOP_PUBLISH_PERIOD_SEC = 0.05 +_STOP_PUBLISH_COUNT = 5 +_ODOM_READY_TIMEOUT_SEC = 1.0 +_ODOM_STALE_SEC = 1.0 +_MAX_TRANSLATION_M = 1.0 +_MAX_ROTATION_DEG = 60.0 +_LINEAR_COMMAND_MPS = 0.2 +_ANGULAR_COMMAND_RAD_PER_SEC = 0.35 +_ANGULAR_CORRECTION_RAD_PER_SEC = 0.08 +_TRANSLATION_TOLERANCE_M = 0.04 +_ROTATION_TOLERANCE_RAD = math.radians(2.0) +_SETTLED_POSITION_TOLERANCE_M = 0.003 +_SETTLED_YAW_TOLERANCE_RAD = math.radians(0.3) +_SETTLED_SAMPLE_COUNT = 3 +_SETTLE_TIMEOUT_SEC = 2.0 +_MAX_ROTATION_CORRECTIONS = 3 +_MIN_PROGRESS_M = 0.01 +_MIN_ROTATION_PROGRESS_RAD = math.radians(0.5) +_MAX_STAGNANT_SAMPLES = 25 + +_SegmentStatus = Literal["completed", "blocked", "odometry-timeout"] + + +@dataclass(frozen=True) +class _TranslationResult: + status: _SegmentStatus + along_m: float + lateral_m: float + + +@dataclass(frozen=True) +class _RotationResult: + status: _SegmentStatus + radians: float + + +def _normalize_angle(radians: float) -> float: + return math.atan2(math.sin(radians), math.cos(radians)) + + +def _yaw(pose: PoseStamped) -> float: + return pose.orientation.to_euler().yaw + + +class DimSimUnitreeSkillContainer(UnitreeSkillContainer): + """Unitree skills with bounded, collision-aware local motion for DimSim.""" + + odom: In[PoseStamped] + tele_cmd_vel: Out[Twist] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._odom_condition = Condition() + self._latest_odom: PoseStamped | None = None + self._latest_odom_received_at = float("-inf") + + @rpc + def start(self) -> None: + super().start() + self.register_disposable( + Disposable(self.odom.subscribe(self._on_odom)), + ) + + def _on_odom(self, pose: PoseStamped) -> None: + with self._odom_condition: + if self._latest_odom is not None and pose.ts <= self._latest_odom.ts: + return + self._latest_odom = pose + self._latest_odom_received_at = time.monotonic() + self._odom_condition.notify_all() + + def _fresh_odom(self, timeout: float = _ODOM_READY_TIMEOUT_SEC) -> PoseStamped | None: + deadline = time.monotonic() + timeout + with self._odom_condition: + while True: + now = time.monotonic() + if ( + self._latest_odom is not None + and now - self._latest_odom_received_at <= _ODOM_STALE_SEC + ): + return self._latest_odom + remaining = deadline - now + if remaining <= 0: + return None + self._odom_condition.wait(remaining) + + def _next_odom(self, after_ts: float, timeout: float) -> PoseStamped | None: + deadline = time.monotonic() + timeout + with self._odom_condition: + while self._latest_odom is None or self._latest_odom.ts <= after_ts: + remaining = deadline - time.monotonic() + if remaining <= 0: + return None + self._odom_condition.wait(remaining) + return self._latest_odom + + @skill(uses=["movement"]) + def relative_move( + self, + forward: float = 0.0, + left: float = 0.0, + degrees: float = 0.0, + ) -> str: + """Move the simulated robot relative to its current camera heading. + + Rapier remains authoritative for collision handling. Translation is + limited to 1 metre and rotation to 60 degrees per call. The result + reports measured progress, allowing blocked motion to be distinguished + from a completed move. + + The camera image is not mirrored: an object on the left side of the + image requires a positive `left` value, and an object on the right side + requires a negative `left` value. For example, + `relative_move(left=0.5)` moves toward screen-left and + `relative_move(left=-0.5)` moves toward screen-right. + + Args: + forward: Forward distance in metres; negative moves backward. + left: Screen-left distance in metres; negative moves screen-right. + degrees: Final relative yaw in degrees; positive turns left. + """ + forward, left, degrees = float(forward), float(left), float(degrees) + if not all(math.isfinite(value) for value in (forward, left, degrees)): + raise ValueError("relative movement values must be finite") + + requested_distance = math.hypot(forward, left) + if requested_distance > _MAX_TRANSLATION_M: + return ( + f"Movement rejected: request {_MAX_TRANSLATION_M:.2f}m or less per call, " + "then observe." + ) + if abs(degrees) > _MAX_ROTATION_DEG: + return ( + f"Movement rejected: request at most {_MAX_ROTATION_DEG:.0f} degrees " + "per call, then observe." + ) + if requested_distance == 0 and degrees == 0: + return "No movement requested." + + start_pose = self._fresh_odom() + if start_pose is None: + return "Movement not started: no fresh DimSim odometry is available." + + translation: _TranslationResult | None = None + rotation: _RotationResult | None = None + try: + if requested_distance: + translation = self._run_translation( + start_pose, + forward, + left, + requested_distance, + ) + if degrees: + rotation_start = self._fresh_odom() + if rotation_start is None: + rotation = _RotationResult("odometry-timeout", 0.0) + else: + rotation = self._run_precise_rotation( + rotation_start, + math.radians(degrees), + ) + finally: + self._clear_velocity() + + return self._format_result( + requested_distance=requested_distance, + requested_degrees=degrees, + translation=translation, + rotation=rotation, + ) + + def _clear_velocity(self) -> None: + """Hold zero velocity long enough to cross the process/LCM boundary. + + A single final publication can be overtaken by odometry and lidar work + in the live DimSim stack. Repeating zero over several control periods + makes the stop authoritative before a later tool call starts. + """ + zero = Twist.zero() + for index in range(_STOP_PUBLISH_COUNT): + self.tele_cmd_vel.publish(zero) + if index + 1 < _STOP_PUBLISH_COUNT: + time.sleep(_STOP_PUBLISH_PERIOD_SEC) + + def _settled_odom(self) -> PoseStamped | None: + """Return odometry after consecutive stationary server poses.""" + current = self._fresh_odom() + if current is None: + return None + + deadline = time.monotonic() + _SETTLE_TIMEOUT_SEC + stable_samples = 0 + while time.monotonic() < deadline: + next_pose = self._next_odom( + current.ts, + min(_CONTROL_PERIOD_SEC, deadline - time.monotonic()), + ) + if next_pose is None: + continue + + position_delta = (next_pose.position - current.position).magnitude() + yaw_delta = abs(_normalize_angle(_yaw(next_pose) - _yaw(current))) + if ( + position_delta <= _SETTLED_POSITION_TOLERANCE_M + and yaw_delta <= _SETTLED_YAW_TOLERANCE_RAD + ): + stable_samples += 1 + if stable_samples >= _SETTLED_SAMPLE_COUNT: + return next_pose + else: + stable_samples = 0 + current = next_pose + + return current + + def _run_precise_rotation( + self, + start_pose: PoseStamped, + requested_radians: float, + ) -> _RotationResult: + """Correct delayed-odometry overshoot against an absolute yaw target.""" + target_yaw = _normalize_angle(_yaw(start_pose) + requested_radians) + current = start_pose + command_radians = requested_radians + speed = _ANGULAR_COMMAND_RAD_PER_SEC + + for attempt in range(_MAX_ROTATION_CORRECTIONS + 1): + self._run_rotation(current, command_radians, speed) + self._clear_velocity() + settled = self._settled_odom() + if settled is None: + return _RotationResult("odometry-timeout", 0.0) + + actual = _normalize_angle(_yaw(settled) - _yaw(start_pose)) + error = _normalize_angle(target_yaw - _yaw(settled)) + if abs(error) <= _ROTATION_TOLERANCE_RAD: + return _RotationResult("completed", actual) + if attempt == _MAX_ROTATION_CORRECTIONS: + return _RotationResult("blocked", actual) + + current = settled + command_radians = error + speed = _ANGULAR_CORRECTION_RAD_PER_SEC + + raise AssertionError("unreachable") + + def _run_translation( + self, + start_pose: PoseStamped, + forward: float, + left: float, + requested_distance: float, + ) -> _TranslationResult: + local_direction = Vector3(forward / requested_distance, left / requested_distance, 0) + world_direction = start_pose.orientation.rotate_vector(local_direction) + perpendicular = Vector3(-world_direction.y, world_direction.x, 0) + command = Twist( + linear=Vector3( + _LINEAR_COMMAND_MPS * forward / requested_distance, + _LINEAR_COMMAND_MPS * left / requested_distance, + 0, + ), + angular=Vector3(), + ) + + deadline = time.monotonic() + min(8.0, max(2.0, requested_distance * 4.0 + 2.0)) + last_pose = start_pose + best_progress = 0.0 + stagnant_samples = 0 + along = 0.0 + lateral = 0.0 + last_command_at = float("-inf") + + while time.monotonic() < deadline: + now = time.monotonic() + if now - last_command_at >= _CONTROL_PERIOD_SEC: + self.tele_cmd_vel.publish(command) + last_command_at = now + next_pose = self._next_odom(last_pose.ts, _CONTROL_PERIOD_SEC) + if next_pose is None: + continue + last_pose = next_pose + + delta = next_pose.position - start_pose.position + along = delta.dot(world_direction) + lateral = abs(delta.dot(perpendicular)) + if along >= requested_distance - _TRANSLATION_TOLERANCE_M: + return _TranslationResult("completed", along, lateral) + + if along >= best_progress + _MIN_PROGRESS_M: + best_progress = along + stagnant_samples = 0 + else: + stagnant_samples += 1 + if stagnant_samples >= _MAX_STAGNANT_SAMPLES: + return _TranslationResult("blocked", max(0.0, along), lateral) + + status: _SegmentStatus = "blocked" if last_pose.ts > start_pose.ts else "odometry-timeout" + return _TranslationResult(status, max(0.0, along), lateral) + + def _run_rotation( + self, + start_pose: PoseStamped, + requested_radians: float, + command_speed: float = _ANGULAR_COMMAND_RAD_PER_SEC, + ) -> _RotationResult: + direction = math.copysign(1.0, requested_radians) + command = Twist( + linear=Vector3(), + angular=Vector3(0, 0, direction * command_speed), + ) + deadline = time.monotonic() + min( + 6.0, + max(2.0, abs(requested_radians) * 3.0 + 2.0), + ) + last_pose = start_pose + last_yaw = _yaw(start_pose) + accumulated = 0.0 + best_progress = 0.0 + stagnant_samples = 0 + last_command_at = float("-inf") + + while time.monotonic() < deadline: + now = time.monotonic() + if now - last_command_at >= _CONTROL_PERIOD_SEC: + self.tele_cmd_vel.publish(command) + last_command_at = now + next_pose = self._next_odom(last_pose.ts, _CONTROL_PERIOD_SEC) + if next_pose is None: + continue + last_pose = next_pose + + next_yaw = _yaw(next_pose) + accumulated += _normalize_angle(next_yaw - last_yaw) + last_yaw = next_yaw + progress = direction * accumulated + if progress >= abs(requested_radians) - _ROTATION_TOLERANCE_RAD: + return _RotationResult("completed", accumulated) + + if progress >= best_progress + _MIN_ROTATION_PROGRESS_RAD: + best_progress = progress + stagnant_samples = 0 + else: + stagnant_samples += 1 + if stagnant_samples >= _MAX_STAGNANT_SAMPLES: + return _RotationResult("blocked", accumulated) + + status: _SegmentStatus = "blocked" if last_pose.ts > start_pose.ts else "odometry-timeout" + return _RotationResult(status, accumulated) + + @staticmethod + def _format_result( + *, + requested_distance: float, + requested_degrees: float, + translation: _TranslationResult | None, + rotation: _RotationResult | None, + ) -> str: + segments: list[str] = [] + statuses: list[_SegmentStatus] = [] + if translation is not None: + statuses.append(translation.status) + segments.append( + f"travelled {translation.along_m:.2f}m of the requested " + f"{requested_distance:.2f}m along the requested direction " + f"(lateral drift {translation.lateral_m:.2f}m)" + ) + if rotation is not None: + statuses.append(rotation.status) + segments.append( + f"turned {math.degrees(rotation.radians):.1f} degrees of the requested " + f"{requested_degrees:.1f} degrees" + ) + + detail = "; ".join(segments) + if "odometry-timeout" in statuses: + return f"Movement stopped because DimSim odometry became unavailable: {detail}." + if "blocked" in statuses: + return ( + f"Movement was blocked or only partially completed: {detail}. " + "Rotate toward another open route, then observe." + ) + return f"Movement completed: {detail}. Observe to verify the camera view." diff --git a/misc/DimSim/cli/bridge/eval-reset.ts b/misc/DimSim/cli/bridge/eval-reset.ts new file mode 100644 index 0000000000..710887a0af --- /dev/null +++ b/misc/DimSim/cli/bridge/eval-reset.ts @@ -0,0 +1,48 @@ +import type { + EvalResetMessage, + EvalStartPose, + ResetAckMessage, +} from "../../evals/protocol.ts"; +import { isFiniteStartPose } from "../../evals/protocol.ts"; + +export interface ResettablePhysics { + resetPose(pose: EvalStartPose): EvalStartPose; + clearMotion(): void; +} + +/** Validate and apply an authoritative reset without depending on the WS server. */ +export function applyEvalReset( + physics: ResettablePhysics | null, + message: EvalResetMessage, +): ResetAckMessage { + if (!physics) { + return { + type: "resetAck", + runId: message.runId, + ok: false, + reason: "server physics is unavailable", + }; + } + if (!isFiniteStartPose(message.startPose)) { + return { + type: "resetAck", + runId: message.runId, + ok: false, + reason: "startPose requires finite x, y, z, and yaw", + }; + } + try { + const pose = physics.resetPose(message.startPose); + if (!isFiniteStartPose(pose)) { + throw new Error("server physics returned an invalid actual pose"); + } + return { type: "resetAck", runId: message.runId, ok: true, pose }; + } catch (error) { + return { + type: "resetAck", + runId: message.runId, + ok: false, + reason: error instanceof Error ? error.message : String(error), + }; + } +} diff --git a/misc/DimSim/cli/bridge/physics.ts b/misc/DimSim/cli/bridge/physics.ts index ff87552240..e1f5ab933c 100644 --- a/misc/DimSim/cli/bridge/physics.ts +++ b/misc/DimSim/cli/bridge/physics.ts @@ -15,6 +15,7 @@ import { geometry_msgs, std_msgs } from "@dimos/msgs"; import type { LCM } from "../vendor/lcm/lcm.ts"; +import type { EvalStartPose } from "../../evals/protocol.ts"; // -- Agent dimensions (must match AiAvatar.js / engine.js) -------------------- const DEFAULT_AGENT_RADIUS = 0.12; @@ -66,17 +67,18 @@ type MotionOut = { dx: number; dy: number; dz: number; dyaw: number; clampMaxY?: const _clamp = (v: number, lo: number, hi: number) => Math.max(lo, Math.min(hi, v)); -const MOTION_MODELS: Record< +export const MOTION_MODELS: Record< string, (cmd: MotionCmd, yaw: number, cfg: MotionCfg, dt: number) => MotionOut > = { - // Ground / holonomic: forward drive along heading, yaw from angular vel, gravity down. + // Ground / holonomic: planar translation relative to heading, yaw from + // angular velocity, and gravity down. holonomic(cmd, yaw, cfg, dt) { const dyaw = cmd.angZ * dt; const y = yaw + dyaw; return { - dx: cmd.linX * Math.sin(y) * dt, - dz: cmd.linX * Math.cos(y) * dt, + dx: (cmd.linX * Math.sin(y) + cmd.linY * Math.cos(y)) * dt, + dz: (cmd.linX * Math.cos(y) - cmd.linY * Math.sin(y)) * dt, dy: cfg.gravity * dt * dt * 0.5, dyaw, }; @@ -119,7 +121,11 @@ function resolveMotionModel(embodiment?: EmbodimentConfig): string { const CH_ODOM = "/odom#geometry_msgs.PoseStamped"; const CH_CMD_VEL = "/cmd_vel#geometry_msgs.Twist"; -const CMD_VEL_TIMEOUT_MS = 500; +// DimOS motion skills refresh cmd_vel at 10 Hz. Keep one additional half +// period of jitter tolerance, but stop promptly when their explicit zero is +// delayed behind other LCM work. A 500 ms hold let each bounded relative move +// coast roughly 0.3–0.4 m after the skill had already reported completion. +const CMD_VEL_TIMEOUT_MS = 150; // -- ServerPhysics ------------------------------------------------------------ @@ -315,6 +321,58 @@ export class ServerPhysics { // quiet } + /** Clear all commanded and Rapier-side motion state. */ + clearMotion(): void { + this.linX = 0; + this.linY = 0; + this.linZ = 0; + this.angZ = 0; + this.cmdVelStamp = 0; + this.lastStepAt = 0; + try { + this.body.setLinvel?.({ x: 0, y: 0, z: 0 }, true); + this.body.setAngvel?.({ x: 0, y: 0, z: 0 }, true); + } catch { + // Kinematic Rapier bodies may not expose dynamic-body velocity setters. + } + } + + /** + * Apply an eval start pose authoritatively on the server. + * Coordinates are Three.js Y-up and yaw is in degrees, matching workflows. + */ + resetPose(pose: EvalStartPose): EvalStartPose { + if (![pose.x, pose.y, pose.z, pose.yaw].every(Number.isFinite)) { + throw new Error("startPose requires finite x, y, z, and yaw"); + } + + this.clearMotion(); + this.yaw = (pose.yaw * Math.PI) / 180; + this.body.setNextKinematicTranslation({ + x: pose.x, + y: pose.y, + z: pose.z, + }); + this.body.setNextKinematicRotation?.({ + x: 0, + y: Math.sin(this.yaw / 2), + z: 0, + w: Math.cos(this.yaw / 2), + }); + this.world.step(); + + const actual = this.body.translation(); + const result = { + x: actual.x, + y: actual.y, + z: actual.z, + yaw: (this.yaw * 180) / Math.PI, + }; + this._publishOdom(actual); + this.onPoseUpdate?.(actual.x, actual.y, actual.z, this.yaw); + return result; + } + /** Set callback for browser position sync. */ setOnPoseUpdate( cb: (x: number, y: number, z: number, yaw: number) => void, diff --git a/misc/DimSim/cli/bridge/physics_test.ts b/misc/DimSim/cli/bridge/physics_test.ts new file mode 100644 index 0000000000..2d9f5e0e96 --- /dev/null +++ b/misc/DimSim/cli/bridge/physics_test.ts @@ -0,0 +1,115 @@ +import { applyEvalReset } from "./eval-reset.ts"; +import { MOTION_MODELS, ServerPhysics } from "./physics.ts"; + +function assert( + condition: unknown, + message = "assertion failed", +): asserts condition { + if (!condition) throw new Error(message); +} + +function assertClose(actual: number, expected: number): void { + if (Math.abs(actual - expected) > 1e-9) { + throw new Error(`expected ${expected}, got ${actual}`); + } +} + +Deno.test("holonomic motion applies forward and lateral velocity in the local frame", () => { + const output = MOTION_MODELS.holonomic( + { linX: 1, linY: 2, linZ: 0, angZ: 0 }, + Math.PI / 2, + { + gravity: -9.81, + maxAltitude: 100, + wheelBase: 1, + maxSteerAngle: Math.PI / 4, + }, + 0.5, + ); + + assertClose(output.dx, 0.5); + assertClose(output.dz, -1); +}); + +Deno.test("server physics reset applies position/yaw and zeroes motion", () => { + const physics = Object.create(ServerPhysics.prototype) as any; + let nextPosition = { x: 0, y: 0, z: 0 }; + let rotation = { x: 0, y: 0, z: 0, w: 1 }; + let odomCount = 0; + let poseUpdate: number[] = []; + let linvel: unknown; + let angvel: unknown; + physics.body = { + setNextKinematicTranslation(value: typeof nextPosition) { + nextPosition = value; + }, + setNextKinematicRotation(value: typeof rotation) { + rotation = value; + }, + translation() { + return nextPosition; + }, + setLinvel(value: unknown) { + linvel = value; + }, + setAngvel(value: unknown) { + angvel = value; + }, + }; + physics.world = { step() {} }; + physics._publishOdom = () => { + odomCount++; + }; + physics.onPoseUpdate = (...values: number[]) => { + poseUpdate = values; + }; + physics.linX = 1; + physics.linY = 2; + physics.linZ = 3; + physics.angZ = 4; + physics.cmdVelStamp = 123; + physics.lastStepAt = 456; + + const result = physics.resetPose({ x: 1, y: 2, z: 3, yaw: 90 }); + + assertClose(result.x, 1); + assertClose(result.y, 2); + assertClose(result.z, 3); + assertClose(result.yaw, 90); + assertClose(rotation.y, Math.SQRT1_2); + assertClose(rotation.w, Math.SQRT1_2); + assert(physics.linX === 0 && physics.linY === 0 && physics.linZ === 0); + assert(physics.angZ === 0 && physics.cmdVelStamp === 0); + assert(physics.lastStepAt === 0); + assert(JSON.stringify(linvel) === JSON.stringify({ x: 0, y: 0, z: 0 })); + assert(JSON.stringify(angvel) === JSON.stringify({ x: 0, y: 0, z: 0 })); + assert(odomCount === 1); + assertClose(poseUpdate[3], Math.PI / 2); +}); + +Deno.test("bridge reset acknowledgement returns actual pose", () => { + const ack = applyEvalReset( + { + clearMotion() {}, + resetPose: () => ({ x: 1.25, y: 0.5, z: -2, yaw: 45 }), + }, + { + type: "evalReset", + runId: "run-1", + startPose: { x: 1, y: 0.5, z: -2, yaw: 45 }, + }, + ); + assert(ack.ok); + assert(ack.pose?.x === 1.25); + assert(ack.runId === "run-1"); +}); + +Deno.test("bridge reset fails when server physics is unavailable", () => { + const ack = applyEvalReset(null, { + type: "evalReset", + runId: "run-2", + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + assert(!ack.ok); + assert(ack.reason?.includes("unavailable")); +}); diff --git a/misc/DimSim/cli/bridge/server.ts b/misc/DimSim/cli/bridge/server.ts index 851c415e63..fa4234de90 100644 --- a/misc/DimSim/cli/bridge/server.ts +++ b/misc/DimSim/cli/bridge/server.ts @@ -19,6 +19,11 @@ import { MAGIC_SHORT, SHORT_HEADER_SIZE } from "../vendor/lcm/types.ts"; import { serveDir } from "@std/http/file-server"; import { ServerLidar } from "./lidar.ts"; import { ServerPhysics } from "./physics.ts"; +import { applyEvalReset } from "./eval-reset.ts"; +import type { + EvalAbortMessage, + EvalResetMessage, +} from "../../evals/protocol.ts"; // Magic prefix for Rapier world snapshot (ASCII "DSSN") const SNAPSHOT_MAGIC = 0x4453534E; @@ -67,12 +72,24 @@ interface ChannelState { name: string; controlClients: Set; activeControlClient: WebSocket | null; + browserControlClient: WebSocket | null; sensorClients: Set; lcm: LCM | null; sentSeqs: Set; serverLidar: ServerLidar | null; serverPhysics: ServerPhysics | null; embodiment: Record | null; + activeEval: { runId: string; controller: WebSocket } | null; + evalBrowser: { runId: string; socket: WebSocket } | null; + pendingEvalCommand: { + runId: string; + type: "runEval" | "evalStart"; + data: string; + controller: WebSocket; + delivered: Set; + } | null; + lastPoseBroadcastAt: number; + lastPoseBroadcast: { x: number; y: number; z: number; yaw: number } | null; } export async function startBridgeServer(options: BridgeServerOptions) { @@ -156,12 +173,18 @@ export async function startBridgeServer(options: BridgeServerOptions) { name, controlClients: new Set(), activeControlClient: null, + browserControlClient: null, sensorClients: new Set(), lcm: null, sentSeqs: new Set(), serverLidar: null, serverPhysics: null, embodiment: null, + activeEval: null, + evalBrowser: null, + pendingEvalCommand: null, + lastPoseBroadcastAt: 0, + lastPoseBroadcast: null, }; if (!evalOnly) { @@ -169,23 +192,23 @@ export async function startBridgeServer(options: BridgeServerOptions) { await state.lcm.start(); console.log(`[bridge] channel "${name || "default"}" LCM on ${lcmUrl}`); - // LCM → WS: forward external packets to this channel's active control client + // Consume looped-back packets so sentSeqs remains bounded. Server-side + // physics owns cmd_vel/odom, and the browser control client only handles + // JSON commands and poses; forwarding arbitrary binary LCM packets here + // creates head-of-line blocking for evalStart/reset messages. state.lcm.subscribePacket((packet: Uint8Array) => { if (packet.length < 8) return; const view = new DataView(packet.buffer, packet.byteOffset, packet.byteLength); const magic = view.getUint32(0, false); - if (magic !== MAGIC_SHORT) return; - const seq = view.getUint32(4, false); if (state.sentSeqs.has(seq)) { state.sentSeqs.delete(seq); return; } if (state.sentSeqs.size > 1000) state.sentSeqs.clear(); - - const copy = packet.slice(); - const client = state.activeControlClient; - if (client && client.readyState === WebSocket.OPEN) client.send(copy); + if (magic !== MAGIC_SHORT) return; + // External LCM packets are consumed by their server-side subscribers. + // DimosBridge intentionally has no binary control-frame handler. }); } @@ -234,6 +257,8 @@ export async function startBridgeServer(options: BridgeServerOptions) { chState.serverLidar = new ServerLidar(chState.lcm, world, RAPIER, chState.sentSeqs, chState.embodiment ?? undefined, sensorRates?.lidar); chState.serverLidar.setExcludeBody(chState.serverPhysics.getBody()); + chState.lastPoseBroadcastAt = 0; + chState.lastPoseBroadcast = null; chState.serverPhysics.setOnPoseUpdate((x, y, z, yaw) => { const t0 = PROFILE ? performance.now() : 0; @@ -241,10 +266,52 @@ export async function startBridgeServer(options: BridgeServerOptions) { const qy = Math.sin(yaw / 2); chState.serverLidar!.updatePose(x, y, z, 0, qy, 0, qw); + // Physics and odometry stay at 50 Hz, but a CPU-rendered browser + // cannot consume an unbounded stream of unchanged visual poses. + // Coalesce normal updates to 10 Hz while delivering reset-sized jumps + // immediately so eval start poses appear without visible lag. + const now = performance.now(); + const previous = chState.lastPoseBroadcast; + const positionDelta = previous + ? Math.hypot( + x - previous.x, + y - previous.y, + z - previous.z, + ) + : Infinity; + const yawDelta = previous ? Math.abs(yaw - previous.yaw) : Infinity; + if (positionDelta < 0.01 && yawDelta < 0.005) { + return; + } + const positionJump = positionDelta >= 0.5; + const yawJump = yawDelta >= 0.25; + if ( + !positionJump && + !yawJump && + now - chState.lastPoseBroadcastAt < 200 + ) { + return; + } + chState.lastPoseBroadcastAt = now; + chState.lastPoseBroadcast = { x, y, z, yaw }; + const msg = JSON.stringify({ type: "pose", x, y, z, yaw }); - const client = chState.activeControlClient; - if (client && client.readyState === WebSocket.OPEN) { - try { client.send(msg); } catch { /* ignore */ } + // Pose packets are tiny and already coalesced above. Broadcasting to + // every live control client avoids visual/scoring stalls when a + // short-lived eval runner overlaps a reconnecting browser socket. + // Consumers that do not handle pose messages simply ignore them. + const poseClients = new Set(chState.controlClients); + if (chState.browserControlClient) { + poseClients.add(chState.browserControlClient); + } + if (chState.activeControlClient) { + poseClients.add(chState.activeControlClient); + } + for (const client of poseClients) { + if (client.readyState !== WebSocket.OPEN) continue; + try { + client.send(msg); + } catch { /* ignore */ } } if (PROFILE) { const dt = performance.now() - t0; @@ -256,6 +323,13 @@ export async function startBridgeServer(options: BridgeServerOptions) { chState.serverPhysics.start(); chState.serverLidar.start(); + const readyMessage = JSON.stringify({ type: "physicsReady" }); + for (const client of chState.controlClients) { + if (client.readyState !== WebSocket.OPEN) continue; + try { + client.send(readyMessage); + } catch { /* reconnect registration retries this notification */ } + } } catch (e) { console.error(`[bridge:${chState.name || "default"}] server systems init error:`, e); } @@ -272,13 +346,23 @@ export async function startBridgeServer(options: BridgeServerOptions) { const channelParam = url.searchParams.get("channel"); const chState = resolveChannel(channelParam); const isSensor = ch !== "control"; + const isBrowserControl = !isSensor && + url.searchParams.get("client") === "browser"; const logPrefix = `[bridge:${chState.name || "default"}]`; if (isSensor) { // ── SENSOR WebSocket ────────────────────────────────────────── - socket.onopen = () => { chState.sensorClients.add(socket); }; - socket.onclose = () => { chState.sensorClients.delete(socket); }; - socket.onerror = () => chState.sensorClients.delete(socket); + socket.onopen = () => { + chState.sensorClients.add(socket); + }; + socket.onclose = () => { + chState.sensorClients.delete(socket); + }; + // A WebSocket error does not guarantee that the connection has + // closed. Keep live sockets registered until onclose; otherwise a + // transient error can leave an open browser socket receiving direct + // traffic but excluded from later broadcasts. + socket.onerror = () => {}; // Chunked snapshot reassembly state (DSC1 protocol). // Browser ships the Rapier snapshot in many small frames so a @@ -290,103 +374,281 @@ export async function startBridgeServer(options: BridgeServerOptions) { parts: Uint8Array[]; } | null = null; + let sensorMessageQueue = Promise.resolve(); socket.onmessage = (event: MessageEvent) => { - if (!(event.data instanceof ArrayBuffer) || !chState.lcm) return; - const packet = new Uint8Array(event.data); - - // While reassembling a chunked snapshot, treat every binary frame - // on this socket as the next chunk in order. - if (chunkedSnapshot) { - chunkedSnapshot.parts.push(packet); - chunkedSnapshot.received += packet.byteLength; - if (chunkedSnapshot.received >= chunkedSnapshot.total) { - const combined = new Uint8Array(chunkedSnapshot.received); - let off = 0; - for (const p of chunkedSnapshot.parts) { combined.set(p, off); off += p.byteLength; } - const snapshot = combined.subarray(0, chunkedSnapshot.total); - const spawn = chunkedSnapshot.spawn; - chunkedSnapshot = null; - initServerSystems(chState, snapshot, spawn); - } - return; - } - - // Check for Rapier snapshot - if (packet.length > 4) { - const dv = new DataView(packet.buffer, packet.byteOffset); - const magic = dv.getUint32(0, false); - - if (magic === 0x44534331) { // "DSC1" — chunked prelude - const total = dv.getUint32(4, true); - const sx = dv.getFloat32(8, true); - const sy = dv.getFloat32(12, true); - const sz = dv.getFloat32(16, true); - chunkedSnapshot = { - total, - spawn: { x: sx, y: sy, z: sz }, - received: 0, - parts: [], - }; + // Chrome and Deno can negotiate binary WebSocket messages as an + // ArrayBuffer, Blob, or typed view. Decode all three and serialize + // Blob conversion so snapshot chunks remain in wire order. + sensorMessageQueue = sensorMessageQueue.then(async () => { + if (!chState.lcm) return; + const data = event.data; + const packet = data instanceof ArrayBuffer + ? new Uint8Array(data) + : data instanceof Blob + ? new Uint8Array(await data.arrayBuffer()) + : ArrayBuffer.isView(data) + ? new Uint8Array(data.buffer, data.byteOffset, data.byteLength) + : null; + if (!packet) return; + + // While reassembling a chunked snapshot, treat every binary frame + // on this socket as the next chunk in order. + if (chunkedSnapshot) { + chunkedSnapshot.parts.push(packet); + chunkedSnapshot.received += packet.byteLength; + if (chunkedSnapshot.received >= chunkedSnapshot.total) { + const combined = new Uint8Array(chunkedSnapshot.received); + let off = 0; + for (const p of chunkedSnapshot.parts) { + combined.set(p, off); + off += p.byteLength; + } + const snapshot = combined.subarray(0, chunkedSnapshot.total); + const spawn = chunkedSnapshot.spawn; + console.log( + `${logPrefix} snapshot received bytes=${snapshot.byteLength}`, + ); + chunkedSnapshot = null; + initServerSystems(chState, snapshot, spawn); + } return; } - if (magic === 0x44535332) { // "DSS2" - const sx = dv.getFloat32(4, true); - const sy = dv.getFloat32(8, true); - const sz = dv.getFloat32(12, true); - const snapshot = packet.slice(16); - initServerSystems(chState, snapshot, { x: sx, y: sy, z: sz }); - return; - } + // Check for Rapier snapshot + if (packet.length > 4) { + const dv = new DataView(packet.buffer, packet.byteOffset); + const magic = dv.getUint32(0, false); + + if (magic === 0x44534331) { // "DSC1" — chunked prelude + const total = dv.getUint32(4, true); + const sx = dv.getFloat32(8, true); + const sy = dv.getFloat32(12, true); + const sz = dv.getFloat32(16, true); + console.log(`${logPrefix} snapshot prelude bytes=${total}`); + chunkedSnapshot = { + total, + spawn: { x: sx, y: sy, z: sz }, + received: 0, + parts: [], + }; + return; + } - if (magic === SNAPSHOT_MAGIC) { // "DSSN" - const snapshot = packet.slice(4); - initServerSystems(chState, snapshot); - return; + if (magic === 0x44535332) { // "DSS2" + const sx = dv.getFloat32(4, true); + const sy = dv.getFloat32(8, true); + const sz = dv.getFloat32(12, true); + const snapshot = packet.slice(16); + initServerSystems(chState, snapshot, { x: sx, y: sy, z: sz }); + return; + } + + if (magic === SNAPSHOT_MAGIC) { // "DSSN" + const snapshot = packet.slice(4); + initServerSystems(chState, snapshot); + return; + } } - } - const t0 = PROFILE ? performance.now() : 0; - try { - const decoded = decodePacket(packet); - if (decoded && decoded.type === "small") { - chState.sentSeqs.add(chState.lcm.getNextSeq()); - chState.lcm.publishRaw(decoded.channel, decoded.data).catch(() => {}); + const t0 = PROFILE ? performance.now() : 0; + try { + const decoded = decodePacket(packet); + if (decoded && decoded.type === "small") { + chState.sentSeqs.add(chState.lcm.getNextSeq()); + chState.lcm.publishRaw(decoded.channel, decoded.data).catch( + () => {}, + ); + } + } catch { /* ignore */ } + if (PROFILE) { + const dt = performance.now() - t0; + profRelay.n++; + profRelay.sum += dt; + profRelay.bytes += packet.byteLength; + if (dt > profRelay.max) profRelay.max = dt; } - } catch { /* ignore */ } - if (PROFILE) { - const dt = performance.now() - t0; - profRelay.n++; - profRelay.sum += dt; - profRelay.bytes += packet.byteLength; - if (dt > profRelay.max) profRelay.max = dt; - } + }).catch((error) => { + console.error(`${logPrefix} sensor message error:`, error); + }); }; } else { // ── CONTROL WebSocket ───────────────────────────────────────── - socket.onopen = () => { - if (!chState.activeControlClient || chState.activeControlClient.readyState !== WebSocket.OPEN) { + const replayPendingEval = (force = false) => { + const pending = chState.pendingEvalCommand; + if ( + !isBrowserControl || + chState.browserControlClient !== socket || + !pending || + (!force && pending.delivered.has(socket)) || + socket.readyState !== WebSocket.OPEN + ) { + return; + } + try { + socket.send(pending.data); + pending.delivered.add(socket); + } catch { /* retry on the next browser heartbeat */ } + }; + const registerControlSocket = () => { + if (isBrowserControl) { + // A reconnecting stale tab must not steal authority from a live + // browser and receive/replay its pending eval command. A browser + // that sends a fresh embodimentConfig below can explicitly claim + // authority after completing scene initialization. + if ( + !chState.browserControlClient || + chState.browserControlClient.readyState !== WebSocket.OPEN + ) { + chState.browserControlClient = socket; + chState.activeControlClient = socket; + } + } else if ( + !chState.activeControlClient || + chState.activeControlClient.readyState !== WebSocket.OPEN + ) { chState.activeControlClient = socket; } chState.controlClients.add(socket); - // quiet + replayPendingEval(); + if ( + isBrowserControl && + chState.serverPhysics && + socket.readyState === WebSocket.OPEN + ) { + try { + socket.send(JSON.stringify({ type: "physicsReady" })); + } catch { /* retry on the next registration/heartbeat */ } + } }; - socket.onerror = () => chState.controlClients.delete(socket); + // Deno may complete the server side of an upgraded WebSocket before + // an `onopen` callback assigned after upgradeWebSocket() is observed. + // Membership is safe to establish immediately: relays already guard + // on readyState, Set insertion is idempotent, and onclose remains the + // authoritative cleanup event. + registerControlSocket(); + socket.onopen = registerControlSocket; + // Do not evict on a transient error. onclose is the authoritative + // lifecycle event and performs all membership/active-client cleanup. + socket.onerror = () => {}; socket.onclose = () => { chState.controlClients.delete(socket); - if (chState.activeControlClient === socket) chState.activeControlClient = null; + if (chState.activeControlClient === socket) { + chState.activeControlClient = null; + } + if (chState.browserControlClient === socket) { + chState.browserControlClient = null; + } + if (chState.evalBrowser?.socket === socket) { + chState.evalBrowser = null; + } + if (chState.activeEval?.controller === socket) { + const runId = chState.activeEval.runId; + chState.serverPhysics?.clearMotion(); + chState.activeEval = null; + const abort: EvalAbortMessage = { + type: "evalAbort", + runId, + reason: "eval controller websocket closed", + failureStage: "socket", + }; + const encoded = JSON.stringify(abort); + for (const client of chState.controlClients) { + if (client.readyState === WebSocket.OPEN) { + try { + client.send(encoded); + } catch { /* ignore */ } + } + } + } + if (chState.pendingEvalCommand?.controller === socket) { + chState.pendingEvalCommand = null; + chState.serverPhysics?.clearMotion(); + } // quiet }; socket.onmessage = (event: MessageEvent) => { // Text messages: handle special types, relay the rest if (typeof event.data === "string") { + let relayType = ""; + let relayRunId = ""; try { const msg = JSON.parse(event.data); + relayType = typeof msg.type === "string" ? msg.type : ""; + relayRunId = typeof msg.runId === "string" ? msg.runId : ""; + + if (msg.type === "physicsReadyRequest") { + if ( + chState.serverPhysics && + socket.readyState === WebSocket.OPEN + ) { + try { + socket.send(JSON.stringify({ type: "physicsReady" })); + } catch { /* the socket lifecycle reports the failure */ } + } + return; + } + + if (msg.type === "heartbeat") { + registerControlSocket(); + try { + const pending = chState.pendingEvalCommand; + const evalBrowser = chState.evalBrowser; + const pendingTarget = pending?.type === "runEval" + ? chState.browserControlClient + : evalBrowser && evalBrowser.runId === pending?.runId + ? evalBrowser.socket + : null; + socket.send(JSON.stringify({ + type: "heartbeatAck", + ts: msg.ts, + command: pending && socket === pendingTarget + ? JSON.parse(pending.data) + : undefined, + })); + } catch { /* ignore */ } + return; + } + + if (msg.type === "runEval" || msg.type === "evalStart") { + if (msg.type === "runEval") { + chState.evalBrowser = null; + } + chState.pendingEvalCommand = { + runId: relayRunId, + type: msg.type, + data: event.data, + controller: socket, + delivered: new Set(), + }; + } else if ( + msg.type === "evalReady" && + chState.pendingEvalCommand?.type === "runEval" && + chState.pendingEvalCommand.runId === relayRunId + ) { + // Pin the complete correlated lifecycle to the browser that + // actually acknowledged this run. Later reconnects must not + // receive evalStart or terminate scoring in a different tab. + chState.evalBrowser = { runId: relayRunId, socket }; + chState.pendingEvalCommand = null; + } else if ( + (msg.type === "evalResult" || + msg.type === "evalAbort" || + msg.type === "evalCleanup") && + chState.pendingEvalCommand?.runId === relayRunId + ) { + chState.pendingEvalCommand = null; + } // -- Embodiment config: store & reconfigure running systems -- if (msg.type === "embodimentConfig") { + // This message originates from the rendered DimSim client and + // is also an application-level browser identity signal. It + // covers runtimes where the server-side WebSocket `onopen` + // event or URL marker is not observed reliably. + chState.browserControlClient = socket; + chState.activeControlClient = socket; chState.embodiment = msg.config ?? msg; console.log(`${logPrefix} embodiment config stored:`, JSON.stringify(chState.embodiment)); if (chState.serverPhysics) chState.serverPhysics.reconfigure(chState.embodiment as any); @@ -410,17 +672,138 @@ export async function startBridgeServer(options: BridgeServerOptions) { return; // don't relay teleport commands } + // -- Correlated eval reset: server physics is authoritative. -- + if (msg.type === "evalReset") { + const ack = applyEvalReset( + chState.serverPhysics, + msg as EvalResetMessage, + ); + if (ack.ok) { + chState.activeEval = { runId: ack.runId, controller: socket }; + console.log( + `${logPrefix} eval reset ${ack.runId} to ` + + `(${ack.pose!.x},${ack.pose!.y},${ack.pose!.z}) yaw=${ + ack.pose!.yaw + }`, + ); + // Deliver the authoritative pose explicitly even when the + // reset matches the last broadcast. This lets a newly + // reconnected browser synchronize before the first camera + // observation without waiting for later motion. + const resetPose = JSON.stringify({ + type: "pose", + x: ack.pose!.x, + y: ack.pose!.y, + z: ack.pose!.z, + yaw: (ack.pose!.yaw * Math.PI) / 180, + }); + for (const client of chState.controlClients) { + if (client.readyState !== WebSocket.OPEN) continue; + try { + client.send(resetPose); + } catch { /* ignore */ } + } + } + try { + socket.send(JSON.stringify(ack)); + } catch { /* ignore */ } + return; + } + + // Terminal cleanup always zeros bridge motion. Abort is relayed + // so the browser can release its pending workflow promise. + if (msg.type === "evalAbort" || msg.type === "evalCleanup") { + if ( + !chState.activeEval || + chState.activeEval.runId === msg.runId + ) { + chState.serverPhysics?.clearMotion(); + chState.activeEval = null; + } + if (msg.type === "evalCleanup") return; + } + + if ( + msg.type === "evalResult" && + chState.activeEval?.runId === msg.runId + ) { + chState.serverPhysics?.clearMotion(); + chState.activeEval = null; + } + // Relay-only: server physics is built from the boot snapshot. if (msg.type === "physicsColliderAdd" || msg.type === "physicsColliderRemove") { // handled by the browser; just relay } } catch { /* not JSON, relay as-is */ } - for (const client of chState.controlClients) { + // The active control socket is the authoritative browser/render + // client. Include it explicitly so command delivery cannot be + // lost if auxiliary membership bookkeeping ever drifts while the + // socket remains open (it may still be receiving direct poses). + const isEvalCommand = !isBrowserControl && + ( + relayType === "runEval" || + relayType === "evalStart" || + relayType === "evalAgentOutput" || + relayType === "evalAbort" + ); + const evalCommandTarget = relayType === "runEval" + ? chState.browserControlClient + : chState.evalBrowser?.runId === relayRunId + ? chState.evalBrowser.socket + : null; + // Eval commands have exactly one destination: the most recently + // identified browser/render client. Broadcasting them to every + // control socket lets a stale browser race the authoritative one + // and return an evalReady/evalResult from an older bundle. + const relayClients = isEvalCommand + ? new Set( + evalCommandTarget ? [evalCommandTarget] : [], + ) + : new Set(chState.controlClients); + if (!isEvalCommand && chState.activeControlClient) { + relayClients.add(chState.activeControlClient); + } + if (!isEvalCommand && chState.browserControlClient) { + relayClients.add(chState.browserControlClient); + } + if ( + relayType === "runEval" || + relayType === "evalStart" || + relayType === "evalAgentOutput" || + relayType === "evalAbort" + ) { + console.log( + `${logPrefix} relay ${relayType} runId=${relayRunId} ` + + `clients=${relayClients.size} browserOpen=${ + chState.browserControlClient?.readyState === WebSocket.OPEN + }`, + ); + } + for (const client of relayClients) { if (client !== socket && client.readyState === WebSocket.OPEN) { - try { client.send(event.data); } catch { /* ignore */ } + try { + client.send(event.data); + if ( + chState.pendingEvalCommand?.runId === relayRunId && + chState.pendingEvalCommand.type === relayType + ) { + chState.pendingEvalCommand.delivered.add(client); + } + } catch { /* ignore */ } } } + if ( + ( + relayType === "evalResult" || + relayType === "evalAbort" || + relayType === "evalCleanup" + ) && + chState.evalBrowser?.runId === relayRunId + ) { + chState.evalBrowser = null; + } return; } if (!(event.data instanceof ArrayBuffer) || !chState.lcm) return; diff --git a/misc/DimSim/cli/cli.ts b/misc/DimSim/cli/cli.ts index 7a5d6a79c0..c51fab58ae 100644 --- a/misc/DimSim/cli/cli.ts +++ b/misc/DimSim/cli/cli.ts @@ -13,7 +13,15 @@ import { resolve, dirname, fromFileUrl } from "@std/path"; import { startBridgeServer } from "./bridge/server.ts"; import { launchHeadless, launchMultiPage, type RenderMode } from "./headless/launcher.ts"; -import { runEvals, runEvalsMultiPage, collectWorkflows, toJunitXml } from "../evals/runner.ts"; +import { + collectWorkflows, + configurationResult, + exitCodeForResults, + formatResults, + runEvals, + runEvalsMultiPage, + type EvalResult, +} from "../evals/runner.ts"; const CLI_DIR = dirname(fromFileUrl(import.meta.url)); const PROJECT_DIR = resolve(CLI_DIR, ".."); @@ -99,7 +107,7 @@ async function resolveDistDir(): Promise { console.error(`[dimsim] No dist/ found and tryBuildFromSource() failed.`); console.error(`[dimsim] Build manually: cd ${PROJECT_DIR} && npm run build`); - Deno.exit(1); + Deno.exit(2); } function printUsage() { @@ -127,6 +135,8 @@ Dev: Eval: --connect Connect to existing bridge (use with dimos) + --agent Dispatch one workflow task through DimOS MCP + --mcp-url MCP endpoint (env: DIMOS_MCP_URL) --headless Headless Chromium (required for CI) --parallel N parallel browser pages (default: 1) --render gpu|cpu gpu = Metal/ANGLE, cpu = SwiftShader (default: cpu) @@ -143,6 +153,7 @@ const KNOWN_FLAGS = new Set([ "help", "version", "scene", "port", "headless", "render", "channels", "eval", "env", "output", "parallel", "connect", "timeout", "workflow", + "agent", "mcp-url", "camera-fov", "image-rate", "lidar-rate", "no-depth", ]); @@ -189,7 +200,7 @@ async function main() { `[dimsim] unknown flag${unknownFlags.length > 1 ? "s" : ""}: ${unknownFlags.map((f) => `--${f}`).join(", ")}`, ); console.error("[dimsim] run `dimsim help` for valid flags."); - Deno.exit(1); + Deno.exit(2); } const port = parseInt(opts.port as string) || 8090; @@ -303,6 +314,11 @@ async function main() { // ── Eval ──────────────────────────────────────────────────────────── if (subcommand === "eval") { + // Machine-readable eval output owns stdout. Route all progress from the + // runner, bridge, browser launcher, and build helpers to stderr. + const emitOutput = console.log.bind(console); + console.log = console.error.bind(console); + // Positional workflow: `dimsim eval go-to-tv` is shorthand for // `dimsim eval --workflow go-to-tv --connect`. Accepts either bare // workflow name ("go-to-tv") or scene-qualified ("apartment/go-to-tv"). @@ -326,16 +342,87 @@ async function main() { const wsUrl = `ws://localhost:${port}`; const filterScene = posScene ?? (opts.scene as string) ?? (opts.env as string); const filterWorkflow = posWorkflow ?? (opts.workflow as string); + const agentMode = opts.agent === true; + const emitAndExit = (results: EvalResult[]): never => { + emitOutput(formatResults(results, outputFormat)); + const passed = results.filter((result) => result.status === "passed").length; + const failed = results.filter((result) => result.status === "failed").length; + const errors = results.filter((result) => result.status === "error").length; + console.error( + `[dimsim] Done: ${passed} passed, ${failed} failed, ${errors} errors, ` + + `${results.length} total`, + ); + Deno.exit(exitCodeForResults(results)); + }; + + if (agentMode) { + const matches = collectWorkflows({ + scenesRoot: SCENES_DIR, + filterScene, + filterWorkflow, + }); + let reason = ""; + if (!connectMode) { + reason = "agent mode requires --connect or a positional workflow"; + } else if (opts.headless === true) { + reason = + "agent mode cannot launch a standalone --headless scorer; connect to the DimOS-launched browser"; + } else if (opts.parallel !== undefined) { + reason = "agent mode does not support --parallel"; + } else if (matches.length !== 1) { + reason = + `agent mode requires exactly one workflow; matched ${matches.length}`; + } + if (reason) { + emitAndExit([ + configurationResult(filterScene ?? "", filterWorkflow ?? "", reason), + ]); + } + } + + const mcpUrl = (opts["mcp-url"] as string | undefined) ?? + Deno.env.get("DIMOS_MCP_URL") ?? + "http://127.0.0.1:9990/mcp"; + if (agentMode) { + try { + const parsed = new URL(mcpUrl); + if (parsed.protocol !== "http:" && parsed.protocol !== "https:") { + throw new Error("expected http or https"); + } + } catch (error) { + emitAndExit([ + configurationResult( + filterScene ?? "", + filterWorkflow ?? "", + `invalid MCP URL "${mcpUrl}": ${ + error instanceof Error ? error.message : String(error) + }`, + ), + ]); + } + } // --connect mode: just run the runner against an existing bridge if (connectMode) { - console.log(`[dimsim] Connecting to existing bridge at ${wsUrl}…`); - const results = await runEvals({ wsUrl, scenesRoot: SCENES_DIR, filterScene, filterWorkflow }); - if (outputFormat === "junit") console.log(toJunitXml(results)); - const passed = results.filter((r) => r.passed).length; - const failed = results.length - passed; - console.log(`\n[dimsim] Done: ${passed} passed, ${failed} failed, ${results.length} total`); - Deno.exit(failed > 0 ? 1 : 0); + console.error(`[dimsim] Connecting to existing bridge at ${wsUrl}…`); + const results = await runEvals({ + wsUrl, + scenesRoot: SCENES_DIR, + filterScene, + filterWorkflow, + agent: agentMode, + mcpUrl, + }); + if (results.length === 0) { + emitAndExit([ + configurationResult( + filterScene ?? "", + filterWorkflow ?? "", + "no workflows match filter", + ), + ]); + } + emitAndExit(results); } const distDir = await resolveDistDir(); @@ -371,14 +458,16 @@ async function main() { }); await instance.close(); - - if (outputFormat === "junit") console.log(toJunitXml(allResults)); - else console.log(JSON.stringify(allResults, null, 2)); - - const passed = allResults.filter((r) => r.passed).length; - const failed = allResults.length - passed; - console.log(`\n[dimsim] Done: ${passed} passed, ${failed} failed, ${allResults.length} total`); - Deno.exit(failed > 0 ? 1 : 0); + if (allResults.length === 0) { + emitAndExit([ + configurationResult( + filterScene ?? "", + filterWorkflow ?? "", + "no workflows match filter", + ), + ]); + } + emitAndExit(allResults); } // -- Single worker eval (sequential) ----------------------------------- @@ -393,11 +482,18 @@ async function main() { await new Promise((r) => setTimeout(r, 3000)); const results = await runEvals({ wsUrl, scenesRoot: SCENES_DIR, filterScene, filterWorkflow }); - if (outputFormat === "junit") console.log(toJunitXml(results)); await instance.close(); - const failed = results.filter((r) => !r.passed).length; - Deno.exit(failed > 0 ? 1 : 0); + if (results.length === 0) { + emitAndExit([ + configurationResult( + filterScene ?? "", + filterWorkflow ?? "", + "no workflows match filter", + ), + ]); + } + emitAndExit(results); } else { console.log(`[dimsim] Open ${url} in your browser to start evals`); console.log("[dimsim] Press Ctrl+C to stop."); @@ -406,7 +502,7 @@ async function main() { } printUsage(); - Deno.exit(1); + Deno.exit(2); } main(); diff --git a/misc/DimSim/cli/deno.lock b/misc/DimSim/cli/deno.lock index 7cbc0c61ac..27cd20f6ed 100644 --- a/misc/DimSim/cli/deno.lock +++ b/misc/DimSim/cli/deno.lock @@ -105,6 +105,44 @@ "bin": true } }, + "remote": { + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/actionlib_msgs.mjs": "2f706dfd97204114149ca547b9b724ca944bf1ba75fc72623d021b7d22939811", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/builtin_interfaces.mjs": "f9da584bb34a9ad38d202c0421ac89602af077251c4b2c0b3a662ff234438955", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/diagnostic_msgs.mjs": "78a159ad0713650b86a8e8d64a898a47da883c47f3c0b32ab584cf1015e9409e", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/dimos__msgs.mjs": "0ccac00c5626361db8433a7a74251008e8e15f279cd6d92c3ceb25e059969e6e", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/foxglove_msgs.mjs": "09423ba04f26f6219462312d0ca467998d2a0b7e553a910589cce0a0fbdb1064", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/builtin_interfaces/Duration.mjs": "6071b8dbdf39898af805768d01b526782838dd12c9f1bb82bffa64aaa3c1e369", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/builtin_interfaces/Time.mjs": "93510f3a3920c3c2978231f122e5cbbc44b8f2bc2df368e143f4cf4d8d0b40e6", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Point.mjs": "19978e44bb2055ea6c60addd61e93ccba06db2659f3d058f7de9105a5ef13075", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Point32.mjs": "626e33e8841a5640b7006de5d581c28faee7a2b13e87ab6d05355a2c2423f244", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Pose.mjs": "1765e4383c7a7d6416362349fb3f97865622d0974c3672bad338a215f3b13667", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/PoseStamped.mjs": "871f0305ae474882ba491592afac24adfeb66db2950fc89eb90f9f8064294049", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/PoseWithCovariance.mjs": "6acd11ebb2b660676a125490c16ac3e4d003cef068f0d10bb363a233fe0859e1", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Quaternion.mjs": "0f52bb6f1e879284579e06db80e0217d64e18a20e7dbd549887b0b9ad51c8935", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Transform.mjs": "dee89ea7bcd2bce2ba367f98ec843551ecb2a0a94d69e09e85ab84bf2926b56d", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/TransformStamped.mjs": "4845ddc97b2608316d40807599e738383c6f65ad78ab7bfb7137b1aad5d15798", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Twist.mjs": "b8df01fa9746271482330b4a768aa42c7bccb9bb97e4909e42c521513f490f52", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/TwistWithCovariance.mjs": "b1b2a3990912e4a89e1514c60da4838ef63c908ce919f4548ef0bd1d1232c3c4", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Vector3.mjs": "a4752ef921dd217387cc0bc8da293a5c9c7eed177f8ffacf9328fca58a64f214", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/geometry_msgs/Wrench.mjs": "1fdd2055ab81583c18a28450dc7c699fa9463387ae7d813c67b45c4bdf322e57", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/sensor_msgs/Image.mjs": "46bcbe13aaf3d7bbf6e318dcdde79a30a670ab22dce3a10cadfa6a48dcbba600", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/sensor_msgs/RegionOfInterest.mjs": "e722e28398afd8db7457db7cb390134c1ff2d7d8fa5a16a743e4b37fc6b5553f", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/std_msgs/ColorRGBA.mjs": "e771e1f7bcfb7119e717433ad031fe037f9afb63e4680882482c3b2f2527878c", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/std_msgs/Duration.mjs": "ad03970dabf464ca247a485aeaaa14c1d8fccd181aa01ca7019148b3345ca280", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/std_msgs/Header.mjs": "69ef0a5c12b33cf57c2822667cd9bf0ee8c0f67304c0d7c5a5b5e1ac1af854f7", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/generated/std_msgs/Time.mjs": "d5e78ca2112b1b1b8fa3c96a5f5af854349c99a87d319d081dac365372aba818", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/geometry_msgs.mjs": "264d8ac36ae556851ebe4c91e55f1c9b32f7126c803cdcca8e09c9d54965fcff", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/nav_msgs.mjs": "d6797b588b531344885f3ecfeb33e8c714a74515ee375ff919642e082bd55645", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/sensor_msgs.mjs": "c6bd485819d07eee8bbfd6f92094f0067c4e7e46b68b40c1bdcb9e858d3bc3ba", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/shape_msgs.mjs": "0a309b2e24bc7761cc30712f7ddecb1a0383cdf0e9d8af5a8ef3d437ab7c6511", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/std_msgs.mjs": "2564bcb809f51f80146ed50cf8d57c3103f848dfe192b1a02f6a1acf60f298e7", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/stereo_msgs.mjs": "c07613cac332515ee816388962acb21adf7ee1bdd4616679292f1b2e02ceac36", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/tf2_msgs.mjs": "771a8a752af5715dd4c3e06d6864dbfadf6f7fd2b33b0a235b95bacf68f0f08a", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/trajectory_msgs.mjs": "fa9822243df889f5a55fa8d7ab0fcbc114c082800a7d2d946fd4e5a403438733", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/vision_msgs.mjs": "2c9c3c6d395ff36fb0b06490705f3834c7685142fafccefe0238a859ac7c2d5f", + "https://esm.sh/@jsr/dimos__msgs@0.1.4/denonext/visualization_msgs.mjs": "80599f128872ace20f59e9a3a60bf84166e4fcb1bd44531b3f4ed52562cb66f8", + "https://esm.sh/jsr/@dimos/msgs@0.1.4": "91b8ee023a7f83b1fde6f6f8a5295db9c0ac413860e1a0013ce1d2f564d517a8" + }, "workspace": { "dependencies": [ "jsr:@dimos/msgs@~0.1.4", diff --git a/misc/DimSim/docs/evals.md b/misc/DimSim/docs/evals.md index 79615045b6..31933b9fb4 100644 --- a/misc/DimSim/docs/evals.md +++ b/misc/DimSim/docs/evals.md @@ -23,11 +23,76 @@ Drop the file under any scene's `evals/` folder and `dimsim eval list` picks it ```bash dimsim eval go-to-couch # against the open sim +dimsim eval apartment/go-to-couch --agent # closed-loop through a running DimOS agent dimsim eval --headless --scene apartment --workflow go-to-couch # standalone / CI deno run -A misc/DimSim/scenes/apartment/evals/go-to-couch.js # direct execution ``` -All three end up at the same harness in the browser. Pick whichever fits the moment. +All modes end up at the same harness in the browser. Pick whichever fits the moment. + +## Closed-loop agent mode + +`--agent` evaluates an already-running DimOS agent rather than only scoring +browser state: + +```bash +# Terminal 1: start the upstream MCP-enabled agent stack with DimSim. +dimos --simulation dimsim run unitree-go2-agentic + +# Terminal 2: reset, dispatch the exact workflow task once, then score. +dimsim eval apartment/go-to-couch --agent +``` + +Agent mode requires connect mode, exactly one workflow, and no `--parallel` or +standalone `--headless` launch. The MCP endpoint is selected in this order: +`--mcp-url`, `DIMOS_MCP_URL`, then `http://127.0.0.1:9990/mcp`. API keys, model +names, and model endpoints remain DimOS configuration; DimSim never reads them. + +The correlated lifecycle is: + +```text +runEval → evalReady → evalReset → resetAck → agent_send → evalStart → evalResult + ↳ evalAgentOutput +``` + +The browser imports and validates the workflow and runs `setup`, but scoring +does not start until `evalStart`. The bridge applies `startPose` to its +authoritative Rapier body, clears prior motion, publishes the resulting +pose/odometry, and acknowledges the actual pose before `agent_send` is called. +Agent workflows therefore require finite `x`, `y`, `z`, and `yaw` fields. +An initially satisfied rubric is rejected because it cannot measure agent +behavior. + +A workflow may set `requiredAgentOutput` to require an exact, standalone +assistant response. For those workflows only, the runner starts a read-only +Python sidecar that subscribes to the existing `/agent` stream. Human messages, +tool output, AI messages with tool calls, and non-exact text do not count. +The sidecar does not add an agent tool or modify DimOS. + +The outside bathtub workflow uses this contract: + +```bash +dimsim eval apartment/find-and-go-to-bathtub --agent +``` + +It passes only after the agent emits exactly `FOUND_BATHTUB` during the active +run and the robot is within 1 metre of the bathtub. JSON output includes the +accepted message timestamp and the robot pose at which it was received. + +Results retain `passed` and add `runId`, `status`, and (for infrastructure +errors) `failureStage`. Exit codes are `0` for pass, `1` for task failure, and +`2` for configuration or infrastructure errors. Both JSON and JUnit keep +machine-readable output on stdout; progress is written to stderr. + +### Manual live-model smoke test + +With the model credentials configured in DimOS: + +1. Start `dimos --simulation dimsim run unitree-go2-agentic`. +2. Run `dimsim eval apartment/go-to-couch --agent`. +3. Confirm the logs show one `agent_send`, after `resetAck`, and scoring begins + only after dispatch. +4. Stop MCP and repeat. The command must exit `2` before `evalStart`. ## The workflow object @@ -39,6 +104,7 @@ All three end up at the same harness in the browser. Pick whichever fits the mom | `timeoutSec` | – | Default 120. Wall-clock cap. | | `startPose` | – | `{x, y, z, yaw?}`, applied before `setup`. Yaw in degrees. | | `setup(ctx)` | – | Async fn run once at start. Spawn obstacles, set props, anything. | +| `requiredAgentOutput` | – | Exact standalone AI response required in connected `--agent` mode. | ## The `ctx` object @@ -49,6 +115,7 @@ Both `setup(ctx)` and `success(ctx)` receive: | `ctx.agent` | The live agent: `setPosition`, `getPosition`, `group`, etc. | | `ctx.agentPos` | `{x, y, z}`, current translation, convenience copy. | | `ctx.sceneState` | `{assets, agentPos}`, used by rubric helpers. | +| `ctx.agentOutput` | Accepted exact agent output evidence, or `null`. | | `ctx.setAgentPose({x, y, z, yaw?})` | Teleport the agent. | | `ctx.findAsset(query)` | Case-insensitive search by title or id. | | `ctx.dist(a, b)` | Euclidean distance. | diff --git a/misc/DimSim/evals/agent-driver.ts b/misc/DimSim/evals/agent-driver.ts new file mode 100644 index 0000000000..3323c8fab0 --- /dev/null +++ b/misc/DimSim/evals/agent-driver.ts @@ -0,0 +1,906 @@ +import type { + EvalAbortMessage, + EvalAgentOutputMessage, + EvalCleanupMessage, + EvalEvidence, + EvalFailureStage, + EvalProtocolMessage, + EvalReadyMessage, + EvalResetMessage, + EvalResultMessage, + EvalStartMessage, + PhysicsReadyMessage, + PhysicsReadyRequestMessage, + ResetAckMessage, + RunEvalMessage, +} from "./protocol.ts"; +import { isFiniteStartPose } from "./protocol.ts"; +import { + type AgentIdleEvent, + type AgentOutputEvent, + type AgentOutputObserver, + createAgentOutputObserver, +} from "./agent-output.ts"; + +export interface EvalSocket extends EventTarget { + readonly readyState: number; + send(data: string): void; + close(): void; +} + +export interface McpTool { + name: string; +} + +export interface McpTransport { + listTools(url: string, timeoutMs: number): Promise; + callTool( + url: string, + name: string, + args: Record, + timeoutMs: number, + ): Promise; +} + +export interface AgentWorkflow { + scene: string; + workflow: string; + url: string; +} + +export interface AgentEvalTimeouts { + physicsReadyMs: number; + browserReadyMs: number; + resetMs: number; + sensorSettleMs: number; + agentOutputMs: number; + agentIdleProbeMs: number; + agentDispatchMs: number; + agentIdleMs: number; + mcpMs: number; + resultGraceMs: number; +} + +export const DEFAULT_AGENT_TIMEOUTS: AgentEvalTimeouts = { + // Restoring the apartment's ~27 MB Rapier snapshot can take around a minute + // in a CPU-rendered background browser. + physicsReadyMs: 120_000, + browserReadyMs: 30_000, + resetMs: 5_000, + sensorSettleMs: 10_000, + agentOutputMs: 5_000, + agentIdleProbeMs: 1_000, + agentDispatchMs: 5_000, + agentIdleMs: 60_000, + mcpMs: 10_000, + // A CPU-rendered background tab can deliver a workflow's final timer tick + // several seconds late even though its control heartbeat remains healthy. + // Keep the result watchdog bounded while leaving enough time for the + // browser's already-computed result to cross the WebSocket. + resultGraceMs: 15_000, +}; + +const RESET_POSITION_TOLERANCE_M = 0.25; +const RESET_YAW_TOLERANCE_DEG = 5; + +export interface AgentEvalResult { + runId: string; + scene: string; + workflow: string; + workflowUrl: string; + task: string; + passed: boolean; + status: "passed" | "failed" | "error"; + failureStage?: EvalFailureStage; + reason: string; + score: number | null; + durationMs: number; + evidence?: EvalEvidence; +} + +interface WaitOptions { + runId: string; + timeoutMs: number; + stage: EvalFailureStage; + types: Set; + signal?: AbortSignal; +} + +class AgentEvalError extends Error { + constructor( + message: string, + readonly stage: EvalFailureStage, + ) { + super(message); + } +} + +/** Minimal JSON-over-HTTP client for the DimOS MCP endpoint. */ +export class HttpMcpTransport implements McpTransport { + private requestId = 0; + + async listTools(url: string, timeoutMs: number): Promise { + const result = await this._request(url, "tools/list", {}, timeoutMs); + if (!result || typeof result !== "object") { + throw new Error("MCP tools/list returned an invalid result"); + } + const tools = (result as { tools?: unknown }).tools; + if (!Array.isArray(tools)) { + throw new Error("MCP tools/list response is missing tools"); + } + return tools.filter( + (tool): tool is McpTool => + !!tool && typeof tool === "object" && + typeof (tool as { name?: unknown }).name === "string", + ); + } + + async callTool( + url: string, + name: string, + args: Record, + timeoutMs: number, + ): Promise { + const result = await this._request( + url, + "tools/call", + { name, arguments: args }, + timeoutMs, + ); + if (result && typeof result === "object") { + const toolResult = result as { + isError?: unknown; + content?: Array<{ type?: unknown; text?: unknown }>; + }; + const text = toolResult.content + ?.filter((item) => + item?.type === "text" && typeof item.text === "string" + ) + .map((item) => item.text as string) + .join("\n") ?? ""; + if ( + toolResult.isError === true || + /^(Error running tool|Tool not found:|Cannot start\b)/.test(text) + ) { + throw new Error(text || `MCP tool ${name} failed`); + } + } + return result; + } + + private async _request( + url: string, + method: string, + params: Record, + timeoutMs: number, + ): Promise { + const controller = new AbortController(); + const timer = setTimeout(() => controller.abort(), timeoutMs); + try { + const response = await fetch(url, { + method: "POST", + headers: { + "content-type": "application/json", + "accept": "application/json, text/event-stream", + }, + body: JSON.stringify({ + jsonrpc: "2.0", + id: ++this.requestId, + method, + params, + }), + signal: controller.signal, + }); + if (!response.ok) { + throw new Error(`MCP ${method} failed with HTTP ${response.status}`); + } + const body = await response.json(); + if (body?.error) { + const detail = body.error.message ?? JSON.stringify(body.error); + throw new Error(`MCP ${method} error: ${detail}`); + } + if (!Object.hasOwn(body ?? {}, "result")) { + throw new Error(`MCP ${method} response is missing result`); + } + return body.result; + } catch (error) { + if (controller.signal.aborted) { + throw new Error(`MCP ${method} timed out after ${timeoutMs}ms`); + } + throw error; + } finally { + clearTimeout(timer); + } + } +} + +function send(socket: EvalSocket, message: EvalProtocolMessage): void { + socket.send(JSON.stringify(message)); +} + +function yawDeltaDeg(a: number, b: number): number { + const delta = ((a - b + 180) % 360 + 360) % 360 - 180; + return Math.abs(delta); +} + +async function withTimeout( + operation: Promise, + timeoutMs: number, + label: string, +): Promise { + let timer: ReturnType | undefined; + try { + return await Promise.race([ + operation, + new Promise((_resolve, reject) => { + timer = setTimeout( + () => reject(new Error(`${label} timed out after ${timeoutMs}ms`)), + timeoutMs, + ); + }), + ]); + } finally { + if (timer !== undefined) clearTimeout(timer); + } +} + +async function probeWithTimeout( + operation: Promise, + timeoutMs: number, +): Promise { + let timer: ReturnType | undefined; + try { + return await Promise.race([ + operation, + new Promise((resolve) => { + timer = setTimeout(() => resolve(null), timeoutMs); + }), + ]); + } finally { + if (timer !== undefined) clearTimeout(timer); + } +} + +function waitForMessage( + socket: EvalSocket, + options: WaitOptions, +): Promise { + return new Promise((resolve, reject) => { + let settled = false; + const cleanup = () => { + clearTimeout(timer); + socket.removeEventListener("message", onMessage); + socket.removeEventListener("error", onSocketFailure); + socket.removeEventListener("close", onSocketFailure); + options.signal?.removeEventListener("abort", onAbort); + }; + const settle = (fn: () => void) => { + if (settled) return; + settled = true; + cleanup(); + fn(); + }; + const onMessage = (event: Event) => { + const data = (event as MessageEvent).data; + if (typeof data !== "string") return; + let message: EvalProtocolMessage; + try { + message = JSON.parse(data) as EvalProtocolMessage; + } catch { + return; + } + if (!message || message.runId !== options.runId) return; + if (message.type === "evalResult") { + settle(() => resolve(message)); + return; + } + if (!options.types.has(message.type)) return; + settle(() => resolve(message)); + }; + const onSocketFailure = (event: Event) => { + settle(() => + reject( + new AgentEvalError( + event.type === "close" + ? "websocket closed during agent eval" + : "websocket error during agent eval", + "socket", + ), + ) + ); + }; + const onAbort = () => { + settle(() => + reject( + new AgentEvalError( + `${options.stage} wait cancelled`, + options.stage, + ), + ) + ); + }; + const timer = setTimeout(() => { + settle(() => + reject( + new AgentEvalError( + `${options.stage} timed out after ${options.timeoutMs}ms`, + options.stage, + ), + ) + ); + }, options.timeoutMs); + socket.addEventListener("message", onMessage); + socket.addEventListener("error", onSocketFailure); + socket.addEventListener("close", onSocketFailure); + options.signal?.addEventListener("abort", onAbort, { once: true }); + }); +} + +/** + * Establish that the bridge has restored browser physics before asking the + * browser to import a workflow. The explicit request closes the race where a + * one-shot `physicsReady` broadcast happened before this socket subscribed. + */ +function waitForPhysicsReady( + socket: EvalSocket, + timeoutMs: number, +): Promise { + return new Promise((resolve, reject) => { + let settled = false; + const cleanup = () => { + clearTimeout(timer); + socket.removeEventListener("message", onMessage); + socket.removeEventListener("error", onSocketFailure); + socket.removeEventListener("close", onSocketFailure); + }; + const settle = (fn: () => void) => { + if (settled) return; + settled = true; + cleanup(); + fn(); + }; + const onMessage = (event: Event) => { + const data = (event as MessageEvent).data; + if (typeof data !== "string") return; + try { + const message = JSON.parse(data) as Partial; + if (message.type !== "physicsReady") return; + } catch { + return; + } + settle(resolve); + }; + const onSocketFailure = (event: Event) => { + settle(() => + reject( + new AgentEvalError( + event.type === "close" + ? "websocket closed while waiting for bridge physics" + : "websocket error while waiting for bridge physics", + "socket", + ), + ) + ); + }; + const timer = setTimeout(() => { + settle(() => + reject( + new AgentEvalError( + `physicsReady timed out after ${timeoutMs}ms`, + "physicsReady", + ), + ) + ); + }, timeoutMs); + socket.addEventListener("message", onMessage); + socket.addEventListener("error", onSocketFailure); + socket.addEventListener("close", onSocketFailure); + + try { + const request: PhysicsReadyRequestMessage = { + type: "physicsReadyRequest", + }; + socket.send(JSON.stringify(request)); + } catch (error) { + settle(() => + reject( + new AgentEvalError( + error instanceof Error ? error.message : String(error), + "socket", + ), + ) + ); + } + }); +} + +function normalizeResult( + workflow: AgentWorkflow, + runId: string, + message: EvalResultMessage, +): AgentEvalResult { + const status = message.status ?? + (message.passed ? "passed" : "failed"); + return { + runId, + scene: workflow.scene, + workflow: workflow.workflow, + workflowUrl: workflow.url, + task: message.task ?? "", + passed: status === "passed" && !!message.passed, + status, + failureStage: message.failureStage, + reason: message.reason ?? (message.passed ? "ok" : "fail"), + score: typeof message.score === "number" ? message.score : null, + durationMs: typeof message.durationMs === "number" ? message.durationMs : 0, + evidence: message.evidence, + }; +} + +function errorResult( + workflow: AgentWorkflow, + runId: string, + error: unknown, +): AgentEvalResult { + const stage = error instanceof AgentEvalError ? error.stage : "mcp"; + return { + runId, + scene: workflow.scene, + workflow: workflow.workflow, + workflowUrl: workflow.url, + task: "", + passed: false, + status: "error", + failureStage: stage, + reason: error instanceof Error ? error.message : String(error), + score: null, + durationMs: 0, + }; +} + +/** + * Run the correlated agent lifecycle on an already-open bridge socket. + * The workflow task is learned from the browser and sent to `agent_send` + * exactly once, after the authoritative bridge reset succeeds. + */ +export async function runAgentEvalOnSocket(options: { + socket: EvalSocket; + workflow: AgentWorkflow; + mcpUrl: string; + mcp?: McpTransport; + agentOutputObserverFactory?: () => AgentOutputObserver; + runId?: string; + timeouts?: Partial; +}): Promise { + const { + socket, + workflow, + mcpUrl, + mcp = new HttpMcpTransport(), + runId = crypto.randomUUID(), + } = options; + const timeouts = { ...DEFAULT_AGENT_TIMEOUTS, ...options.timeouts }; + let tools: McpTool[] = []; + let terminalError: AgentEvalError | null = null; + let finalResult: AgentEvalResult | null = null; + let outputObserver: AgentOutputObserver | null = null; + let requiredAgentOutput: string | undefined; + let captureAgentOutput = false; + let scoringStarted = false; + let outputForwarded = false; + let pendingAgentOutput: AgentOutputEvent | null = null; + let resultWaitAbort: AbortController | null = null; + let latestAgentIdle: boolean | null = null; + let idleSupported = false; + let dispatchWindowOpen = false; + let sawDispatchedTurnBusy = false; + let resolveFirstIdle!: (event: AgentIdleEvent) => void; + let firstIdleResolved = false; + const firstIdle = new Promise((resolve) => { + resolveFirstIdle = resolve; + }); + let resolveIdle!: () => void; + let idlePromise = new Promise((resolve) => { + resolveIdle = resolve; + }); + let resolveDispatchedTurnIdle!: () => void; + const dispatchedTurnIdle = new Promise((resolve) => { + resolveDispatchedTurnIdle = resolve; + }); + let resolveDispatchedTurnBusy!: () => void; + const dispatchedTurnBusy = new Promise((resolve) => { + resolveDispatchedTurnBusy = resolve; + }); + + const forwardAgentOutput = (event: AgentOutputEvent) => { + if (outputForwarded) return; + const message: EvalAgentOutputMessage = { + type: "evalAgentOutput", + runId, + text: event.text.trim(), + timestampMs: event.timestampMs, + }; + send(socket, message); + outputForwarded = true; + }; + + const recordResult = (result: AgentEvalResult): AgentEvalResult => { + finalResult = result; + return result; + }; + + const recordIsolationFailure = (error: unknown) => { + if (!finalResult || finalResult.status === "error") return; + finalResult.passed = false; + finalResult.status = "error"; + finalResult.failureStage = "agentIdle"; + finalResult.reason = error instanceof Error ? error.message : String(error); + }; + + const onAgentOutput = (event: AgentOutputEvent) => { + if ( + !captureAgentOutput || + !requiredAgentOutput || + event.hasToolCalls || + event.text.trim() !== requiredAgentOutput || + pendingAgentOutput + ) { + return; + } + pendingAgentOutput = event; + if (scoringStarted) forwardAgentOutput(event); + }; + + const onAgentIdle = (event: AgentIdleEvent) => { + latestAgentIdle = event.idle; + if (!firstIdleResolved) { + firstIdleResolved = true; + resolveFirstIdle(event); + } + if (event.idle) { + resolveIdle(); + if (dispatchWindowOpen && sawDispatchedTurnBusy) { + resolveDispatchedTurnIdle(); + } + return; + } + idlePromise = new Promise((resolve) => { + resolveIdle = resolve; + }); + if (dispatchWindowOpen) { + sawDispatchedTurnBusy = true; + resolveDispatchedTurnBusy(); + } + }; + + const raceObserverFailure = async (operation: Promise): Promise => { + if (!outputObserver) return await operation; + return await Promise.race([ + operation, + outputObserver.failure.then((error) => { + throw new AgentEvalError(error.message, "agentOutput"); + }), + ]); + }; + + try { + await waitForPhysicsReady(socket, timeouts.physicsReadyMs); + + const runMessage: RunEvalMessage = { + type: "runEval", + runId, + workflowUrl: workflow.url, + agent: true, + }; + const readyPromise = waitForMessage(socket, { + runId, + timeoutMs: timeouts.browserReadyMs, + stage: "browserReady", + types: new Set(["evalReady"]), + }); + send(socket, runMessage); + const readyOrResult = await readyPromise; + if (readyOrResult.type === "evalResult") { + return recordResult(normalizeResult(workflow, runId, readyOrResult)); + } + const ready = readyOrResult as EvalReadyMessage; + if ( + ready.workflowUrl !== workflow.url || + typeof ready.task !== "string" || + ready.task.length === 0 || + !Number.isFinite(ready.timeoutMs) || + ready.timeoutMs <= 0 || + !isFiniteStartPose(ready.startPose) || + (ready.requiredAgentOutput !== undefined && + (typeof ready.requiredAgentOutput !== "string" || + ready.requiredAgentOutput.trim().length === 0)) + ) { + throw new AgentEvalError( + "browser returned invalid evalReady data", + "browserReady", + ); + } + requiredAgentOutput = ready.requiredAgentOutput?.trim(); + + if (requiredAgentOutput || timeouts.agentIdleProbeMs > 0) { + outputObserver = options.agentOutputObserverFactory?.() ?? + createAgentOutputObserver(); + try { + await withTimeout( + outputObserver.start(onAgentOutput, onAgentIdle), + timeouts.agentOutputMs, + "agent output sidecar", + ); + } catch (error) { + throw new AgentEvalError( + error instanceof Error ? error.message : String(error), + "agentOutput", + ); + } + } + + if (outputObserver && timeouts.agentIdleProbeMs > 0) { + const firstState = await probeWithTimeout( + raceObserverFailure(firstIdle), + timeouts.agentIdleProbeMs, + ); + if (firstState) { + idleSupported = true; + if (!firstState.idle) { + try { + await withTimeout( + raceObserverFailure(idlePromise), + timeouts.agentIdleMs, + "prior DimSim agent turn", + ); + } catch (error) { + throw new AgentEvalError( + error instanceof Error ? error.message : String(error), + "agentIdle", + ); + } + } + } else { + console.error( + "[agent-eval] agent idle stream unavailable; continuing without the optional isolation barrier", + ); + } + } + + const reset: EvalResetMessage = { + type: "evalReset", + runId, + startPose: ready.startPose, + }; + const resetPromise = waitForMessage(socket, { + runId, + timeoutMs: timeouts.resetMs, + stage: "reset", + types: new Set(["resetAck"]), + }); + send(socket, reset); + const resetOrResult = await resetPromise; + if (resetOrResult.type === "evalResult") { + return recordResult(normalizeResult(workflow, runId, resetOrResult)); + } + const ack = resetOrResult as ResetAckMessage; + if (!ack.ok || !isFiniteStartPose(ack.pose)) { + throw new AgentEvalError( + ack.reason || "bridge reset failed or returned an invalid pose", + "reset", + ); + } + const resetPositionError = Math.hypot( + ack.pose.x - ready.startPose.x, + ack.pose.y - ready.startPose.y, + ack.pose.z - ready.startPose.z, + ); + const resetYawError = yawDeltaDeg(ack.pose.yaw, ready.startPose.yaw); + if ( + resetPositionError > RESET_POSITION_TOLERANCE_M || + resetYawError > RESET_YAW_TOLERANCE_DEG + ) { + throw new AgentEvalError( + `bridge reset acknowledgement mismatched requested pose: ` + + `position error ${resetPositionError.toFixed(3)}m, ` + + `yaw error ${resetYawError.toFixed(1)}deg`, + "reset", + ); + } + + // Camera frames are produced asynchronously by the browser and then + // consumed by the Python agent. Give that pipeline a bounded interval to + // replace any frame captured immediately before the authoritative reset. + // Scoring still begins only after dispatch and evalStart below. + if (timeouts.sensorSettleMs > 0) { + console.error( + `[agent-eval] waiting ${timeouts.sensorSettleMs}ms for a post-reset camera frame`, + ); + await new Promise((resolve) => + setTimeout(resolve, timeouts.sensorSettleMs) + ); + if (socket.readyState !== WebSocket.OPEN) { + throw new AgentEvalError( + "bridge socket closed while waiting for post-reset sensors", + "socket", + ); + } + } + + try { + const mcpDeadline = Date.now() + timeouts.mcpMs; + tools = await withTimeout( + raceObserverFailure(mcp.listTools(mcpUrl, timeouts.mcpMs)), + timeouts.mcpMs, + "MCP tools/list", + ); + if (!tools.some((tool) => tool.name === "agent_send")) { + throw new Error( + "MCP server does not advertise required tool agent_send", + ); + } + const remainingMs = mcpDeadline - Date.now(); + if (remainingMs <= 0) { + throw new Error(`MCP stage timed out after ${timeouts.mcpMs}ms`); + } + captureAgentOutput = true; + dispatchWindowOpen = true; + await withTimeout( + raceObserverFailure( + mcp.callTool( + mcpUrl, + "agent_send", + { message: ready.task }, + remainingMs, + ), + ), + remainingMs, + "MCP agent_send", + ); + if (idleSupported) { + try { + await withTimeout( + raceObserverFailure(dispatchedTurnBusy), + timeouts.agentDispatchMs, + "DimSim agent dispatch", + ); + } catch (error) { + throw new AgentEvalError( + error instanceof Error ? error.message : String(error), + "agentIdle", + ); + } + } + } catch (error) { + captureAgentOutput = false; + if (error instanceof AgentEvalError) throw error; + throw new AgentEvalError( + error instanceof Error ? error.message : String(error), + "mcp", + ); + } + + const start: EvalStartMessage = { type: "evalStart", runId }; + resultWaitAbort = new AbortController(); + const resultPromise = waitForMessage(socket, { + runId, + timeoutMs: ready.timeoutMs + timeouts.resultGraceMs, + stage: "result", + types: new Set(["evalResult"]), + signal: resultWaitAbort.signal, + }); + send(socket, start); + scoringStarted = true; + if (pendingAgentOutput) forwardAgentOutput(pendingAgentOutput); + const result = await raceObserverFailure( + resultPromise as Promise, + ); + captureAgentOutput = false; + return recordResult(normalizeResult(workflow, runId, result)); + } catch (error) { + terminalError = error instanceof AgentEvalError + ? error + : new AgentEvalError( + error instanceof Error ? error.message : String(error), + "mcp", + ); + return recordResult(errorResult(workflow, runId, terminalError)); + } finally { + captureAgentOutput = false; + resultWaitAbort?.abort(); + if (terminalError) { + try { + const abort: EvalAbortMessage = { + type: "evalAbort", + runId, + reason: terminalError.message, + failureStage: terminalError.stage, + }; + send(socket, abort); + } catch { + // Socket failures are already represented by the terminal result. + } + } + let cancellationFailure: unknown; + if (outputObserver && dispatchWindowOpen) { + try { + await withTimeout( + outputObserver.cancelActiveTurn(runId), + timeouts.agentDispatchMs, + "DimSim agent turn cancellation", + ); + } catch (error) { + cancellationFailure = error; + console.error( + `[runner] agent turn cancellation failed: ${ + error instanceof Error ? error.message : String(error) + }`, + ); + } + } + for ( + const cleanupTool of [ + "end_exploration", + "stop_looking_out", + "stop_navigation", + ] + ) { + if (!tools.some((tool) => tool.name === cleanupTool)) continue; + try { + await withTimeout( + mcp.callTool( + mcpUrl, + cleanupTool, + {}, + timeouts.mcpMs, + ), + timeouts.mcpMs, + `MCP ${cleanupTool}`, + ); + } catch (error) { + console.error( + `[runner] cleanup ${cleanupTool} failed: ${ + error instanceof Error ? error.message : String(error) + }`, + ); + } + } + if (idleSupported && dispatchWindowOpen) { + try { + await withTimeout( + raceObserverFailure(dispatchedTurnIdle), + timeouts.agentIdleMs, + "DimSim agent turn completion", + ); + cancellationFailure = undefined; + } catch (error) { + cancellationFailure = error; + console.error( + `[runner] agent isolation barrier failed: ${ + error instanceof Error ? error.message : String(error) + }`, + ); + } + } + if (cancellationFailure) { + recordIsolationFailure(cancellationFailure); + } + try { + const cleanup: EvalCleanupMessage = { type: "evalCleanup", runId }; + send(socket, cleanup); + } catch { + // Best effort: the bridge also clears motion when the eval socket closes. + } + if (outputObserver) { + try { + await outputObserver.stop(); + } catch (error) { + console.error( + `[runner] cleanup agent output sidecar failed: ${ + error instanceof Error ? error.message : String(error) + }`, + ); + } + } + } +} diff --git a/misc/DimSim/evals/agent-driver_test.ts b/misc/DimSim/evals/agent-driver_test.ts new file mode 100644 index 0000000000..3f33dfce49 --- /dev/null +++ b/misc/DimSim/evals/agent-driver_test.ts @@ -0,0 +1,1076 @@ +import { + type EvalSocket, + HttpMcpTransport, + type McpTool, + type McpTransport, + runAgentEvalOnSocket, +} from "./agent-driver.ts"; +import type { + AgentIdleEvent, + AgentOutputEvent, + AgentOutputObserver, +} from "./agent-output.ts"; + +function assert( + condition: unknown, + message = "assertion failed", +): asserts condition { + if (!condition) throw new Error(message); +} + +function assertEquals(actual: unknown, expected: unknown): void { + const a = JSON.stringify(actual); + const e = JSON.stringify(expected); + if (a !== e) throw new Error(`expected ${e}, got ${a}`); +} + +class FakeSocket extends EventTarget implements EvalSocket { + readyState: number = WebSocket.OPEN; + sent: Array> = []; + onSend?: (message: Record) => void; + autoPhysicsReady = true; + + send(data: string): void { + const message = JSON.parse(data) as Record; + this.sent.push(message); + this.onSend?.(message); + if (message.type === "physicsReadyRequest" && this.autoPhysicsReady) { + queueMicrotask(() => this.emit({ type: "physicsReady" })); + } + } + + emit(message: Record): void { + this.dispatchEvent( + new MessageEvent("message", { data: JSON.stringify(message) }), + ); + } + + close(): void { + this.readyState = WebSocket.CLOSED; + } +} + +class FakeMcp implements McpTransport { + calls: Array<{ name: string; args?: Record }> = []; + + constructor( + readonly tools: McpTool[], + readonly failTool?: string, + readonly onCall?: ( + name: string, + args: Record, + ) => void, + ) {} + + listTools(_url: string, _timeoutMs: number): Promise { + this.calls.push({ name: "tools/list" }); + return Promise.resolve(this.tools); + } + + callTool( + _url: string, + name: string, + args: Record, + _timeoutMs: number, + ): Promise { + this.calls.push({ name, args }); + this.onCall?.(name, args); + if (name === this.failTool) { + return Promise.reject(new Error(`${name} failed`)); + } + return Promise.resolve({ ok: true }); + } +} + +class FakeAgentOutputObserver implements AgentOutputObserver { + started = false; + stopped = false; + cancelledRunIds: string[] = []; + private handler?: (event: AgentOutputEvent) => void; + private idleHandler?: (event: AgentIdleEvent) => void; + private failObserver!: (error: Error) => void; + readonly failure: Promise; + + constructor( + readonly startError?: Error, + readonly onStart?: (observer: FakeAgentOutputObserver) => void, + readonly cancelError?: Error, + readonly onCancel?: ( + runId: string, + observer: FakeAgentOutputObserver, + ) => void, + ) { + this.failure = new Promise((resolve) => { + this.failObserver = resolve; + }); + } + + start( + handler: (event: AgentOutputEvent) => void, + idleHandler?: (event: AgentIdleEvent) => void, + ): Promise { + this.started = true; + this.handler = handler; + this.idleHandler = idleHandler; + if (this.startError) return Promise.reject(this.startError); + this.onStart?.(this); + return Promise.resolve(); + } + + emitIdle(idle: boolean, timestampMs = 1234): void { + this.idleHandler?.({ + type: "agent_idle", + idle, + timestampMs, + }); + } + + emit( + text: string, + hasToolCalls = false, + timestampMs = 1234, + ): void { + this.handler?.({ + type: "agent_output", + text, + hasToolCalls, + timestampMs, + }); + } + + fail(message: string): void { + this.failObserver(new Error(message)); + } + + cancelActiveTurn(runId: string): Promise { + this.cancelledRunIds.push(runId); + this.onCancel?.(runId, this); + if (this.cancelError) return Promise.reject(this.cancelError); + return Promise.resolve(); + } + + stop(): Promise { + this.stopped = true; + return Promise.resolve(); + } +} + +const workflow = { + scene: "apartment", + workflow: "go-to-couch", + url: "/scenes/apartment/evals/go-to-couch.js", +}; + +Deno.test("agent eval orders reset, one dispatch, start, result, and cleanup", async () => { + const socket = new FakeSocket(); + const mcp = new FakeMcp([ + { name: "agent_send" }, + { name: "stop_navigation" }, + ]); + const runId = "run-current"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId: "run-stale", + workflowUrl: workflow.url, + scene: workflow.scene, + task: "wrong task", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalStart") { + socket.emit({ + type: "evalResult", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + passed: true, + status: "passed", + reason: "at couch", + durationMs: 12, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentIdleProbeMs: 0, + mcpMs: 50, + resultGraceMs: 50, + }, + }); + + assert(result.passed, JSON.stringify(result)); + assertEquals( + socket.sent.map((message) => message.type), + [ + "physicsReadyRequest", + "runEval", + "evalReset", + "evalStart", + "evalCleanup", + ], + ); + assertEquals( + mcp.calls, + [ + { name: "tools/list" }, + { + name: "agent_send", + args: { message: "Go to the couch" }, + }, + { name: "stop_navigation", args: {} }, + ], + ); + assertEquals(result.runId, runId); +}); + +Deno.test("agent eval fences dispatch and terminal cleanup with DimSim idle state", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver( + undefined, + (started) => started.emitIdle(true), + undefined, + (_runId, cancelling) => cancelling.emitIdle(true), + ); + const mcp = new FakeMcp( + [ + { name: "agent_send" }, + { name: "stop_navigation" }, + ], + undefined, + (name) => { + if (name === "agent_send") observer.emitIdle(false); + }, + ); + const runId = "idle-fenced-run"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } else if (message.type === "evalStart") { + socket.emit({ + type: "evalResult", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + passed: true, + status: "passed", + reason: "at couch", + durationMs: 12, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 50, + agentDispatchMs: 50, + agentIdleMs: 50, + mcpMs: 50, + resultGraceMs: 50, + }, + }); + + assert(result.passed, JSON.stringify(result)); + assert(observer.started); + assert(observer.stopped); + assertEquals(observer.cancelledRunIds, [runId]); + assertEquals( + socket.sent.map((message) => message.type), + [ + "physicsReadyRequest", + "runEval", + "evalReset", + "evalStart", + "evalCleanup", + ], + ); + assertEquals( + mcp.calls.map((call) => call.name), + ["tools/list", "agent_send", "stop_navigation"], + ); +}); + +Deno.test("agent eval reports an isolation error when turn cancellation cannot stop the active turn", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver( + undefined, + (started) => started.emitIdle(true), + new Error("control channel unavailable"), + ); + const mcp = new FakeMcp( + [{ name: "agent_send" }, { name: "stop_navigation" }], + undefined, + (name) => { + if (name === "agent_send") observer.emitIdle(false); + }, + ); + const runId = "turn-cancel-failure"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } else if (message.type === "evalStart") { + socket.emit({ + type: "evalResult", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + passed: true, + status: "passed", + reason: "at couch", + durationMs: 12, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 50, + agentDispatchMs: 20, + agentIdleMs: 20, + mcpMs: 50, + resultGraceMs: 50, + }, + }); + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "agentIdle"); + assert(result.reason.includes("timed out"), result.reason); + assertEquals(observer.cancelledRunIds, [runId]); +}); + +Deno.test("agent eval refuses to start scoring without a correlated busy edge", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver( + undefined, + (started) => started.emitIdle(true), + ); + const runId = "missing-busy-edge"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 50, + agentDispatchMs: 5, + agentIdleMs: 5, + mcpMs: 50, + }, + }); + + assertEquals(result.failureStage, "agentIdle"); + assert(!socket.sent.some((message) => message.type === "evalStart")); + assert(observer.stopped); +}); + +Deno.test("agent eval forwards only exact tool-free required output and owns sidecar", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver( + undefined, + (started) => started.emit("FOUND_BATHTUB"), + ); + const mcp = new FakeMcp([ + { name: "agent_send" }, + { name: "end_exploration" }, + { name: "stop_looking_out" }, + { name: "stop_navigation" }, + ]); + const runId = "required-output-run"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Find the bathtub", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + requiredAgentOutput: "FOUND_BATHTUB", + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalStart") { + observer.emit("FOUND_BATHTUB", true); + observer.emit("I found it: FOUND_BATHTUB"); + observer.emit(" FOUND_BATHTUB ", false, 5678); + } else if (message.type === "evalAgentOutput") { + socket.emit({ + type: "evalResult", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Find the bathtub", + passed: true, + status: "passed", + reason: "declared and nearby", + durationMs: 12, + evidence: { + agentOutput: { + text: "FOUND_BATHTUB", + timestampMs: 5678, + pose: { x: 1, y: 0.5, z: 2, yaw: 0 }, + }, + }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 0, + mcpMs: 50, + resultGraceMs: 50, + }, + }); + + assert(result.passed, JSON.stringify(result)); + assert(observer.started); + assert(observer.stopped); + assertEquals( + socket.sent.map((message) => message.type), + [ + "physicsReadyRequest", + "runEval", + "evalReset", + "evalStart", + "evalAgentOutput", + "evalCleanup", + ], + ); + assertEquals( + mcp.calls.map((call) => call.name), + [ + "tools/list", + "agent_send", + "end_exploration", + "stop_looking_out", + "stop_navigation", + ], + ); + assertEquals(result.evidence?.agentOutput?.timestampMs, 5678); +}); + +Deno.test("agent eval classifies sidecar startup failure before dispatch", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver( + new Error("observer unavailable"), + ); + const mcp = new FakeMcp([{ name: "agent_send" }]); + const runId = "sidecar-start-failure"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Find the bathtub", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + requiredAgentOutput: "FOUND_BATHTUB", + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 0, + }, + }); + + assertEquals(result.failureStage, "agentOutput"); + assertEquals(mcp.calls, []); + assert(observer.stopped); + assert(!socket.sent.some((message) => message.type === "evalStart")); +}); + +Deno.test("agent eval aborts if required-output sidecar exits during scoring", async () => { + const socket = new FakeSocket(); + const observer = new FakeAgentOutputObserver(); + const runId = "sidecar-runtime-failure"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Find the bathtub", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + requiredAgentOutput: "FOUND_BATHTUB", + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalStart") { + observer.fail("observer exited"); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId, + agentOutputObserverFactory: () => observer, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentOutputMs: 50, + agentIdleProbeMs: 0, + mcpMs: 50, + resultGraceMs: 50, + }, + }); + + assertEquals(result.failureStage, "agentOutput"); + assert(observer.stopped); + assert( + socket.sent.some((message) => + message.type === "evalAbort" && + message.failureStage === "agentOutput" + ), + ); +}); + +Deno.test("agent eval browser-ready watchdog aborts and cleans up", async () => { + const socket = new FakeSocket(); + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId: "watchdog-run", + timeouts: { browserReadyMs: 5 }, + }); + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "browserReady"); + assertEquals( + socket.sent.map((message) => message.type), + ["physicsReadyRequest", "runEval", "evalAbort", "evalCleanup"], + ); +}); + +Deno.test("agent eval waits for delayed bridge physics before runEval", async () => { + const socket = new FakeSocket(); + socket.autoPhysicsReady = false; + socket.onSend = (message) => { + if (message.type === "physicsReadyRequest") { + setTimeout(() => socket.emit({ type: "physicsReady" }), 5); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId: "delayed-physics", + timeouts: { + physicsReadyMs: 50, + browserReadyMs: 5, + }, + }); + + assertEquals(result.failureStage, "browserReady"); + assertEquals( + socket.sent.map((message) => message.type), + ["physicsReadyRequest", "runEval", "evalAbort", "evalCleanup"], + ); +}); + +Deno.test("agent eval physics-ready watchdog aborts before runEval", async () => { + const socket = new FakeSocket(); + socket.autoPhysicsReady = false; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId: "physics-watchdog", + timeouts: { physicsReadyMs: 5 }, + }); + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "physicsReady"); + assertEquals( + socket.sent.map((message) => message.type), + ["physicsReadyRequest", "evalAbort", "evalCleanup"], + ); +}); + +Deno.test("agent eval reports socket failure while waiting for physics", async () => { + const socket = new FakeSocket(); + socket.autoPhysicsReady = false; + socket.onSend = (message) => { + if (message.type === "physicsReadyRequest") { + queueMicrotask(() => socket.dispatchEvent(new Event("error"))); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId: "physics-socket-failure", + timeouts: { physicsReadyMs: 50 }, + }); + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "socket"); + assertEquals( + socket.sent.map((message) => message.type), + ["physicsReadyRequest", "evalAbort", "evalCleanup"], + ); +}); + +Deno.test("agent eval reset watchdog aborts before MCP dispatch", async () => { + const socket = new FakeSocket(); + const mcp = new FakeMcp([{ name: "agent_send" }]); + const runId = "reset-watchdog"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 5, + agentIdleProbeMs: 0, + }, + }); + + assertEquals(result.failureStage, "reset"); + assertEquals(mcp.calls, []); + assertEquals( + socket.sent.map((message) => message.type), + [ + "physicsReadyRequest", + "runEval", + "evalReset", + "evalAbort", + "evalCleanup", + ], + ); +}); + +Deno.test("agent eval rejects a finite reset acknowledgement at the wrong pose", async () => { + const socket = new FakeSocket(); + const mcp = new FakeMcp([{ name: "agent_send" }]); + const runId = "wrong-reset-pose"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 2, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentIdleProbeMs: 0, + mcpMs: 50, + }, + }); + + assertEquals(result.failureStage, "reset"); + assertEquals(mcp.calls, []); + assert(!socket.sent.some((message) => message.type === "evalStart")); +}); + +Deno.test("agent eval MCP watchdog prevents evalStart", async () => { + const socket = new FakeSocket(); + const runId = "mcp-watchdog"; + const mcp: McpTransport = { + listTools: () => new Promise(() => {}), + callTool: () => Promise.resolve(), + }; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentIdleProbeMs: 0, + mcpMs: 5, + }, + }); + + assertEquals(result.failureStage, "mcp"); + assert(!socket.sent.some((message) => message.type === "evalStart")); +}); + +Deno.test("agent eval result watchdog uses workflow timeout plus grace", async () => { + const socket = new FakeSocket(); + const runId = "result-watchdog"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 5, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp: new FakeMcp([{ name: "agent_send" }]), + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentIdleProbeMs: 0, + mcpMs: 50, + resultGraceMs: 5, + }, + }); + + assertEquals(result.failureStage, "result"); + assert(socket.sent.some((message) => message.type === "evalStart")); +}); + +Deno.test("agent eval MCP failure never starts scoring and dispatches once", async () => { + const socket = new FakeSocket(); + const mcp = new FakeMcp([{ name: "agent_send" }], "agent_send"); + const runId = "mcp-failure"; + socket.onSend = (message) => { + if (message.type === "runEval") { + socket.emit({ + type: "evalReady", + runId, + workflowUrl: workflow.url, + scene: workflow.scene, + task: "Go to the couch", + timeoutMs: 100, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } else if (message.type === "evalReset") { + socket.emit({ + type: "resetAck", + runId, + ok: true, + pose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + }); + } + }; + + const result = await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: "http://127.0.0.1:9990/mcp", + mcp, + runId, + timeouts: { + browserReadyMs: 50, + resetMs: 50, + sensorSettleMs: 0, + agentIdleProbeMs: 0, + mcpMs: 50, + }, + }); + + assertEquals(result.failureStage, "mcp"); + assertEquals( + socket.sent.map((message) => message.type), + [ + "physicsReadyRequest", + "runEval", + "evalReset", + "evalAbort", + "evalCleanup", + ], + ); + assertEquals( + mcp.calls.filter((call) => call.name === "agent_send").length, + 1, + ); +}); + +Deno.test("HTTP MCP transport surfaces JSON-RPC errors", async () => { + const originalFetch = globalThis.fetch; + globalThis.fetch = (() => + Promise.resolve( + new Response( + JSON.stringify({ + jsonrpc: "2.0", + id: 1, + error: { code: -32603, message: "boom" }, + }), + { status: 200, headers: { "content-type": "application/json" } }, + ), + )) as typeof fetch; + try { + let caught: unknown; + try { + await new HttpMcpTransport().listTools("http://mcp.invalid/mcp", 50); + } catch (error) { + caught = error; + } + assert(caught instanceof Error); + assert(caught.message.includes("boom")); + } finally { + globalThis.fetch = originalFetch; + } +}); + +Deno.test("HTTP MCP transport surfaces DimOS tool error content", async () => { + const originalFetch = globalThis.fetch; + globalThis.fetch = (() => + Promise.resolve( + new Response( + JSON.stringify({ + jsonrpc: "2.0", + id: 1, + result: { + content: [{ + type: "text", + text: "Error running tool 'agent_send': transport unavailable", + }], + }, + }), + { status: 200, headers: { "content-type": "application/json" } }, + ), + )) as typeof fetch; + try { + let caught: unknown; + try { + await new HttpMcpTransport().callTool( + "http://mcp.invalid/mcp", + "agent_send", + { message: "Go to the couch" }, + 50, + ); + } catch (error) { + caught = error; + } + assert(caught instanceof Error); + assert(caught.message.includes("transport unavailable")); + } finally { + globalThis.fetch = originalFetch; + } +}); diff --git a/misc/DimSim/evals/agent-output.ts b/misc/DimSim/evals/agent-output.ts new file mode 100644 index 0000000000..73603fde61 --- /dev/null +++ b/misc/DimSim/evals/agent-output.ts @@ -0,0 +1,250 @@ +import { dirname, fromFileUrl, resolve } from "@std/path"; + +export interface AgentOutputEvent { + type: "agent_output"; + text: string; + hasToolCalls: boolean; + timestampMs: number; +} + +export interface AgentIdleEvent { + type: "agent_idle"; + idle: boolean; + timestampMs: number; +} + +export interface AgentOutputObserver { + readonly failure: Promise; + start( + onOutput: (event: AgentOutputEvent) => void, + onIdle?: (event: AgentIdleEvent) => void, + ): Promise; + cancelActiveTurn(runId: string): Promise; + stop(): Promise; +} + +interface Deferred { + promise: Promise; + resolve(value: T): void; + reject(error: unknown): void; +} + +function deferred(): Deferred { + let resolve!: (value: T) => void; + let reject!: (error: unknown) => void; + const promise = new Promise((res, rej) => { + resolve = res; + reject = rej; + }); + return { promise, resolve, reject }; +} + +export function parseAgentOutputLine( + line: string, +): AgentOutputEvent | AgentIdleEvent | "ready" | null { + let value: unknown; + try { + value = JSON.parse(line); + } catch { + return null; + } + if (!value || typeof value !== "object") return null; + const event = value as Record; + if (event.type === "ready") return "ready"; + if ( + event.type === "agent_idle" && + typeof event.idle === "boolean" && + typeof event.timestampMs === "number" && + Number.isFinite(event.timestampMs) + ) { + return event as unknown as AgentIdleEvent; + } + if ( + event.type !== "agent_output" || + typeof event.text !== "string" || + typeof event.hasToolCalls !== "boolean" || + typeof event.timestampMs !== "number" || + !Number.isFinite(event.timestampMs) + ) { + return null; + } + return event as unknown as AgentOutputEvent; +} + +const EVALS_DIR = dirname(fromFileUrl(import.meta.url)); +const REPO_ROOT = resolve(EVALS_DIR, "../../.."); + +export class ProcessAgentOutputObserver implements AgentOutputObserver { + private child: Deno.ChildProcess | null = null; + private stopping = false; + private ready = false; + private readonly readyState = deferred(); + private readonly failureState = deferred(); + + readonly failure = this.failureState.promise; + + async start( + onOutput: (event: AgentOutputEvent) => void, + onIdle?: (event: AgentIdleEvent) => void, + ): Promise { + if (this.child) throw new Error("agent output observer already started"); + try { + this.child = new Deno.Command("uv", { + args: [ + "run", + "--project", + REPO_ROOT, + "python", + "-m", + "dimos.simulation.dimsim.agent_output_sidecar", + ], + cwd: REPO_ROOT, + stdin: "null", + stdout: "piped", + stderr: "inherit", + }).spawn(); + } catch (error) { + const wrapped = new Error( + `failed to start agent output sidecar: ${ + error instanceof Error ? error.message : String(error) + }`, + ); + this.readyState.reject(wrapped); + this.failureState.resolve(wrapped); + throw wrapped; + } + + void this.readOutput(onOutput, onIdle); + void this.watchStatus(); + await this.readyState.promise; + } + + private async readOutput( + onOutput: (event: AgentOutputEvent) => void, + onIdle?: (event: AgentIdleEvent) => void, + ): Promise { + const child = this.child; + if (!child) return; + try { + const reader = child.stdout.pipeThrough(new TextDecoderStream()) + .getReader(); + let buffered = ""; + while (true) { + const { value, done } = await reader.read(); + if (done) break; + buffered += value; + let newline = buffered.indexOf("\n"); + while (newline >= 0) { + const line = buffered.slice(0, newline); + buffered = buffered.slice(newline + 1); + this.handleLine(line, onOutput, onIdle); + newline = buffered.indexOf("\n"); + } + } + if (buffered) { + this.handleLine(buffered, onOutput, onIdle); + } + } catch (error) { + if (!this.stopping) { + this.fail( + new Error( + `agent output sidecar stream failed: ${ + error instanceof Error ? error.message : String(error) + }`, + ), + ); + } + } + } + + private handleLine( + line: string, + onOutput: (event: AgentOutputEvent) => void, + onIdle?: (event: AgentIdleEvent) => void, + ): void { + const event = parseAgentOutputLine(line); + if (event === "ready") { + if (!this.ready) { + this.ready = true; + this.readyState.resolve(); + } + } else if (event?.type === "agent_idle") { + onIdle?.(event); + } else if (event) { + onOutput(event); + } + } + + private async watchStatus(): Promise { + const child = this.child; + if (!child) return; + const status = await child.status; + if (!this.stopping) { + this.fail( + new Error( + `agent output sidecar exited unexpectedly with code ${status.code}`, + ), + ); + } + } + + private fail(error: Error): void { + if (!this.ready) this.readyState.reject(error); + this.failureState.resolve(error); + } + + async cancelActiveTurn(runId: string): Promise { + if (!runId) throw new Error("runId must not be empty"); + const output = await new Deno.Command("uv", { + args: [ + "run", + "--project", + REPO_ROOT, + "python", + "-m", + "dimos.simulation.dimsim.agent_turn_control", + runId, + ], + cwd: REPO_ROOT, + stdin: "null", + stdout: "null", + stderr: "piped", + }).output(); + if (!output.success) { + const detail = new TextDecoder().decode(output.stderr).trim(); + throw new Error( + `failed to cancel active DimSim agent turn${ + detail ? `: ${detail}` : ` (exit ${output.code})` + }`, + ); + } + } + + async stop(): Promise { + const child = this.child; + if (!child) return; + this.stopping = true; + try { + child.kill("SIGTERM"); + } catch { + // It may have already exited. + } + const graceful = await Promise.race([ + child.status.then(() => true), + new Promise((resolve) => setTimeout(() => resolve(false), 2_000)), + ]); + if (!graceful) { + try { + child.kill("SIGKILL"); + } catch { + // It may have exited during the grace interval. + } + await child.status.catch(() => {}); + } + this.child = null; + } +} + +export function createAgentOutputObserver(): AgentOutputObserver { + return new ProcessAgentOutputObserver(); +} diff --git a/misc/DimSim/evals/agent-output_test.ts b/misc/DimSim/evals/agent-output_test.ts new file mode 100644 index 0000000000..30b42fc57c --- /dev/null +++ b/misc/DimSim/evals/agent-output_test.ts @@ -0,0 +1,61 @@ +import { + type AgentIdleEvent, + type AgentOutputEvent, + parseAgentOutputLine, +} from "./agent-output.ts"; + +function assertEquals(actual: unknown, expected: unknown): void { + const a = JSON.stringify(actual); + const e = JSON.stringify(expected); + if (a !== e) throw new Error(`expected ${e}, got ${a}`); +} + +Deno.test("agent output parser accepts ready and typed output events", () => { + assertEquals(parseAgentOutputLine('{"type":"ready"}'), "ready"); + assertEquals( + parseAgentOutputLine( + '{"type":"agent_output","text":"FOUND_BATHTUB",' + + '"hasToolCalls":false,"timestampMs":1234}', + ), + { + type: "agent_output", + text: "FOUND_BATHTUB", + hasToolCalls: false, + timestampMs: 1234, + } satisfies AgentOutputEvent, + ); + assertEquals( + parseAgentOutputLine( + '{"type":"agent_idle","idle":false,"timestampMs":5678}', + ), + { + type: "agent_idle", + idle: false, + timestampMs: 5678, + } satisfies AgentIdleEvent, + ); +}); + +Deno.test("agent output parser rejects malformed and incomplete lines", () => { + assertEquals(parseAgentOutputLine("not json"), null); + assertEquals( + parseAgentOutputLine( + '{"type":"agent_output","text":"FOUND_BATHTUB",' + + '"hasToolCalls":"false","timestampMs":1234}', + ), + null, + ); + assertEquals( + parseAgentOutputLine( + '{"type":"agent_output","text":"FOUND_BATHTUB",' + + '"hasToolCalls":false,"timestampMs":null}', + ), + null, + ); + assertEquals( + parseAgentOutputLine( + '{"type":"agent_idle","idle":"false","timestampMs":1234}', + ), + null, + ); +}); diff --git a/misc/DimSim/evals/deno.lock b/misc/DimSim/evals/deno.lock new file mode 100644 index 0000000000..1d6442991c --- /dev/null +++ b/misc/DimSim/evals/deno.lock @@ -0,0 +1,23 @@ +{ + "version": "5", + "specifiers": { + "jsr:@std/internal@^1.0.14": "1.0.14", + "jsr:@std/path@1": "1.1.6" + }, + "jsr": { + "@std/internal@1.0.14": { + "integrity": "291516b3d4c35024d6ffbc0a9df5bf4c64116e05b50012cf846710152d2ffdf7" + }, + "@std/path@1.1.6": { + "integrity": "c68485c2a4dfbb5ae3cc74fae4e8c4e5d874cf8a8ed12927917235c758b46cbe", + "dependencies": [ + "jsr:@std/internal" + ] + } + }, + "workspace": { + "dependencies": [ + "jsr:@std/path@1" + ] + } +} diff --git a/misc/DimSim/evals/harness.ts b/misc/DimSim/evals/harness.ts index e28de4c7f4..f2d9d9e632 100644 --- a/misc/DimSim/evals/harness.ts +++ b/misc/DimSim/evals/harness.ts @@ -20,12 +20,35 @@ * orchestration — the workflow file is the source of truth. */ +/// + import { - type SceneState, type AssetEntry, - type ObjectDistanceOpts, type RadiusContainsOpts, - findAsset, dist, objectDistance, radiusContains, + type AssetEntry, + dist, + type EvalMetrics, + findAsset, + objectDistance, + type ObjectDistanceOpts, + orderedRegionVisits, + type OrderedRegionVisitsOpts, + radiusContains, + type RadiusContainsOpts, + type SceneState, + searchEvidence, + type SearchEvidenceOpts, } from "./rubrics.ts"; import type { DimosBridge } from "../src/bridge.ts"; +import type { + EvalAbortMessage, + EvalAgentOutputEvidence, + EvalAgentOutputMessage, + EvalFailureStage, + EvalReadyMessage, + EvalResultMessage, + EvalStartMessage, + RunEvalMessage, +} from "./protocol.ts"; +import { isFiniteStartPose } from "./protocol.ts"; export interface AgentPose { x: number; y: number; z: number; yaw: number; pitch: number; } export interface StartPose { x?: number; y?: number; z?: number; yaw?: number; } @@ -42,6 +65,10 @@ export interface EvalContext { agent: any; agentPos: { x: number; y: number; z: number }; sceneState: SceneState; + /** Pose-history evidence collected only after authoritative eval start. */ + metrics: EvalMetrics; + /** Exact agent output accepted for this run, if the workflow requires one. */ + agentOutput: EvalAgentOutputEvidence | null; setAgentPose: (p: StartPose) => void; findAsset: (q: string) => AssetEntry | null; dist: (a: { x: number; y: number; z: number }, b: { x: number; y: number; z: number }) => number; @@ -49,6 +76,8 @@ export interface EvalContext { rubrics: { objectDistance: (opts: ObjectDistanceOpts) => EvalSuccess; radiusContains: (opts: RadiusContainsOpts) => EvalSuccess; + searchEvidence: (opts: SearchEvidenceOpts) => EvalSuccess; + orderedRegionVisits: (opts: OrderedRegionVisitsOpts) => EvalSuccess; }; } @@ -59,20 +88,14 @@ export interface EvalWorkflow { timeoutSec?: number; startPose?: StartPose; setup?: (ctx: EvalContext) => void | Promise; + /** Static goal check used to reject an agent eval already solved at reset. */ + initialSuccess?: (ctx: EvalContext) => EvalSuccess; + /** Exact, standalone assistant output required during an agent eval. */ + requiredAgentOutput?: string; success: (ctx: EvalContext) => EvalSuccess; } -export interface EvalResultMsg { - type: "evalResult"; - workflowUrl: string; - scene: string; - task: string; - passed: boolean; - reason?: string; - score?: number; - durationMs: number; - channel?: string; -} +export type EvalResultMsg = EvalResultMessage; export interface EvalHarnessOptions { bridge: DimosBridge; @@ -85,6 +108,43 @@ declare global { interface Window { __dimosAgent?: any; } } +interface ActiveCommand { + runId: string; + workflowUrl: string; + agent: boolean; +} + +interface TrajectoryState extends EvalMetrics { + lastPose: AgentPose | null; + viewpoints: AgentPose[]; +} + +const EMPTY_METRICS: EvalMetrics = { + pathLengthM: 0, + headingTravelDeg: 0, + distinctViewpoints: 0, + trajectory: [], +}; + +const MIN_PATH_SAMPLE_M = 0.005; +const MAX_PATH_SAMPLE_M = 2.0; +const MIN_HEADING_SAMPLE_DEG = 0.5; +const VIEWPOINT_SEPARATION_M = 0.75; + +function angleDeltaDeg(a: number, b: number): number { + const delta = Math.atan2(Math.sin(a - b), Math.cos(a - b)); + return Math.abs(delta) * 180 / Math.PI; +} + +interface PendingAgentEval { + runId: string; + workflowUrl: string; + workflow: EvalWorkflow; + resolve: (message: EvalResultMsg) => void; + timer: ReturnType | null; + started: boolean; +} + // ── Singleton registration ────────────────────────────────────────────────── // // Workflow files import `runEval` from `@dimsim/eval`. The importmap in @@ -131,6 +191,16 @@ export class EvalHarness { _activeUrl: string | null = null; _overlay: HTMLDivElement | null = null; + _command: ActiveCommand | null = null; + _pendingAgentEval: PendingAgentEval | null = null; + _trajectory: TrajectoryState = { + ...EMPTY_METRICS, + lastPose: null, + viewpoints: [], + }; + _agentOutput: EvalAgentOutputEvidence | null = null; + _earlyAborts = new Map(); + _patchedSockets = new WeakSet(); constructor({ bridge, getSceneState, getAgentPose, channel }: EvalHarnessOptions) { this.bridge = bridge; @@ -156,22 +226,26 @@ export class EvalHarness { } _patchWsOnMessage(ws: WebSocket): void { - const origOnMessage = ws.onmessage; - const evalTypes = new Set(["runEval"]); - ws.onmessage = (event: MessageEvent) => { - if (typeof event.data === "string") { - try { - const cmd = JSON.parse(event.data); - if (cmd.type && evalTypes.has(cmd.type)) { - this._handleCommand(cmd); - return; - } - } catch { /* not JSON */ } - if (origOnMessage) (origOnMessage as (e: MessageEvent) => void).call(ws, event); - return; - } - if (origOnMessage) (origOnMessage as (e: MessageEvent) => void).call(ws, event); - }; + if (this._patchedSockets.has(ws)) return; + this._patchedSockets.add(ws); + const evalTypes = new Set([ + "runEval", + "evalStart", + "evalAgentOutput", + "evalAbort", + ]); + // Listen alongside DimosBridge's transport handler. Replacing + // `ws.onmessage` here can orphan heartbeat and pose processing after a + // reconnect, leaving the scorer at the browser's stale spawn pose. + ws.addEventListener("message", (event: MessageEvent) => { + if (typeof event.data !== "string") return; + try { + const cmd = JSON.parse(event.data); + if (cmd.type && evalTypes.has(cmd.type)) { + this._handleCommand(cmd); + } + } catch { /* not JSON */ } + }); } _send(msg: Record): void { @@ -183,7 +257,16 @@ export class EvalHarness { if (this.channel && cmd.channel && cmd.channel !== this.channel) return; switch (cmd.type) { case "runEval": - await this._loadAndRunWorkflowFile(cmd.workflowUrl); + await this._loadAndRunWorkflowFile(cmd as RunEvalMessage); + break; + case "evalStart": + this._startAgentEval(cmd as EvalStartMessage); + break; + case "evalAgentOutput": + this._recordAgentOutput(cmd as EvalAgentOutputMessage); + break; + case "evalAbort": + this._abortAgentEval(cmd as EvalAbortMessage); break; } } @@ -194,17 +277,43 @@ export class EvalHarness { * `this.runEval(workflow)` and sends the result WS message itself. We * just await the import — when it resolves the eval is done. */ - async _loadAndRunWorkflowFile(workflowUrl: string): Promise { + async _loadAndRunWorkflowFile(cmd: RunEvalMessage): Promise { + const workflowUrl = cmd.workflowUrl; + const runId = cmd.runId || crypto.randomUUID(); + if ( + this._command?.runId === runId || + this._pendingAgentEval?.runId === runId + ) { + return; + } + const stale = this._pendingAgentEval; + if (stale && stale.runId !== runId) { + this._abortAgentEval({ + type: "evalAbort", + runId: stale.runId, + reason: `superseded by agent eval ${runId}`, + failureStage: "socket", + }); + } + this._command = { runId, workflowUrl, agent: cmd.agent === true }; try { const cacheBust = `?t=${Date.now()}`; await import(/* @vite-ignore */ workflowUrl + cacheBust); } catch (e: any) { console.error("[eval] failed to import %s:", workflowUrl, e); - this._send({ - type: "evalResult", workflowUrl, scene: "", task: "", - passed: false, reason: `import failed: ${e?.message ?? e}`, - durationMs: 0, - }); + this._activeUrl = null; + this._fail( + runId, + workflowUrl, + "", + "", + `import failed: ${e?.message ?? e}`, + 0, + "import", + ); + } finally { + this._earlyAborts.delete(runId); + if (this._command?.runId === runId) this._command = null; } } @@ -223,19 +332,51 @@ export class EvalHarness { * for the Deno runner. */ async runEval(workflow: EvalWorkflow): Promise { - if (!workflow || typeof workflow.success !== "function") { + const command = this._command ?? { + runId: crypto.randomUUID(), + workflowUrl: "", + agent: false, + }; + if ( + !workflow || + typeof workflow.scene !== "string" || + typeof workflow.task !== "string" || + workflow.task.length === 0 || + typeof workflow.success !== "function" + ) { const msg = "runEval(workflow) requires { scene, task, success() }"; console.error("[eval] %s", msg); - return this._fail("", "", "", msg); + return this._fail( + command.runId, + command.workflowUrl, + workflow?.scene ?? "", + workflow?.task ?? "", + msg, + 0, + "configuration", + ); } const tag = `${workflow.scene ?? "?"}/${workflow.task}`; if (this._activeUrl) { const err = `another eval is already running: ${this._activeUrl}`; console.warn("[eval] %s", err); - return this._fail("", workflow.scene, workflow.task, err); + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + err, + 0, + "configuration", + ); } this._activeUrl = tag; + if (command.agent) { + return await this._prepareAgentEval(command, workflow); + } + + this._agentOutput = null; console.log("[eval] running: %s", tag); this._showOverlay(workflow.task, workflow.timeoutSec ?? 120); @@ -250,13 +391,23 @@ export class EvalHarness { const reason = `setup() threw: ${e?.message ?? e}`; console.error("[eval] %s", reason); this._activeUrl = null; - return this._fail("", workflow.scene, workflow.task, reason, Date.now() - start); + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + reason, + Date.now() - start, + "setup", + ); } } + this._resetTrajectory(this.getAgentPose()); return new Promise((resolve) => { const tick = () => { const elapsed = Date.now() - start; + this._recordTrajectory(this.getAgentPose()); let result: EvalSuccess; try { result = workflow.success(this._makeContext()); @@ -264,11 +415,27 @@ export class EvalHarness { result = { passed: false, reason: `success() threw: ${e?.message ?? e}` }; } if (result.passed) { - this._finish(workflow, true, result, elapsed, resolve); + this._finish( + command.runId, + command.workflowUrl, + workflow, + true, + result, + elapsed, + resolve, + ); return; } if (elapsed >= timeoutMs) { - this._finish(workflow, false, { passed: false, ...result, reason: result.reason ?? "timeout" }, elapsed, resolve); + this._finish( + command.runId, + command.workflowUrl, + workflow, + false, + { ...result, passed: false, reason: result.reason ?? "timeout" }, + elapsed, + resolve, + ); return; } setTimeout(tick, 250); @@ -279,18 +446,301 @@ export class EvalHarness { // ── Internals ────────────────────────────────────────────────────────────── - _makeContext(): EvalContext { + async _prepareAgentEval( + command: ActiveCommand, + workflow: EvalWorkflow, + ): Promise { + this._agentOutput = null; + const alreadyAborted = this._takeEarlyAbort(command, workflow); + if (alreadyAborted) return alreadyAborted; + + const timeoutSec = workflow.timeoutSec ?? 120; + if (!Number.isFinite(timeoutSec) || timeoutSec <= 0) { + this._activeUrl = null; + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + "agent eval requires a finite positive timeoutSec", + 0, + "configuration", + ); + } + const startPose = workflow.startPose; + if (!isFiniteStartPose(startPose)) { + this._activeUrl = null; + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + "agent eval startPose requires finite x, y, z, and yaw", + 0, + "configuration", + ); + } + + const setupStart = Date.now(); + if (workflow.setup) { + try { + await workflow.setup(this._makeContext()); + } catch (e: any) { + const aborted = this._takeEarlyAbort(command, workflow); + if (aborted) return aborted; + const reason = `setup() threw: ${e?.message ?? e}`; + this._activeUrl = null; + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + reason, + Date.now() - setupStart, + "setup", + ); + } + } + + const abortedAfterSetup = this._takeEarlyAbort(command, workflow); + if (abortedAfterSetup) return abortedAfterSetup; + + // Evaluate against the declared authoritative start pose without moving the + // browser-only avatar. A task that is already satisfied cannot measure + // agent behavior and is rejected before reset or model dispatch. + let initial: EvalSuccess; + try { + const initialSuccess = workflow.initialSuccess ?? workflow.success; + initial = initialSuccess(this._makeContext(startPose, EMPTY_METRICS)); + } catch (e: any) { + this._activeUrl = null; + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + `initial success() threw: ${e?.message ?? e}`, + 0, + "rubric", + ); + } + if (initial.passed) { + this._activeUrl = null; + return this._fail( + command.runId, + command.workflowUrl, + workflow.scene, + workflow.task, + "agent eval rubric is already satisfied at startPose", + 0, + "initialRubric", + ); + } + + return await new Promise((resolve) => { + this._pendingAgentEval = { + runId: command.runId, + workflowUrl: command.workflowUrl, + workflow, + resolve, + timer: null, + started: false, + }; + const ready: EvalReadyMessage = { + type: "evalReady", + runId: command.runId, + workflowUrl: command.workflowUrl, + scene: workflow.scene, + task: workflow.task, + timeoutMs: timeoutSec * 1000, + startPose, + requiredAgentOutput: workflow.requiredAgentOutput, + }; + console.log( + "[eval] ready and waiting for authoritative reset: %s", + this._activeUrl, + ); + this._send(ready); + }); + } + + _startAgentEval(message: EvalStartMessage): void { + const pending = this._pendingAgentEval; + if (!pending || pending.runId !== message.runId || pending.started) return; + pending.started = true; + + const workflow = pending.workflow; + const timeoutMs = (workflow.timeoutSec ?? 120) * 1000; + const start = Date.now(); + this._agentOutput = null; + this._resetTrajectory(this.getAgentPose()); + console.log("[eval] agent scoring started: %s", this._activeUrl); + this._showOverlay(workflow.task, workflow.timeoutSec ?? 120); + + const tick = () => { + if (this._pendingAgentEval !== pending) return; + const elapsed = Date.now() - start; + this._recordTrajectory(this.getAgentPose()); + let result: EvalSuccess; + try { + result = workflow.success(this._makeContext()); + } catch (e: any) { + const reason = `success() threw: ${e?.message ?? e}`; + this._pendingAgentEval = null; + this._activeUrl = null; + pending.resolve( + this._fail( + pending.runId, + pending.workflowUrl, + workflow.scene, + workflow.task, + reason, + elapsed, + "rubric", + ), + ); + return; + } + if (result.passed) { + this._finish( + pending.runId, + pending.workflowUrl, + workflow, + true, + result, + elapsed, + pending.resolve, + ); + return; + } + if (elapsed >= timeoutMs) { + this._finish( + pending.runId, + pending.workflowUrl, + workflow, + false, + { ...result, passed: false, reason: result.reason ?? "timeout" }, + elapsed, + pending.resolve, + ); + return; + } + pending.timer = setTimeout(tick, 250); + }; + tick(); + } + + _recordAgentOutput(message: EvalAgentOutputMessage): void { + const pending = this._pendingAgentEval; + if ( + !pending || + !pending.started || + pending.runId !== message.runId || + this._agentOutput || + pending.workflow.requiredAgentOutput !== message.text || + !Number.isFinite(message.timestampMs) + ) { + return; + } + const pose = this.getAgentPose(); + this._agentOutput = { + text: message.text, + timestampMs: message.timestampMs, + pose: pose + ? { x: pose.x, y: pose.y, z: pose.z, yaw: pose.yaw } + : undefined, + }; + console.log( + "[eval] accepted required agent output for %s: %s", + message.runId, + message.text, + ); + } + + _abortAgentEval(message: EvalAbortMessage): void { + const pending = this._pendingAgentEval; + if (!pending || pending.runId !== message.runId) { + if (this._command?.runId === message.runId) { + this._earlyAborts.set(message.runId, message); + } + return; + } + if (pending.timer) clearTimeout(pending.timer); + this._pendingAgentEval = null; + this._activeUrl = null; + this._agentOutput = null; + if (this._overlay) { + this._overlay.remove(); + this._overlay = null; + } + pending.resolve({ + type: "evalResult", + runId: pending.runId, + workflowUrl: pending.workflowUrl, + scene: pending.workflow.scene, + task: pending.workflow.task, + passed: false, + status: "error", + failureStage: message.failureStage, + reason: message.reason, + durationMs: 0, + }); + } + + _takeEarlyAbort( + command: ActiveCommand, + workflow: EvalWorkflow, + ): EvalResultMsg | null { + const abort = this._earlyAborts.get(command.runId); + if (!abort) return null; + this._earlyAborts.delete(command.runId); + this._activeUrl = null; + this._agentOutput = null; + return { + type: "evalResult", + runId: command.runId, + workflowUrl: command.workflowUrl, + scene: workflow.scene, + task: workflow.task, + passed: false, + status: "error", + failureStage: abort.failureStage, + reason: abort.reason, + durationMs: 0, + }; + } + + _makeContext( + agentPosOverride?: { x: number; y: number; z: number }, + metricsOverride?: EvalMetrics, + ): EvalContext { const sceneState = this.getSceneState(); const pose = this.getAgentPose(); - const agentPos = pose + const agentPos = agentPosOverride + ? { + x: agentPosOverride.x, + y: agentPosOverride.y, + z: agentPosOverride.z, + } + : pose ? { x: pose.x, y: pose.y, z: pose.z } : { x: 0, y: 0, z: 0 }; sceneState.agentPos = agentPos; const ctxLite = { agentPos, sceneState }; + const metrics = metricsOverride ?? this._metricsSnapshot(); return { agent: window.__dimosAgent, agentPos, sceneState, + metrics, + agentOutput: this._agentOutput + ? { + ...this._agentOutput, + pose: this._agentOutput.pose + ? { ...this._agentOutput.pose } + : undefined, + } + : null, setAgentPose: (p) => { const a = window.__dimosAgent; if (!a) return; @@ -302,36 +752,126 @@ export class EvalHarness { rubrics: { objectDistance: (opts) => objectDistance(ctxLite, opts), radiusContains: (opts) => radiusContains(ctxLite, opts), + searchEvidence: (opts) => searchEvidence({ metrics }, opts), + orderedRegionVisits: (opts) => orderedRegionVisits({ metrics }, opts), }, }; } + _resetTrajectory(pose: AgentPose | null): void { + this._trajectory = { + pathLengthM: 0, + headingTravelDeg: 0, + distinctViewpoints: pose ? 1 : 0, + trajectory: pose ? [{ x: pose.x, y: pose.y, z: pose.z }] : [], + lastPose: pose ? { ...pose } : null, + viewpoints: pose ? [{ ...pose }] : [], + }; + } + + _recordTrajectory(pose: AgentPose | null): void { + if (!pose) return; + const trajectory = this._trajectory; + const previous = trajectory.lastPose; + if (!previous) { + this._resetTrajectory(pose); + return; + } + + const stepM = Math.hypot(pose.x - previous.x, pose.z - previous.z); + const plausibleStep = stepM <= MAX_PATH_SAMPLE_M; + if (stepM >= MIN_PATH_SAMPLE_M && plausibleStep) { + trajectory.pathLengthM += stepM; + trajectory.trajectory.push({ x: pose.x, y: pose.y, z: pose.z }); + } + + const headingStepDeg = angleDeltaDeg(pose.yaw, previous.yaw); + if (headingStepDeg >= MIN_HEADING_SAMPLE_DEG) { + trajectory.headingTravelDeg += headingStepDeg; + } + + const isDistinct = plausibleStep && + trajectory.viewpoints.every((viewpoint) => + Math.hypot(pose.x - viewpoint.x, pose.z - viewpoint.z) >= + VIEWPOINT_SEPARATION_M + ); + if (isDistinct) { + trajectory.viewpoints.push({ ...pose }); + trajectory.distinctViewpoints = trajectory.viewpoints.length; + } + trajectory.lastPose = { ...pose }; + } + + _metricsSnapshot(): EvalMetrics { + return { + pathLengthM: this._trajectory.pathLengthM, + headingTravelDeg: this._trajectory.headingTravelDeg, + distinctViewpoints: this._trajectory.distinctViewpoints, + trajectory: this._trajectory.trajectory.map((point) => ({ ...point })), + }; + } + _finish( - wf: EvalWorkflow, passed: boolean, - result: EvalSuccess, durationMs: number, + runId: string, + workflowUrl: string, + wf: EvalWorkflow, + passed: boolean, + result: EvalSuccess, + durationMs: number, resolve: (msg: EvalResultMsg) => void, ): void { + const pending = this._pendingAgentEval; + if (pending?.timer) clearTimeout(pending.timer); const msg: EvalResultMsg = { type: "evalResult", - workflowUrl: "", + runId, + workflowUrl, scene: wf.scene, task: wf.task, passed, + status: passed ? "passed" : "failed", reason: result.reason, score: result.score, durationMs, + evidence: this._agentOutput + ? { + agentOutput: { + ...this._agentOutput, + pose: this._agentOutput.pose + ? { ...this._agentOutput.pose } + : undefined, + }, + } + : undefined, }; console.log("[eval] %s (%dms): %s", passed ? "PASS" : "FAIL", durationMs, result.reason ?? ""); this._showResult(passed, result.reason ?? (passed ? "ok" : "fail")); this._send(msg); + this._pendingAgentEval = null; this._activeUrl = null; resolve(msg); } - _fail(workflowUrl: string, scene: string, task: string, reason: string, durationMs = 0): EvalResultMsg { + _fail( + runId: string, + workflowUrl: string, + scene: string, + task: string, + reason: string, + durationMs = 0, + failureStage?: EvalFailureStage, + ): EvalResultMsg { const msg: EvalResultMsg = { - type: "evalResult", workflowUrl, scene, task, - passed: false, reason, durationMs, + type: "evalResult", + runId, + workflowUrl, + scene, + task, + passed: false, + status: failureStage ? "error" : "failed", + failureStage, + reason, + durationMs, }; this._send(msg); return msg; @@ -346,9 +886,21 @@ export class EvalHarness { const taskEl = document.createElement("div"); taskEl.style.cssText = "color:#4fc3f7;font-size:16px;font-weight:bold;margin-bottom:4px;"; taskEl.textContent = `EVAL: ${task}`; + const closeEl = document.createElement("button"); + closeEl.type = "button"; + closeEl.setAttribute("aria-label", "Hide evaluation goal"); + closeEl.textContent = "×"; + closeEl.style.cssText = + "position:absolute;top:3px;right:7px;border:0;background:transparent;color:#aaa;font:18px/1 monospace;cursor:pointer;pointer-events:auto;"; + closeEl.addEventListener("click", () => { + if (this._overlay === el) this._overlay = null; + el.remove(); + }); const timerEl = document.createElement("div"); timerEl.style.cssText = "color:#aaa;font-size:13px;"; - el.appendChild(taskEl); el.appendChild(timerEl); + el.appendChild(closeEl); + el.appendChild(taskEl); + el.appendChild(timerEl); document.body.appendChild(el); this._overlay = el; diff --git a/misc/DimSim/evals/harness_test.ts b/misc/DimSim/evals/harness_test.ts new file mode 100644 index 0000000000..6ee20dd1a3 --- /dev/null +++ b/misc/DimSim/evals/harness_test.ts @@ -0,0 +1,525 @@ +import { EvalHarness, type EvalWorkflow } from "./harness.ts"; + +function assert( + condition: unknown, + message = "assertion failed", +): asserts condition { + if (!condition) throw new Error(message); +} + +function assertEquals(actual: unknown, expected: unknown): void { + const a = JSON.stringify(actual); + const e = JSON.stringify(expected); + if (a !== e) throw new Error(`expected ${e}, got ${a}`); +} + +class FakeBridge { + ws = null; + sent: Array> = []; + connect(): void {} + sendCommand(message: Record): void { + this.sent.push({ ...message }); + } +} + +function makeHarness(): { harness: EvalHarness; bridge: FakeBridge } { + const bridge = new FakeBridge(); + const harness = new EvalHarness({ + bridge: bridge as any, + getSceneState: () => ({ assets: [] }), + getAgentPose: () => ({ x: 9, y: 0.5, z: 9, yaw: 0, pitch: 0 }), + }); + (harness as any)._showOverlay = () => {}; + (harness as any)._showResult = () => {}; + return { harness, bridge }; +} + +function setAgentCommand(harness: EvalHarness, runId: string): void { + (harness as any)._command = { + runId, + workflowUrl: "/scenes/apartment/evals/test.js", + agent: true, + }; +} + +function makeBathtubHarness(): { + harness: EvalHarness; + bridge: FakeBridge; + setPose: (x: number, z: number) => void; +} { + const bridge = new FakeBridge(); + let pose = { x: 5, y: 0.5, z: 5, yaw: 0, pitch: 0 }; + const harness = new EvalHarness({ + bridge: bridge as any, + getSceneState: () => ({ + assets: [{ + title: "bathtub", + transform: { x: 0, y: 0.5, z: 0 }, + _bbox: { w: 1, h: 1, d: 1 }, + }], + }), + getAgentPose: () => ({ ...pose }), + }); + (harness as any)._showOverlay = () => {}; + (harness as any)._showResult = () => {}; + return { + harness, + bridge, + setPose: (x, z) => { + pose = { ...pose, x, z }; + }, + }; +} + +function bathtubWorkflow(): EvalWorkflow { + return { + scene: "apartment", + task: "Find the bathtub and respond exactly FOUND_BATHTUB.", + timeoutSec: 2, + startPose: { x: 5, y: 0.5, z: 5, yaw: 0 }, + requiredAgentOutput: "FOUND_BATHTUB", + initialSuccess: (ctx) => + ctx.rubrics.objectDistance({ + target: "bathtub", + thresholdM: 1, + }), + success: (ctx) => { + const goal = ctx.rubrics.objectDistance({ + target: "bathtub", + thresholdM: 1, + }); + const declared = ctx.agentOutput?.text === "FOUND_BATHTUB"; + return { + passed: declared && goal.passed, + reason: `${declared ? "declared" : "waiting"}; ${goal.reason}`, + score: goal.score, + }; + }, + }; +} + +const originalWindow = (globalThis as any).window; + +Deno.test({ + name: "agent harness does not start timer before evalStart", + async fn() { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, bridge } = makeHarness(); + setAgentCommand(harness, "timer-run"); + let rubricCalls = 0; + const workflow: EvalWorkflow = { + scene: "apartment", + task: "Go somewhere", + timeoutSec: 1, + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + success: () => { + rubricCalls++; + return { + passed: rubricCalls >= 2, + reason: rubricCalls >= 2 ? "done" : "not yet", + }; + }, + }; + + const resultPromise = harness.runEval(workflow); + await new Promise((resolve) => setTimeout(resolve, 10)); + assertEquals(rubricCalls, 1); + assertEquals(bridge.sent.map((message) => message.type), ["evalReady"]); + + await harness._handleCommand({ type: "evalStart", runId: "stale-run" }); + assertEquals(rubricCalls, 1); + await harness._handleCommand({ type: "evalStart", runId: "timer-run" }); + const result = await resultPromise; + + assertEquals(rubricCalls, 2); + assert(result.passed); + assertEquals( + bridge.sent.map((message) => message.type), + ["evalReady", "evalResult"], + ); + } finally { + (globalThis as any).window = originalWindow; + } + }, + sanitizeOps: false, + sanitizeResources: false, +}); + +Deno.test("agent harness classifies setup failures as infrastructure errors", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, bridge } = makeHarness(); + setAgentCommand(harness, "setup-run"); + const result = await harness.runEval({ + scene: "apartment", + task: "Go somewhere", + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + setup: () => { + throw new Error("setup boom"); + }, + success: () => ({ passed: false }), + }); + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "setup"); + assertEquals(bridge.sent[0].status, "error"); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("agent harness rejects an initially satisfied rubric", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, bridge } = makeHarness(); + setAgentCommand(harness, "initial-run"); + const result = await harness.runEval({ + scene: "apartment", + task: "Already done", + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + success: ({ agentPos }) => ({ + passed: agentPos.x === 0 && agentPos.z === 3, + }), + }); + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "initialRubric"); + assertEquals(bridge.sent.map((message) => message.type), ["evalResult"]); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("agent harness records post-start trajectory evidence", () => { + const { harness } = makeHarness(); + (harness as any)._resetTrajectory({ + x: 0, + y: 0.5, + z: 0, + yaw: 0, + pitch: 0, + }); + (harness as any)._recordTrajectory({ + x: 1, + y: 0.5, + z: 0, + yaw: Math.PI / 2, + pitch: 0, + }); + (harness as any)._recordTrajectory({ + x: 2, + y: 0.5, + z: 0, + yaw: Math.PI, + pitch: 0, + }); + + assertEquals((harness as any)._metricsSnapshot(), { + pathLengthM: 2, + headingTravelDeg: 180, + distinctViewpoints: 3, + trajectory: [ + { x: 0, y: 0.5, z: 0 }, + { x: 1, y: 0.5, z: 0 }, + { x: 2, y: 0.5, z: 0 }, + ], + }); +}); + +Deno.test("agent harness does not count rotation as translated viewpoints", () => { + const { harness } = makeHarness(); + (harness as any)._resetTrajectory({ + x: 0, + y: 0.5, + z: 0, + yaw: 0, + pitch: 0, + }); + (harness as any)._recordTrajectory({ + x: 0, + y: 0.5, + z: 0, + yaw: Math.PI, + pitch: 0, + }); + + const metrics = (harness as any)._metricsSnapshot(); + assertEquals(metrics.distinctViewpoints, 1); + assertEquals(metrics.headingTravelDeg, 180); +}); + +Deno.test("agent harness rejects implausible trajectory jumps", () => { + const { harness } = makeHarness(); + (harness as any)._resetTrajectory({ + x: 0, + y: 0.5, + z: 0, + yaw: 0, + pitch: 0, + }); + (harness as any)._recordTrajectory({ + x: 10, + y: 0.5, + z: 10, + yaw: 0, + pitch: 0, + }); + + const metrics = (harness as any)._metricsSnapshot(); + assertEquals(metrics.pathLengthM, 0); + assertEquals(metrics.distinctViewpoints, 1); + assertEquals(metrics.trajectory, [{ x: 0, y: 0.5, z: 0 }]); +}); + +Deno.test("agent harness uses static initialSuccess with temporal rubrics", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, bridge } = makeHarness(); + setAgentCommand(harness, "temporal-initial-run"); + const result = await harness.runEval({ + scene: "apartment", + task: "Already at the static goal", + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + initialSuccess: ({ agentPos }) => ({ + passed: agentPos.x === 0 && agentPos.z === 3, + }), + success: ({ metrics }) => ({ + passed: metrics.pathLengthM >= 1, + }), + }); + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "initialRubric"); + assertEquals(bridge.sent.map((message) => message.type), ["evalResult"]); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("scorer-only harness resets stale trajectory before scoring", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness } = makeHarness(); + (harness as any)._trajectory = { + pathLengthM: 99, + headingTravelDeg: 999, + distinctViewpoints: 99, + trajectory: [{ x: -99, y: 0.5, z: -99 }], + lastPose: { x: -99, y: 0.5, z: -99, yaw: 0, pitch: 0 }, + viewpoints: [{ x: -99, y: 0.5, z: -99, yaw: 0, pitch: 0 }], + }; + let scoredMetrics: Record | undefined; + const result = await harness.runEval({ + scene: "apartment", + task: "Inspect clean metrics", + success: ({ metrics }) => { + scoredMetrics = metrics as unknown as Record; + return { passed: true }; + }, + }); + + assert(result.passed); + assertEquals(scoredMetrics, { + pathLengthM: 0, + headingTravelDeg: 0, + distinctViewpoints: 1, + trajectory: [{ x: 9, y: 0.5, z: 9 }], + }); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("agent harness remembers aborts received during setup", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, bridge } = makeHarness(); + setAgentCommand(harness, "slow-setup-run"); + let releaseSetup: (() => void) | undefined; + const setupGate = new Promise((resolve) => { + releaseSetup = resolve; + }); + const resultPromise = harness.runEval({ + scene: "apartment", + task: "Slow setup", + startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, + setup: () => setupGate, + success: () => ({ passed: false }), + }); + + await new Promise((resolve) => setTimeout(resolve, 0)); + await harness._handleCommand({ + type: "evalAbort", + runId: "slow-setup-run", + reason: "browserReady timed out", + failureStage: "browserReady", + }); + releaseSetup!(); + const result = await resultPromise; + + assertEquals(result.status, "error"); + assertEquals(result.failureStage, "browserReady"); + assertEquals(bridge.sent, []); + assert((harness as any)._pendingAgentEval === null); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("agent harness classifies workflow import failures as errors", async () => { + const { harness, bridge } = makeHarness(); + await harness._loadAndRunWorkflowFile({ + type: "runEval", + runId: "import-run", + workflowUrl: "file:///definitely/missing/dimsim-workflow.js", + agent: true, + }); + assertEquals(bridge.sent[0].status, "error"); + assertEquals(bridge.sent[0].failureStage, "import"); +}); + +Deno.test("a new agent run clears a stale pending browser run", async () => { + const { harness, bridge } = makeHarness(); + let staleResult: Record | undefined; + (harness as any)._activeUrl = "apartment/stale"; + (harness as any)._pendingAgentEval = { + runId: "stale-run", + workflowUrl: "/scenes/apartment/evals/stale.js", + workflow: { + scene: "apartment", + task: "Stale task", + success: () => ({ passed: false }), + }, + resolve: (result: Record) => { + staleResult = result; + }, + timer: null, + started: false, + }; + + await harness._loadAndRunWorkflowFile({ + type: "runEval", + runId: "new-run", + workflowUrl: "file:///definitely/missing/new-dimsim-workflow.js", + agent: true, + }); + + assertEquals(staleResult?.status, "error"); + assertEquals(staleResult?.failureStage, "socket"); + assert( + String(staleResult?.reason).includes("superseded by agent eval new-run"), + ); + assert((harness as any)._pendingAgentEval === null); + assertEquals(bridge.sent[0].failureStage, "import"); +}); + +Deno.test("agent output before evalStart or for another run is ignored", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness } = makeBathtubHarness(); + setAgentCommand(harness, "output-window-run"); + const resultPromise = harness.runEval(bathtubWorkflow()); + await new Promise((resolve) => setTimeout(resolve, 0)); + + await harness._handleCommand({ + type: "evalAgentOutput", + runId: "output-window-run", + text: "FOUND_BATHTUB", + timestampMs: 100, + }); + await harness._handleCommand({ + type: "evalStart", + runId: "output-window-run", + }); + await harness._handleCommand({ + type: "evalAgentOutput", + runId: "another-run", + text: "FOUND_BATHTUB", + timestampMs: 200, + }); + + assertEquals((harness as any)._agentOutput, null); + await harness._handleCommand({ + type: "evalAbort", + runId: "output-window-run", + reason: "test complete", + failureStage: "result", + }); + const result = await resultPromise; + assertEquals(result.status, "error"); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("agent output followed by proximity passes with pose evidence", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, setPose } = makeBathtubHarness(); + setAgentCommand(harness, "output-first-run"); + const resultPromise = harness.runEval(bathtubWorkflow()); + await new Promise((resolve) => setTimeout(resolve, 0)); + await harness._handleCommand({ + type: "evalStart", + runId: "output-first-run", + }); + await harness._handleCommand({ + type: "evalAgentOutput", + runId: "output-first-run", + text: "FOUND_BATHTUB", + timestampMs: 1234, + }); + + assertEquals((harness as any)._agentOutput, { + text: "FOUND_BATHTUB", + timestampMs: 1234, + pose: { x: 5, y: 0.5, z: 5, yaw: 0 }, + }); + assertEquals( + bathtubWorkflow().success((harness as any)._makeContext()).passed, + false, + ); + setPose(0.75, 0); + const result = await resultPromise; + + assert(result.passed); + assertEquals(result.evidence?.agentOutput, { + text: "FOUND_BATHTUB", + timestampMs: 1234, + pose: { x: 5, y: 0.5, z: 5, yaw: 0 }, + }); + } finally { + (globalThis as any).window = originalWindow; + } +}); + +Deno.test("proximity followed by agent output also passes", async () => { + (globalThis as any).window = { __dimosAgent: null }; + try { + const { harness, setPose } = makeBathtubHarness(); + setAgentCommand(harness, "proximity-first-run"); + const resultPromise = harness.runEval(bathtubWorkflow()); + await new Promise((resolve) => setTimeout(resolve, 0)); + await harness._handleCommand({ + type: "evalStart", + runId: "proximity-first-run", + }); + setPose(0.75, 0); + + assertEquals( + bathtubWorkflow().success((harness as any)._makeContext()).passed, + false, + ); + await harness._handleCommand({ + type: "evalAgentOutput", + runId: "proximity-first-run", + text: "FOUND_BATHTUB", + timestampMs: 5678, + }); + const result = await resultPromise; + + assert(result.passed); + assertEquals(result.evidence?.agentOutput?.timestampMs, 5678); + } finally { + (globalThis as any).window = originalWindow; + } +}); diff --git a/misc/DimSim/evals/protocol.ts b/misc/DimSim/evals/protocol.ts new file mode 100644 index 0000000000..1fb8b86b81 --- /dev/null +++ b/misc/DimSim/evals/protocol.ts @@ -0,0 +1,149 @@ +/** + * Correlated messages used by the Deno eval runner, bridge, and browser + * harness. `runId` is mandatory for new callers; the browser harness still + * accepts legacy `runEval` messages so direct workflow execution keeps working. + */ + +export type EvalStatus = "passed" | "failed" | "error"; + +export type EvalFailureStage = + | "configuration" + | "connection" + | "physicsReady" + | "browserReady" + | "reset" + | "agentOutput" + | "agentIdle" + | "mcp" + | "result" + | "socket" + | "import" + | "setup" + | "initialRubric" + | "rubric"; + +/** Workflow start poses use Three.js coordinates and yaw in degrees. */ +export interface EvalStartPose { + x: number; + y: number; + z: number; + yaw: number; +} + +export interface EvalAgentOutputEvidence { + text: string; + timestampMs: number; + pose?: EvalStartPose; +} + +export interface EvalEvidence { + agentOutput?: EvalAgentOutputEvidence; +} + +/** Bridge lifecycle handshake used before a correlated eval run begins. */ +export interface PhysicsReadyRequestMessage { + type: "physicsReadyRequest"; +} + +export interface PhysicsReadyMessage { + type: "physicsReady"; +} + +export interface RunEvalMessage { + type: "runEval"; + runId: string; + workflowUrl: string; + agent?: boolean; + channel?: string; +} + +export interface EvalReadyMessage { + type: "evalReady"; + runId: string; + workflowUrl: string; + scene: string; + task: string; + timeoutMs: number; + startPose: EvalStartPose; + requiredAgentOutput?: string; + channel?: string; +} + +export interface EvalResetMessage { + type: "evalReset"; + runId: string; + startPose: EvalStartPose; + channel?: string; +} + +export interface ResetAckMessage { + type: "resetAck"; + runId: string; + ok: boolean; + pose?: EvalStartPose; + reason?: string; + channel?: string; +} + +export interface EvalStartMessage { + type: "evalStart"; + runId: string; + channel?: string; +} + +export interface EvalAgentOutputMessage { + type: "evalAgentOutput"; + runId: string; + text: string; + timestampMs: number; + channel?: string; +} + +export interface EvalAbortMessage { + type: "evalAbort"; + runId: string; + reason: string; + failureStage: EvalFailureStage; + channel?: string; +} + +export interface EvalCleanupMessage { + type: "evalCleanup"; + runId: string; + channel?: string; +} + +export interface EvalResultMessage { + type: "evalResult"; + runId: string; + workflowUrl: string; + scene: string; + task: string; + passed: boolean; + status: EvalStatus; + failureStage?: EvalFailureStage; + reason?: string; + score?: number; + durationMs: number; + evidence?: EvalEvidence; + channel?: string; +} + +export type EvalProtocolMessage = + | RunEvalMessage + | EvalReadyMessage + | EvalResetMessage + | ResetAckMessage + | EvalStartMessage + | EvalAgentOutputMessage + | EvalAbortMessage + | EvalCleanupMessage + | EvalResultMessage; + +export function isFiniteStartPose(value: unknown): value is EvalStartPose { + if (!value || typeof value !== "object") return false; + const pose = value as Record; + return ["x", "y", "z", "yaw"].every( + (key) => typeof pose[key] === "number" && Number.isFinite(pose[key]), + ); +} diff --git a/misc/DimSim/evals/reporting_test.ts b/misc/DimSim/evals/reporting_test.ts new file mode 100644 index 0000000000..8261a48a5a --- /dev/null +++ b/misc/DimSim/evals/reporting_test.ts @@ -0,0 +1,102 @@ +import { + connectEvalSocket, + type EvalResult, + exitCodeForResults, + formatResults, + SCORER_RESULT_WATCHDOG_MS, + toJunitXml, +} from "./runner.ts"; + +function assert( + condition: unknown, + message = "assertion failed", +): asserts condition { + if (!condition) throw new Error(message); +} + +function result( + status: "passed" | "failed" | "error", + reason: string, +): EvalResult { + return { + runId: `run-${status}`, + scene: "apartment", + workflow: status, + workflowUrl: `/workflows/${status}.js`, + task: "task", + passed: status === "passed", + status, + failureStage: status === "error" ? "mcp" : undefined, + reason, + score: null, + durationMs: 10, + }; +} + +Deno.test("JSON reporting contains only the result document", () => { + const passed = result("passed", "ok"); + passed.evidence = { + agentOutput: { + text: "FOUND_BATHTUB", + timestampMs: 1234, + pose: { x: 1, y: 0.5, z: 2, yaw: 90 }, + }, + }; + const output = formatResults([passed], "json"); + const parsed = JSON.parse(output); + assert(Array.isArray(parsed)); + assert(parsed[0].status === "passed"); + assert(parsed[0].runId === "run-passed"); + assert(parsed[0].evidence.agentOutput.text === "FOUND_BATHTUB"); +}); + +Deno.test("JUnit distinguishes task failures from infrastructure errors", () => { + const xml = toJunitXml([ + result("passed", "ok"), + result("failed", "too far"), + result("error", "MCP unavailable"), + ]); + assert(xml.includes('failures="1" errors="1"')); + assert(xml.includes('')); + assert(xml.includes('')); +}); + +Deno.test("exit codes classify pass, failure, and infrastructure error", () => { + assert(exitCodeForResults([result("passed", "ok")]) === 0); + assert(exitCodeForResults([result("failed", "no")]) === 1); + assert( + exitCodeForResults([ + result("failed", "no"), + result("error", "infra"), + ]) === 2, + ); +}); + +Deno.test("scorer watchdog does not preempt 15-minute workflows", () => { + assert(SCORER_RESULT_WATCHDOG_MS > 900_000 + 5_000); +}); + +Deno.test("bridge connection watchdog closes an unresponsive socket", async () => { + class DeadSocket extends EventTarget { + readyState = WebSocket.CONNECTING; + closed = false; + send(_data: string): void {} + close(): void { + this.closed = true; + } + } + const socket = new DeadSocket(); + let caught: unknown; + try { + await connectEvalSocket( + "ws://bridge.invalid", + 5, + () => socket, + ); + } catch (error) { + caught = error; + } + assert(caught instanceof Error); + assert(caught.message.includes("timed out connecting")); + assert(socket.closed); +}); diff --git a/misc/DimSim/evals/rubrics.ts b/misc/DimSim/evals/rubrics.ts index 245ccfdc7a..36b0c1e522 100644 --- a/misc/DimSim/evals/rubrics.ts +++ b/misc/DimSim/evals/rubrics.ts @@ -7,6 +7,8 @@ * `EvalSuccess` ({passed, reason, score}) directly: * - objectDistance({ target, thresholdM }) * - radiusContains({ targets, radiusM }) + * - searchEvidence({ minTravelM, minHeadingTravelDeg, minViewpoints }) + * - orderedRegionVisits({ regions }) * * 2. Low-level helpers if you want to write the scoring inline: * - findAsset(query, sceneState) → AssetEntry | null @@ -34,6 +36,14 @@ export interface EvalSuccess { score?: number; } +/** Pose-history evidence collected by the browser after `evalStart`. */ +export interface EvalMetrics { + pathLengthM: number; + headingTravelDeg: number; + distinctViewpoints: number; + trajectory: Vec3[]; +} + // ── Low-level helpers ──────────────────────────────────────────────────────── export function dist(a: Vec3, b: Vec3): number { @@ -125,3 +135,104 @@ export function radiusContains( reason: `${d.toFixed(3)}m to centroid of ${found.length} targets${missingNote} (radius ${radiusM}m)`, }; } + +export interface SearchEvidenceOpts { + minTravelM?: number; + minHeadingTravelDeg?: number; + minViewpoints?: number; +} + +export interface AxisAlignedRegion { + name: string; + minX: number; + maxX: number; + minZ: number; + maxZ: number; +} + +export interface OrderedRegionVisitsOpts { + regions: AxisAlignedRegion[]; +} + +function regionContainsPoint(region: AxisAlignedRegion, point: Vec3): boolean { + return point.x >= region.minX && point.x <= region.maxX && + point.z >= region.minZ && point.z <= region.maxZ; +} + +/** + * Require a physical trajectory to visit calibrated regions in order and end + * in the final region. + * + * Region coordinates are scorer-only ground truth. They are never included in + * the task string or exposed as an agent tool. + */ +export function orderedRegionVisits( + ctx: { metrics: EvalMetrics }, + { regions }: OrderedRegionVisitsOpts, +): EvalSuccess { + if (regions.length === 0) { + return { passed: false, reason: "no ordered regions specified" }; + } + + let nextRegion = 0; + for (const point of ctx.metrics.trajectory) { + if ( + nextRegion < regions.length && + regionContainsPoint(regions[nextRegion], point) + ) { + nextRegion++; + } + } + + const finalRegion = regions[regions.length - 1]; + const finalPoint = ctx.metrics.trajectory.at(-1); + const endedInFinalRegion = finalPoint !== undefined && + regionContainsPoint(finalRegion, finalPoint); + const passed = nextRegion === regions.length && endedInFinalRegion; + const visitedNames = regions.slice(0, nextRegion).map((region) => + region.name + ); + const nextName = regions[nextRegion]?.name; + return { + passed, + reason: passed + ? `visited ${ + regions.map((region) => region.name).join(" -> ") + } and ended in ${finalRegion.name}` + : `visited ${visitedNames.join(" -> ") || "none"}; ${ + nextName + ? `next required ${nextName}` + : `did not end in ${finalRegion.name}` + }`, + }; +} + +/** + * Require physical search motion before accepting a final-position rubric. + * + * A viewpoint is a pose separated from every prior viewpoint by at least + * 0.75m or 45 degrees. This deliberately measures only externally observable + * robot motion; it does not claim that continuous camera publication proves + * the model called `observe`. + */ +export function searchEvidence( + ctx: { metrics: EvalMetrics }, + { + minTravelM = 0, + minHeadingTravelDeg = 0, + minViewpoints = 1, + }: SearchEvidenceOpts, +): EvalSuccess { + const { pathLengthM, headingTravelDeg, distinctViewpoints } = ctx.metrics; + const passed = pathLengthM >= minTravelM && + headingTravelDeg >= minHeadingTravelDeg && + distinctViewpoints >= minViewpoints; + return { + passed, + reason: `${pathLengthM.toFixed(2)}m travelled (min ${minTravelM}m), ` + + `${ + headingTravelDeg.toFixed(0) + }° heading travel (min ${minHeadingTravelDeg}°), ` + + `${distinctViewpoints} distinct viewpoints (min ${minViewpoints})`, + }; +} diff --git a/misc/DimSim/evals/rubrics_test.ts b/misc/DimSim/evals/rubrics_test.ts new file mode 100644 index 0000000000..25d7a0a933 --- /dev/null +++ b/misc/DimSim/evals/rubrics_test.ts @@ -0,0 +1,110 @@ +import { orderedRegionVisits, searchEvidence } from "./rubrics.ts"; + +function assertEquals(actual: unknown, expected: unknown): void { + const a = JSON.stringify(actual); + const e = JSON.stringify(expected); + if (a !== e) throw new Error(`expected ${e}, got ${a}`); +} + +Deno.test("searchEvidence requires every configured motion signal", () => { + const result = searchEvidence( + { + metrics: { + pathLengthM: 7, + headingTravelDeg: 170, + distinctViewpoints: 5, + trajectory: [], + }, + }, + { + minTravelM: 6, + minHeadingTravelDeg: 180, + minViewpoints: 4, + }, + ); + + assertEquals(result.passed, false); +}); + +Deno.test("searchEvidence passes with sufficient trajectory evidence", () => { + const result = searchEvidence( + { + metrics: { + pathLengthM: 7, + headingTravelDeg: 210, + distinctViewpoints: 5, + trajectory: [], + }, + }, + { + minTravelM: 6, + minHeadingTravelDeg: 180, + minViewpoints: 4, + }, + ); + + assertEquals(result.passed, true); +}); + +Deno.test("orderedRegionVisits requires the full route and final containment", () => { + const regions = [ + { name: "outside", minX: 0, maxX: 2, minZ: 7, maxZ: 9 }, + { name: "entrance", minX: -5, maxX: -3, minZ: 4, maxZ: 6 }, + { name: "bathroom", minX: 1, maxX: 6, minZ: -5, maxZ: 0 }, + ]; + const result = orderedRegionVisits( + { + metrics: { + pathLengthM: 20, + headingTravelDeg: 360, + distinctViewpoints: 8, + trajectory: [ + { x: 1, y: 0.5, z: 8 }, + { x: -4, y: 0.5, z: 5 }, + { x: 3, y: 0.5, z: -2 }, + { x: 0, y: 0.5, z: 2 }, + ], + }, + }, + { regions }, + ); + + assertEquals(result.passed, false); + assertEquals(result.reason?.includes("did not end in bathroom"), true); +}); + +Deno.test("orderedRegionVisits passes an in-order route ending in the goal region", () => { + const result = orderedRegionVisits( + { + metrics: { + pathLengthM: 20, + headingTravelDeg: 360, + distinctViewpoints: 8, + trajectory: [ + { x: 1, y: 0.5, z: 8 }, + { x: -4, y: 0.5, z: 5 }, + { x: 0, y: 0.5, z: 0 }, + { x: 1, y: 0.5, z: -2.5 }, + { x: 4, y: 0.5, z: -1 }, + ], + }, + }, + { + regions: [ + { name: "tree", minX: 0, maxX: 2, minZ: 7, maxZ: 9 }, + { name: "entrance", minX: -5, maxX: -3, minZ: 4, maxZ: 6 }, + { name: "main door", minX: -1, maxX: 1, minZ: -1, maxZ: 1 }, + { + name: "bathroom door", + minX: 0.5, + maxX: 1.5, + minZ: -3, + maxZ: -2, + }, + { name: "bathroom", minX: 1, maxX: 6, minZ: -5, maxZ: 0 }, + ], + }, + ); + + assertEquals(result.passed, true); +}); diff --git a/misc/DimSim/evals/runner.ts b/misc/DimSim/evals/runner.ts index b7e1d2c4df..b233fcd247 100644 --- a/misc/DimSim/evals/runner.ts +++ b/misc/DimSim/evals/runner.ts @@ -1,27 +1,40 @@ /** - * Eval Runner — Deno-side orchestrator. + * Deno-side eval orchestration. * - * Walks `scenes//evals/*.js` to discover workflows, then for each one - * opens a control WebSocket to the bridge, sends `{type:'runEval', - * workflowUrl}`, and awaits the `{type:'evalResult', ...}` reply from the - * browser-side harness. - * - * No JSON parsing. No manifest.json. The workflow file is the source of - * truth — its `setup(ctx)` runs in the browser, its `success(ctx)` is - * polled until passed or timeout, and the runner just collects results. + * Scorer mode remains a simple runEval/evalResult exchange. Agent mode uses + * the correlated lifecycle implemented in agent-driver.ts and dispatches the + * browser-provided task only after the bridge acknowledges an authoritative + * physics reset. */ import { resolve } from "@std/path"; +import { + type AgentEvalTimeouts, + type EvalSocket, + type McpTransport, + runAgentEvalOnSocket, +} from "./agent-driver.ts"; +import type { + EvalEvidence, + EvalFailureStage, + EvalResultMessage, + EvalStatus, + RunEvalMessage, +} from "./protocol.ts"; export interface EvalResult { + runId: string; scene: string; workflow: string; workflowUrl: string; task: string; passed: boolean; + status: EvalStatus; + failureStage?: EvalFailureStage; reason: string; score: number | null; durationMs: number; + evidence?: EvalEvidence; } export interface WorkflowEntry { @@ -32,6 +45,13 @@ export interface WorkflowEntry { url: string; } +export type EvalSocketFactory = (url: string) => EvalSocket; + +// The browser owns each workflow's declared timeout and always emits a result +// when that timeout expires. This outer watchdog only catches a wedged browser, +// so it must remain longer than the longest shipped workflow (15 minutes). +export const SCORER_RESULT_WATCHDOG_MS = 3_600_000; + export interface RunEvalOptions { /** Control WebSocket URL (no `?ch=...`). */ wsUrl: string; @@ -39,6 +59,12 @@ export interface RunEvalOptions { scenesRoot: string; filterScene?: string; filterWorkflow?: string; + agent?: boolean; + mcpUrl?: string; + mcp?: McpTransport; + agentTimeouts?: Partial; + connectionTimeoutMs?: number; + socketFactory?: EvalSocketFactory; } /** Walk `scenes//evals/*.js` and return one entry per workflow file. */ @@ -67,7 +93,7 @@ export function collectWorkflows(opts: { try { workflowEnts = [...Deno.readDirSync(evalsDir)]; } catch { - continue; // no evals dir → no workflows for this scene + continue; } for (const ent of workflowEnts) { @@ -90,30 +116,74 @@ export function collectWorkflows(opts: { export async function runEvals(options: RunEvalOptions): Promise { const workflows = collectWorkflows(options); if (workflows.length === 0) { - console.log("[runner] no workflows match filter — nothing to do."); + console.error("[runner] no workflows match filter"); return []; } - console.log(`[runner] running ${workflows.length} workflow(s)…`); + if (options.agent && workflows.length !== 1) { + return [ + configurationResult( + options.filterScene ?? "", + options.filterWorkflow ?? "", + `agent mode requires exactly one workflow; matched ${workflows.length}`, + ), + ]; + } + + console.error(`[runner] running ${workflows.length} workflow(s)…`); + let socket: EvalSocket; + try { + socket = await connectEvalSocket( + options.wsUrl, + options.connectionTimeoutMs ?? 5_000, + options.socketFactory, + ); + } catch (error) { + return workflows.map((workflow) => + infrastructureResult( + workflow, + crypto.randomUUID(), + "connection", + error instanceof Error ? error.message : String(error), + ) + ); + } - const ws = await _connect(options.wsUrl); try { const results: EvalResult[] = []; - for (const wf of workflows) { - console.log(`[runner] → ${wf.scene}/${wf.workflow}`); - const result = await _runOne(ws, wf); + for (const workflow of workflows) { + console.error(`[runner] → ${workflow.scene}/${workflow.workflow}`); + const result = options.agent + ? await runAgentEvalOnSocket({ + socket, + workflow, + mcpUrl: options.mcpUrl ?? + "http://127.0.0.1:9990/mcp", + mcp: options.mcp, + timeouts: options.agentTimeouts, + }) + : await runScorerEvalOnSocket(socket, workflow); results.push(result); - const tag = result.passed ? "PASS" : "FAIL"; - console.log(`[runner] ${tag} (${result.durationMs}ms): ${result.reason}`); + const tag = result.status === "error" + ? "ERROR" + : result.passed + ? "PASS" + : "FAIL"; + console.error( + `[runner] ${tag} (${result.durationMs}ms): ${result.reason}`, + ); } return results; } finally { - try { ws.close(); } catch { /* ignore */ } + try { + socket.close(); + } catch { + // ignore close races + } } } -/** Parallel variant — one control WS per channel, workflows round-robin'd across them. */ +/** Parallel scorer-only variant — one control WS per browser page. */ export interface RunEvalsMultiPageOptions extends RunEvalOptions { - /** Channel names from launchMultiPage (one per browser page). */ channels: string[]; } @@ -121,124 +191,283 @@ export async function runEvalsMultiPage(options: RunEvalsMultiPageOptions): Prom const workflows = collectWorkflows(options); if (workflows.length === 0 || options.channels.length === 0) return []; - // Open one socket per channel. - // Two query params: - // channel= → routes the WS to that channel's bridge state - // ch=control → marks it as a control (not sensor) socket so text - // frames (the `runEval` JSON) are processed, not dropped - const sockets = await Promise.all( - options.channels.map((ch) => - _connect(`${options.wsUrl}/?channel=${encodeURIComponent(ch)}&ch=control`), - ), - ); + let sockets: EvalSocket[]; + try { + sockets = await Promise.all( + options.channels.map((channel) => + connectEvalSocket( + `${options.wsUrl}/?channel=${encodeURIComponent(channel)}&ch=control`, + options.connectionTimeoutMs ?? 5_000, + options.socketFactory, + ) + ), + ); + } catch (error) { + return workflows.map((workflow) => + infrastructureResult( + workflow, + crypto.randomUUID(), + "connection", + error instanceof Error ? error.message : String(error), + ) + ); + } - // Round-robin workflows across sockets. const queues: WorkflowEntry[][] = sockets.map(() => []); - workflows.forEach((wf, i) => queues[i % queues.length].push(wf)); + workflows.forEach((workflow, index) => + queues[index % queues.length].push(workflow) + ); try { const all = await Promise.all( - sockets.map(async (ws, i) => { + sockets.map(async (socket, index) => { const out: EvalResult[] = []; - for (const wf of queues[i]) { - console.log(`[runner:${options.channels[i]}] → ${wf.scene}/${wf.workflow}`); - out.push(await _runOne(ws, wf)); + for (const workflow of queues[index]) { + console.error( + `[runner:${options.channels[index]}] → ${workflow.scene}/${workflow.workflow}`, + ); + out.push(await runScorerEvalOnSocket(socket, workflow)); } return out; }), ); return all.flat(); } finally { - for (const ws of sockets) { - try { ws.close(); } catch { /* ignore */ } + for (const socket of sockets) { + try { + socket.close(); + } catch { + // ignore close races + } } } } -/** JUnit-style XML emitter for CI consumption. */ +/** JUnit emitter: task failures are failures; infrastructure issues are errors. */ export function toJunitXml(results: EvalResult[]): string { const lines: string[] = []; lines.push(''); - const failures = results.filter((r) => !r.passed).length; - lines.push(``); - for (const r of results) { - const name = `${r.scene}/${r.workflow}`; - const time = (r.durationMs / 1000).toFixed(3); - if (r.passed) { - lines.push(` `); - } else { - lines.push(` `); - lines.push(` `); - lines.push(` `); + const failures = results.filter((result) => result.status === "failed").length; + const errors = results.filter((result) => result.status === "error").length; + lines.push( + ``, + ); + for (const result of results) { + const name = `${result.scene}/${result.workflow}`; + const time = (result.durationMs / 1000).toFixed(3); + if (result.status === "passed") { + lines.push(` `); + continue; } + const element = result.status === "error" ? "error" : "failure"; + lines.push(` `); + lines.push( + ` <${element} message="${_escape(result.reason)}"/>`, + ); + lines.push(" "); } lines.push(""); return lines.join("\n"); } -// ── Internals ──────────────────────────────────────────────────────────────── +export function formatResults( + results: EvalResult[], + format: "json" | "junit", +): string { + return format === "junit" + ? toJunitXml(results) + : JSON.stringify(results, null, 2); +} + +export function exitCodeForResults(results: EvalResult[]): 0 | 1 | 2 { + if ( + results.length === 0 || + results.some((result) => result.status === "error") + ) { + return 2; + } + return results.some((result) => result.status === "failed") ? 1 : 0; +} + +export function configurationResult( + scene: string, + workflow: string, + reason: string, +): EvalResult { + return { + runId: crypto.randomUUID(), + scene, + workflow, + workflowUrl: "", + task: "", + passed: false, + status: "error", + failureStage: "configuration", + reason, + score: null, + durationMs: 0, + }; +} -function _connect(wsUrl: string): Promise { - // Force the control channel so eval text messages route correctly. +function infrastructureResult( + workflow: WorkflowEntry, + runId: string, + failureStage: EvalFailureStage, + reason: string, +): EvalResult { + return { + runId, + scene: workflow.scene, + workflow: workflow.workflow, + workflowUrl: workflow.url, + task: "", + passed: false, + status: "error", + failureStage, + reason, + score: null, + durationMs: 0, + }; +} + +export function connectEvalSocket( + wsUrl: string, + timeoutMs = 5_000, + socketFactory: EvalSocketFactory = (url) => new WebSocket(url), +): Promise { const url = wsUrl.includes("?") ? wsUrl : `${wsUrl}/?ch=control`; - const ws = new WebSocket(url); return new Promise((resolve, reject) => { - ws.addEventListener("open", () => resolve(ws), { once: true }); - ws.addEventListener("error", (e) => reject(e), { once: true }); + const socket = socketFactory(url); + let settled = false; + const cleanup = () => { + clearTimeout(timer); + socket.removeEventListener("open", onOpen); + socket.removeEventListener("error", onError); + socket.removeEventListener("close", onClose); + }; + const onOpen = () => { + if (settled) return; + settled = true; + cleanup(); + resolve(socket); + }; + const onError = () => { + if (settled) return; + settled = true; + cleanup(); + reject(new Error(`websocket error connecting to ${url}`)); + }; + const onClose = () => { + if (settled) return; + settled = true; + cleanup(); + reject(new Error(`websocket closed while connecting to ${url}`)); + }; + const timer = setTimeout(() => { + if (settled) return; + settled = true; + cleanup(); + try { + socket.close(); + } catch { + // ignore + } + reject(new Error(`timed out connecting to ${url} after ${timeoutMs}ms`)); + }, timeoutMs); + socket.addEventListener("open", onOpen); + socket.addEventListener("error", onError); + socket.addEventListener("close", onClose); }); } -function _runOne(ws: WebSocket, wf: WorkflowEntry): Promise { +export function runScorerEvalOnSocket( + socket: EvalSocket, + workflow: WorkflowEntry, + timeoutMs = SCORER_RESULT_WATCHDOG_MS, + runId = crypto.randomUUID(), +): Promise { return new Promise((resolve) => { + let settled = false; const cleanup = () => { - ws.removeEventListener("message", onMessage); - ws.removeEventListener("error", onFail); - ws.removeEventListener("close", onFail); + clearTimeout(timer); + socket.removeEventListener("message", onMessage); + socket.removeEventListener("error", onFailure); + socket.removeEventListener("close", onFailure); }; - const onMessage = (event: MessageEvent) => { - if (typeof event.data !== "string") return; - let msg: any; - try { msg = JSON.parse(event.data); } catch { return; } - if (msg.type !== "evalResult") return; - if (msg.workflowUrl && msg.workflowUrl !== wf.url) return; + const finish = (result: EvalResult) => { + if (settled) return; + settled = true; cleanup(); - resolve({ - scene: wf.scene, - workflow: wf.workflow, - workflowUrl: wf.url, - task: msg.task ?? "", - passed: !!msg.passed, - reason: msg.reason ?? (msg.passed ? "ok" : "fail"), - score: typeof msg.score === "number" ? msg.score : null, - durationMs: msg.durationMs ?? 0, - }); + resolve(result); }; - // If the socket closes or errors before the result lands, fail the eval - // instead of hanging the entire runEvals call forever. - const onFail = (event: Event) => { - cleanup(); - const reason = event.type === "close" - ? "websocket closed before evalResult" - : "websocket error before evalResult"; - resolve({ - scene: wf.scene, - workflow: wf.workflow, - workflowUrl: wf.url, - task: "", - passed: false, - reason, - score: null, - durationMs: 0, + const onMessage = (event: Event) => { + const data = (event as MessageEvent).data; + if (typeof data !== "string") return; + let message: EvalResultMessage; + try { + message = JSON.parse(data) as EvalResultMessage; + } catch { + return; + } + if (message.type !== "evalResult") return; + if (message.runId && message.runId !== runId) return; + if (message.workflowUrl && message.workflowUrl !== workflow.url) return; + const status = message.status ?? + (message.passed ? "passed" : "failed"); + finish({ + runId, + scene: workflow.scene, + workflow: workflow.workflow, + workflowUrl: workflow.url, + task: message.task ?? "", + passed: status === "passed" && !!message.passed, + status, + failureStage: message.failureStage, + reason: message.reason ?? (message.passed ? "ok" : "fail"), + score: typeof message.score === "number" ? message.score : null, + durationMs: typeof message.durationMs === "number" + ? message.durationMs + : 0, + evidence: message.evidence, }); }; - ws.addEventListener("message", onMessage); - ws.addEventListener("error", onFail); - ws.addEventListener("close", onFail); - ws.send(JSON.stringify({ type: "runEval", workflowUrl: wf.url })); + const onFailure = (event: Event) => { + finish( + infrastructureResult( + workflow, + runId, + "socket", + event.type === "close" + ? "websocket closed before evalResult" + : "websocket error before evalResult", + ), + ); + }; + const timer = setTimeout(() => { + finish( + infrastructureResult( + workflow, + runId, + "result", + `evalResult timed out after ${timeoutMs}ms`, + ), + ); + }, timeoutMs); + socket.addEventListener("message", onMessage); + socket.addEventListener("error", onFailure); + socket.addEventListener("close", onFailure); + const message: RunEvalMessage = { + type: "runEval", + runId, + workflowUrl: workflow.url, + }; + socket.send(JSON.stringify(message)); }); } -function _escape(s: string): string { - return s.replace(/&/g, "&").replace(//g, ">") - .replace(/"/g, """).replace(/'/g, "'"); +function _escape(value: string): string { + return value.replace(/&/g, "&").replace(//g, ">").replace(/"/g, """) + .replace(/'/g, "'"); } diff --git a/misc/DimSim/scenes/apartment/evals/find-and-go-to-bathtub.js b/misc/DimSim/scenes/apartment/evals/find-and-go-to-bathtub.js new file mode 100644 index 0000000000..3042ac6fb8 --- /dev/null +++ b/misc/DimSim/scenes/apartment/evals/find-and-go-to-bathtub.js @@ -0,0 +1,31 @@ +import { runEval } from "@dimsim/eval"; + +await runEval({ + scene: "apartment", + task: "Find the bathtub and go to it. Keep searching until you find it. " + + "When you believe you have found the bathtub, stop right next to the bathtub with a very close distance, " + + "and respond with exactly FOUND_BATHTUB.", + timeoutSec: 900, + requiredAgentOutput: "FOUND_BATHTUB", + // Start outside beside the oak, facing west along the house. The task stays + // intentionally natural and contains no route, landmark, or tool guidance. + startPose: { x: 1.2, y: 0.5, z: 7.6, yaw: 270 }, + initialSuccess: (ctx) => + ctx.rubrics.objectDistance({ target: "bathtub", thresholdM: 1.0 }), + success: (ctx) => { + const goal = ctx.rubrics.objectDistance({ + target: "bathtub", + thresholdM: 1.0, + }); + const declared = ctx.agentOutput?.text === "FOUND_BATHTUB"; + return { + passed: declared && goal.passed, + score: goal.score, + reason: `${ + declared + ? "agent declared FOUND_BATHTUB" + : "waiting for exact agent output FOUND_BATHTUB" + }; ${goal.reason}`, + }; + }, +}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-bathtub.js b/misc/DimSim/scenes/apartment/evals/go-to-bathtub.js new file mode 100644 index 0000000000..d6830fe3cd --- /dev/null +++ b/misc/DimSim/scenes/apartment/evals/go-to-bathtub.js @@ -0,0 +1,12 @@ +import { runEval } from "@dimsim/eval"; + +await runEval({ + scene: "apartment", + task: + "Go to the large freestanding gray oval soaking bathtub directly ahead in the current camera view. " + + "Stop right next to it, as close as collision allows.", + timeoutSec: 180, + startPose: { x: 2.3, y: 0.5, z: -3.2, yaw: 45 }, + success: (ctx) => + ctx.rubrics.objectDistance({ target: "bathtub", thresholdM: 1.0 }), +}); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-couch.js b/misc/DimSim/scenes/apartment/evals/go-to-couch.js index 30b36db952..342b89187c 100644 --- a/misc/DimSim/scenes/apartment/evals/go-to-couch.js +++ b/misc/DimSim/scenes/apartment/evals/go-to-couch.js @@ -1,9 +1,12 @@ -import { runEval } from '@dimsim/eval'; +import { runEval } from "@dimsim/eval"; await runEval({ - scene: 'apartment', - task: 'Go to the couch', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'sectional', thresholdM: 2.0 }), + scene: "apartment", + task: + "Go to the large brown L-shaped couch directly ahead in the current camera view. " + + "Stop next to it.", + timeoutSec: 60, + startPose: { x: 1, y: 0.5, z: 3, yaw: 117 }, + success: (ctx) => + ctx.rubrics.objectDistance({ target: "sectional", thresholdM: 2.0 }), }); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js b/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js index 5165406385..8f2128079d 100644 --- a/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js +++ b/misc/DimSim/scenes/apartment/evals/go-to-kitchen.js @@ -1,9 +1,11 @@ -import { runEval } from '@dimsim/eval'; +import { runEval } from "@dimsim/eval"; await runEval({ - scene: 'apartment', - task: 'Go to the kitchen', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'refrigerator', thresholdM: 3.0 }), + scene: "apartment", + task: "Go into the kitchen directly ahead through the open doorway. " + + "Stop near the refrigerator.", + timeoutSec: 60, + startPose: { x: 0.8, y: 0.5, z: 2.5, yaw: -120 }, + success: (ctx) => + ctx.rubrics.objectDistance({ target: "refrigerator", thresholdM: 3.0 }), }); diff --git a/misc/DimSim/scenes/apartment/evals/go-to-tv.js b/misc/DimSim/scenes/apartment/evals/go-to-tv.js index c9800d80e5..6632033520 100644 --- a/misc/DimSim/scenes/apartment/evals/go-to-tv.js +++ b/misc/DimSim/scenes/apartment/evals/go-to-tv.js @@ -1,9 +1,12 @@ -import { runEval } from '@dimsim/eval'; +import { runEval } from "@dimsim/eval"; await runEval({ - scene: 'apartment', - task: 'Go to the TV', - timeoutSec: 30, - startPose: { x: 0, y: 0.5, z: 3, yaw: 0 }, - success: (ctx) => ctx.rubrics.objectDistance({ target: 'television', thresholdM: 2.0 }), + scene: "apartment", + task: + "Go to the large wall-mounted television directly ahead in the current camera view. " + + "Stop in front of it.", + timeoutSec: 60, + startPose: { x: 1.5, y: 0.5, z: 3.5, yaw: 57 }, + success: (ctx) => + ctx.rubrics.objectDistance({ target: "television", thresholdM: 2.0 }), }); diff --git a/misc/DimSim/src/bridge.ts b/misc/DimSim/src/bridge.ts index eaa2bdf36e..86fc0ea8f0 100644 --- a/misc/DimSim/src/bridge.ts +++ b/misc/DimSim/src/bridge.ts @@ -56,6 +56,8 @@ export interface DimosBridgeOptions { sensorEnable?: Partial; } +const CONTROL_HEARTBEAT_TIMEOUT_MS = 30_000; + export class DimosBridge { wsUrl: string; agent: any; @@ -74,6 +76,9 @@ export class DimosBridge { _timers: Record>; _connected: boolean; + _publishingPaused: boolean; + _controlHeartbeatTimer: ReturnType | null; + _lastControlHeartbeatAck: number; constructor({ wsUrl, agent, sensorSources, rates, sensorEnable }: DimosBridgeOptions) { const protocol = location.protocol === "https:" ? "wss:" : "ws:"; @@ -88,86 +93,202 @@ export class DimosBridge { this.wsDepth = null; this._timers = {}; this._connected = false; + this._publishingPaused = false; + this._lastRgbPublishedAt = 0; + this._serverPhysicsReady = false; + this._controlHeartbeatTimer = null; + this._lastControlHeartbeatAck = 0; } connect(): void { // Read channel from URL param (for multi-page parallel evals) const channel = new URLSearchParams(location.search).get("channel") || ""; const channelSuffix = channel ? `&channel=${channel}` : ""; + const isLive = (socket: WebSocket | null): boolean => + socket !== null && + (socket.readyState === WebSocket.CONNECTING || + socket.readyState === WebSocket.OPEN); // Control socket: JSON commands out, server pose + embodiment config in - this.wsControl = new WebSocket(this.wsUrl + "?ch=control" + channelSuffix); - this.wsControl.binaryType = "arraybuffer"; - - this.wsControl.onopen = () => { - console.log("[DimosBridge] control WS connected"); - this._connected = true; - this._startPublishing(); - this._flushPendingCommands(); - }; - - this.wsControl.onmessage = (event: MessageEvent) => { - // Text messages: server-side physics pose updates + embodiment config - if (typeof event.data === "string") { - try { - const msg = JSON.parse(event.data); - if (msg.type === "pose") { - this._handleServerPose(msg.x, msg.y, msg.z, msg.yaw); - } else if (msg.type === "embodimentConfig") { - this._handleEmbodimentConfig(msg); - } - } catch {} - } - }; + if (!isLive(this.wsControl)) { + const control = new WebSocket( + this.wsUrl + "?ch=control&client=browser" + channelSuffix, + ); + this.wsControl = control; + control.binaryType = "arraybuffer"; + + control.onopen = () => { + if (this.wsControl !== control) { + control.close(); + return; + } + console.log("[DimosBridge] control WS connected"); + this._connected = true; + this._startControlHeartbeat(control); + this._startPublishing(); + this._flushPendingCommands(); + }; + + // Keep the bridge's transport handler independent from consumers such + // as EvalHarness. Replacing `onmessage` in a consumer can otherwise + // disable heartbeat acknowledgements and authoritative pose updates, + // causing a reconnect loop while an eval appears to keep running. + control.addEventListener("message", (event: MessageEvent) => { + // Text messages: server-side physics pose updates + embodiment config + if (typeof event.data === "string") { + try { + const msg = JSON.parse(event.data); + if (msg.type === "heartbeatAck") { + this._lastControlHeartbeatAck = Date.now(); + if (msg.command) { + queueMicrotask(() => { + if (this.wsControl !== control) return; + control.dispatchEvent( + new MessageEvent("message", { + data: JSON.stringify(msg.command), + }), + ); + }); + } + } else if (msg.type === "pose") { + this._handleServerPose(msg.x, msg.y, msg.z, msg.yaw); + } else if (msg.type === "physicsReady") { + this._serverPhysicsReady = true; + this.resumePublishing(); + } else if (msg.type === "embodimentConfig") { + this._handleEmbodimentConfig(msg); + } + } catch {} + } + }); + + control.onclose = () => { + if (this.wsControl !== control) return; + this._stopControlHeartbeat(); + console.log( + "[DimosBridge] control WS disconnected, reconnecting in 2s...", + ); + this._connected = false; + this._serverPhysicsReady = false; + this._stopPublishing(); + setTimeout(() => { + if (this.wsControl === control) this.connect(); + }, 2000); + }; + + control.onerror = () => {}; + } - this.wsControl.onclose = () => { - console.log("[DimosBridge] control WS disconnected, reconnecting in 2s..."); - this._connected = false; - this._stopPublishing(); - setTimeout(() => this.connect(), 2000); - }; + // Sensor socket: Rapier snapshots out (no incoming expected) + if (!isLive(this.wsSensors)) { + const sensors = new WebSocket( + this.wsUrl + "?ch=sensors" + channelSuffix, + ); + this.wsSensors = sensors; + sensors.binaryType = "arraybuffer"; + sensors.onopen = () => { + if (this.wsSensors === sensors) { + console.log("[DimosBridge] sensor WS connected"); + } + }; + sensors.onclose = () => { + if (this.wsSensors !== sensors) return; + console.log("[DimosBridge] sensor WS disconnected"); + setTimeout(() => { + if (this.wsSensors === sensors) this.connect(); + }, 2000); + }; + sensors.onerror = () => {}; + } - this.wsControl.onerror = () => {}; + if (!isLive(this.wsRgb)) { + const rgb = new WebSocket(this.wsUrl + "?ch=rgb" + channelSuffix); + this.wsRgb = rgb; + rgb.binaryType = "arraybuffer"; + rgb.onclose = () => { + if (this.wsRgb !== rgb) return; + console.log("[DimosBridge] RGB WS disconnected"); + setTimeout(() => { + if (this.wsRgb === rgb) this.connect(); + }, 2000); + }; + rgb.onerror = () => {}; + } - // Sensor socket: Rapier snapshots out (no incoming expected) - this.wsSensors = new WebSocket(this.wsUrl + "?ch=sensors" + channelSuffix); - this.wsSensors.binaryType = "arraybuffer"; - - this.wsSensors.onopen = () => { - console.log("[DimosBridge] sensor WS connected"); - }; - - this.wsSensors.onclose = () => { - console.log("[DimosBridge] sensor WS disconnected"); - }; - - this.wsSensors.onerror = () => {}; - - this.wsRgb = new WebSocket(this.wsUrl + "?ch=rgb" + channelSuffix); - this.wsRgb.binaryType = "arraybuffer"; - this.wsRgb.onclose = () => { - console.log("[DimosBridge] RGB WS disconnected"); - }; - this.wsRgb.onerror = () => {}; - - this.wsDepth = new WebSocket(this.wsUrl + "?ch=depth" + channelSuffix); - this.wsDepth.binaryType = "arraybuffer"; - this.wsDepth.onclose = () => { - console.log("[DimosBridge] depth WS disconnected"); - }; - this.wsDepth.onerror = () => {}; + if (!isLive(this.wsDepth)) { + const depth = new WebSocket( + this.wsUrl + "?ch=depth" + channelSuffix, + ); + this.wsDepth = depth; + depth.binaryType = "arraybuffer"; + depth.onclose = () => { + if (this.wsDepth !== depth) return; + console.log("[DimosBridge] depth WS disconnected"); + setTimeout(() => { + if (this.wsDepth === depth) this.connect(); + }, 2000); + }; + depth.onerror = () => {}; + } } // -- Incoming messages ------------------------------------------------------ + _startControlHeartbeat(control: WebSocket): void { + this._stopControlHeartbeat(); + this._lastControlHeartbeatAck = Date.now(); + this._controlHeartbeatTimer = setInterval(() => { + if (this.wsControl !== control) { + this._stopControlHeartbeat(); + return; + } + if ( + control.readyState !== WebSocket.OPEN || + Date.now() - this._lastControlHeartbeatAck > + CONTROL_HEARTBEAT_TIMEOUT_MS + ) { + this.wsControl = null; + this._stopControlHeartbeat(); + try { + control.close(); + } catch { /* ignore */ } + this.connect(); + return; + } + try { + control.send(JSON.stringify({ type: "heartbeat", ts: Date.now() })); + } catch { /* watchdog reconnects on the next tick */ } + // Background Chrome tabs can throttle the completion-scheduled camera + // setTimeout for a minute or more even while WebSocket heartbeats remain + // reliable. Use that live heartbeat as a bounded sensor fallback. + const cameraFallbackMs = Math.max(5_000, this.rates.images * 3); + if ( + !this._publishingPaused && + this.rates.images > 0 && + Date.now() - this._lastRgbPublishedAt >= cameraFallbackMs + ) { + this._publishImages(); + } + }, 3000); + } + + _stopControlHeartbeat(): void { + if (this._controlHeartbeatTimer) { + clearInterval(this._controlHeartbeatTimer); + this._controlHeartbeatTimer = null; + } + } + /** Handle server-side physics pose update (Three.js Y-up frame). */ _handleServerPose(x: number, y: number, z: number, yaw: number): void { if (!this.agent) return; // Move the agent body to the server-authoritative position if (this.agent.body) { + this.agent.body.setTranslation?.({ x, y, z }, true); this.agent.body.setNextKinematicTranslation({ x, y, z }); } if (this.agent.group) { + this.agent.group.position?.set?.(x, y, z); this.agent.group.rotation.y = yaw; } // Update engine's _dimosYaw for sensor capture / odom pose reading @@ -179,6 +300,8 @@ export class DimosBridge { } _serverPose: { x: number; y: number; z: number; yaw: number } | null = null; + _serverPhysicsReady = false; + _lastRgbPublishedAt = 0; _handleEmbodimentConfig(msg: any): void { console.log("[DimosBridge] embodiment config received:", msg.embodimentType || "quadruped"); @@ -206,10 +329,30 @@ export class DimosBridge { // -- Outgoing sensor data --------------------------------------------------- _startPublishing(): void { + // Reconnects and explicit resume calls must not create duplicate timers. + this._stopPublishing(); + if (this._publishingPaused) return; // Images default 5 Hz (configurable via rates.images). // Odom and lidar are published server-side via LCM directly. if (this.rates.images > 0) { - this._timers["images"] = setInterval(() => this._publishImages(), this.rates.images); + const publishImages = () => { + if (this._publishingPaused) return; + this._publishImages(); + // Schedule from completion instead of using setInterval. Software + // WebGL capture can overrun its nominal period; setInterval then runs + // another expensive capture immediately and starves control/eval + // messages for tens of seconds. + if (!this._publishingPaused && this.rates.images > 0) { + this._timers["images"] = setTimeout( + publishImages, + this.rates.images, + ); + } + }; + this._timers["images"] = setTimeout( + publishImages, + this.rates.images, + ); } } @@ -233,6 +376,16 @@ export class DimosBridge { this._timers = {}; } + pausePublishing(): void { + this._publishingPaused = true; + this._stopPublishing(); + } + + resumePublishing(): void { + this._publishingPaused = false; + this._startPublishing(); + } + /** Send on a sensor WebSocket (images — large data). */ _sendSensor(ws: WebSocket | null, channel: string, msg: any): void { if (!ws || ws.readyState !== WebSocket.OPEN) return; @@ -247,16 +400,21 @@ export class DimosBridge { const frame = this.sensors.captureRgb(); if (!frame) return; - this._sendSensor(this.wsRgb, CH_IMAGE, new sensor_msgs.Image({ - header, - height: frame.height, - width: frame.width, - encoding: "jpeg", - is_bigendian: 0, - step: 0, // not applicable for compressed format - data_length: frame.data.length, - data: frame.data, - })); + this._sendSensor( + this.wsRgb, + CH_IMAGE, + new sensor_msgs.Image({ + header, + height: frame.height, + width: frame.width, + encoding: "jpeg", + is_bigendian: 0, + step: 0, // not applicable for compressed format + data_length: frame.data.length, + data: frame.data, + }), + ); + this._lastRgbPublishedAt = Date.now(); } catch (e) { console.warn("[DimosBridge] RGB publish error:", e); } diff --git a/misc/DimSim/src/engine.js b/misc/DimSim/src/engine.js index 6e02f8f78c..0cc7aadf80 100644 --- a/misc/DimSim/src/engine.js +++ b/misc/DimSim/src/engine.js @@ -4428,7 +4428,7 @@ simCameraModeToggleBtn?.addEventListener("click", () => { updateSimCameraModeToggleUi(); if (simUserCameraMode === "user") { if (agentCameraFollow) disableAgentCameraFollow(); - } else if (agentTask.active) { + } else if (agentTask.active || dimosMode) { enableAgentCameraFollow(); } }); @@ -5576,6 +5576,13 @@ if (dimosMode) { const spawnPos = sceneCfg.spawnPoint || { x: 2, y: 0.5, z: 3 }; agent.setPosition(spawnPos.x, spawnPos.y, spawnPos.z); renderAgentTaskUi(); // update UI: hide spawn button, enable task controls + // A saved Agent camera preference can be restored before the external + // agent exists. In that case the earlier follow attempt is a no-op even + // though the button already says "Camera: Agent". Attach now that the + // externally-driven agent has been created. + if (simUserCameraMode === "agent") { + enableAgentCameraFollow(agent.id); + } // Server-side physics: agent pose is driven by ServerPhysics (Deno). // Browser just receives position updates and moves the visual avatar. let _dimosYaw = 0; @@ -5589,11 +5596,19 @@ if (dimosMode) { // 3. Set up fixed-size offscreen capture for dimos. // Keep sensor cost independent of the headed browser window size. const _dimosCapW = 640, _dimosCapH = 288; - const _dimosCapTarget = new THREE.WebGLRenderTarget(_dimosCapW, _dimosCapH, { - minFilter: THREE.LinearFilter, magFilter: THREE.LinearFilter, - format: THREE.RGBAFormat, depthBuffer: true, stencilBuffer: false, - }); - // Go2 depth camera: 87° horizontal. At 640x288 (2.22:1 aspect), that's 46° vertical. + const _dimosCapTarget = new THREE.WebGLRenderTarget( + _dimosCapW, + _dimosCapH, + { + minFilter: THREE.LinearFilter, + magFilter: THREE.LinearFilter, + format: THREE.RGBAFormat, + depthBuffer: true, + stencilBuffer: false, + }, + ); + // Go2 depth camera: 87° horizontal. At 640x288 (2.22:1 aspect), that's + // 46° vertical. const _dimosFov = window.__dimosCameraFov || 46; const _dimosCapCam = new THREE.PerspectiveCamera(_dimosFov, _dimosCapW / _dimosCapH, camera.near, camera.far); const _dimosCapBuf = new Uint8Array(_dimosCapW * _dimosCapH * 4); @@ -5803,15 +5818,43 @@ if (dimosMode) { // Chunked snapshot protocol (DSC1) — single-frame send stalls when the // browser main thread is CPU-saturated (e.g. headless SwiftShader on a // weak runner): WebSocket.bufferedAmount climbs and never drains. - // Splitting into ~256KB chunks with a setTimeout(0) yield between each - // lets the WS pump run, and bridge reassembles in receive order. + // Splitting into bounded 2MB chunks with an event-loop yield between + // small batches lets the WS pump run, and bridge reassembles in receive + // order. // Wire format: // prelude: [DSC1 4B BE][total u32 LE][sx f32 LE][sy f32 LE][sz f32 LE] (20B) // chunks: raw bytes, in order, until `total` accumulated bridge-side. - const SNAPSHOT_CHUNK_SIZE = 256 * 1024; - const _waitSensorWs = () => { - if (bridge.wsSensors && bridge.wsSensors.readyState === WebSocket.OPEN) { + const SNAPSHOT_CHUNK_SIZE = 512 * 1024; + const SNAPSHOT_CHUNKS_PER_TURN = 2; + const SNAPSHOT_BUFFER_HIGH_WATER = 2 * 1024 * 1024; + const snapshotTaskQueue = []; + const snapshotTaskChannel = new MessageChannel(); + snapshotTaskChannel.port1.onmessage = () => { + snapshotTaskQueue.shift()?.(); + }; + const scheduleSnapshotTask = (task) => { + snapshotTaskQueue.push(task); + snapshotTaskChannel.port2.postMessage(0); + }; + const _waitSensorWs = async () => { + if ( + bridge.wsSensors && bridge.wsSensors.readyState === WebSocket.OPEN + ) { try { + // A browser reload can reconnect to bridge physics that is already + // initialized for this scene. Give the control handshake a brief + // chance to say so instead of uploading a redundant multi-MB + // Rapier snapshot and starving camera publication. + await new Promise((resolve) => setTimeout(resolve, 500)); + if (bridge._serverPhysicsReady) { + bridge.resumePublishing(); + return; + } + // Snapshot transfer is the one-time prerequisite for authoritative + // server physics. RGB readbacks are expensive under software + // WebGL and can starve this transfer for minutes, so pause image + // publication until the snapshot socket has fully drained. + bridge.pausePublishing(); const snapshot = rapierWorld.takeSnapshot(); const [sx, sy, sz] = agent.getPosition?.() || [2, 0.5, 3]; const total = snapshot.byteLength; @@ -5826,26 +5869,54 @@ if (dimosMode) { bridge.wsSensors.send(prelude.buffer); let sent = 0; - let chunkN = 0; const sendNextChunk = () => { + if (bridge._serverPhysicsReady) { + bridge.resumePublishing(); + return; + } if (bridge.wsSensors.readyState !== WebSocket.OPEN) { console.warn("[DimosBridge] sensor WS closed mid-snapshot"); + bridge.resumePublishing(); return; } - // Backpressure: don't outpace the WS pump. - if (bridge.wsSensors.bufferedAmount > 4 * SNAPSHOT_CHUNK_SIZE) { - setTimeout(sendNextChunk, 50); + // Let Chrome's network service drain before adding more data. + // Re-posting MessageChannel tasks while bufferedAmount is high + // can monopolize the renderer task queue and prevent the socket + // pump from making progress at all. + if ( + bridge.wsSensors.bufferedAmount > + SNAPSHOT_BUFFER_HIGH_WATER + ) { + setTimeout(sendNextChunk, 25); return; } - const end = Math.min(sent + SNAPSHOT_CHUNK_SIZE, total); - bridge.wsSensors.send(snapshot.subarray(sent, end)); - sent = end; - chunkN++; - if (sent >= total) return; - setTimeout(sendNextChunk, 0); // yield to event loop + // Queue a finite batch, then yield through MessageChannel. + // Polling bufferedAmount from a continuously re-queued task can + // starve Chrome's WebSocket pump precisely when backpressure is + // present. A finite number of turns guarantees that JavaScript + // stops producing tasks and lets the network service drain. + let chunksThisTurn = 0; + while ( + sent < total && + chunksThisTurn < SNAPSHOT_CHUNKS_PER_TURN + ) { + const end = Math.min(sent + SNAPSHOT_CHUNK_SIZE, total); + bridge.wsSensors.send(snapshot.subarray(sent, end)); + sent = end; + chunksThisTurn++; + } + if (sent >= total) { + // Publishing resumes only when the bridge acknowledges + // restored server physics. Resuming on a timer can restart + // expensive software-rendered camera capture while the + // snapshot is still buffered, starving the transfer itself. + return; + } + scheduleSnapshotTask(sendNextChunk); }; sendNextChunk(); } catch (e) { + bridge.resumePublishing(); console.warn("[DimosBridge] snapshot send failed:", e); } } else { @@ -5888,12 +5959,27 @@ if (dimosMode) { return { assets: enriched }; }, getAgentPose: () => { + const serverPose = bridge._serverPose; + if ( + serverPose && + [serverPose.x, serverPose.y, serverPose.z, serverPose.yaw].every( + Number.isFinite, + ) + ) { + return { + x: serverPose.x, + y: serverPose.y, + z: serverPose.z, + yaw: serverPose.yaw, + pitch: 0, + }; + } const pos = agent.getPosition?.(); if (!pos) return null; - const camOffset = 0.3; - const cx = pos[0] + Math.sin(_dimosYaw) * camOffset; - const cz = pos[2] + Math.cos(_dimosYaw) * camOffset; - return { x: cx, y: pos[1], z: cz, yaw: _dimosYaw, pitch: 0 }; + // Eval distance rubrics score the robot body, matching startPose and + // authoritative server odometry. The observation camera's forward + // offset is a sensor detail, not robot progress. + return { x: pos[0], y: pos[1], z: pos[2], yaw: _dimosYaw, pitch: 0 }; }, }); // Register the singleton so workflow files importing `runEval` from diff --git a/misc/DimSim/src/style.css b/misc/DimSim/src/style.css index e6fbfcf4a5..a5527bad98 100644 --- a/misc/DimSim/src/style.css +++ b/misc/DimSim/src/style.css @@ -427,16 +427,22 @@ html[data-mode="sim"] .side-panel { font-size: 10px; padding: 1px 5px; } -/* Dimos-only: hide the sim side panel + command bar + top-left status - pill (the small "Click to look around" + B/G chip is the human-player - prompt; the agent is driven externally). Bottom-left WASD shortcuts - stay visible. */ +/* Dimos-only: hide the sim side panel + top-left status pill (the small + "Click to look around" + B/G chip is the human-player prompt; the agent is + driven externally). Keep a camera-only command bar so a headed browser can + switch between the independent user view and the external robot camera. */ body.dimos-mode #agent-panel, body.dimos-mode #sim-panel-open, -body.dimos-mode #agent-command-bar, body.dimos-mode .status-floating { display: none !important; } +body.dimos-mode #agent-command-bar { + display: flex !important; + width: auto; +} +body.dimos-mode #agent-command-bar > :not(#sim-camera-toggle) { + display: none !important; +} /* Floating status for sim mode */ .status-floating { position: fixed;