feat(evals): Habitat navigation benchmark: text-only arms, planner arm, video, parallel cases

dimos/agents/skills/nav_skills.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.
+
+"""Point-goal navigation tools over a planner that speaks ``goal`` / ``goal_reached``."""
+
+from dimos_lcm.std_msgs import Bool
+
+from dimos.agents.annotation import skill
+from dimos.agents.capabilities import CAP_MOVEMENT
+from dimos.core.core import rpc
+from dimos.core.module import Module
+from dimos.core.stream import In, Out
+from dimos.msgs.geometry_msgs.PointStamped import PointStamped
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+
+# Under the MCP client's 120 s call timeout.
+MAX_WAIT_S = 100.0
+
+
+class NavSkills(Module):
+    goal: Out[PointStamped]
+    stop_movement: Out[Bool]
+    finished: Out[Bool]
+    goal_reached: In[Bool]
+    odom: In[PoseStamped]
+
+    frame_id: str = "world"
+    _z: float = 0.0  # the floor the robot stands on; goals go there, not to z = 0
+
+    @rpc
+    def start(self) -> None:
+        super().start()
+        self.odom.subscribe(self._on_odom)
+
+    def _on_odom(self, pose: PoseStamped) -> None:
+        self._z = pose.z
+
+    @skill(uses=[CAP_MOVEMENT])
+    def go_to(self, x: float, y: float, wait_s: float = 90.0) -> str:
+        """Drive to a point in the world frame with the planner and wait for arrival.
+
+        Args:
+            x: target x in metres, world frame.
+            y: target y in metres, world frame.
+            wait_s: how long to wait for arrival before returning (max 100). The robot keeps
+                driving after a timeout; call go_to again to keep waiting, or stop_navigation().
+        """
+        self.goal.publish(PointStamped(x, y, self._z, frame_id=self.frame_id))
+        try:
+            self.goal_reached.get_next(timeout=min(wait_s, MAX_WAIT_S))
+        except Exception:
+            return f"not at ({x:.2f}, {y:.2f}) after {wait_s:.0f}s; still driving"
+        return f"reached ({x:.2f}, {y:.2f})"
+
+    @skill
+    def stop_navigation(self) -> str:
+        """Cancel the current goal and stop moving."""
+        self.stop_movement.publish(Bool(True))
+        return "stopped"
+
+    @skill
+    def finish(self, note: str = "") -> str:
+        """Declare the task complete; the evaluation stops timing here.
+
+        Args:
+            note: one line on what was achieved.
+        """
+        self.finished.publish(Bool(True))
+        return f"finished: {note}" if note else "finished"

dimos/agents/typesafe/agent.py

@@ -15,13 +15,15 @@
 
 from __future__ import annotations
 
+from dataclasses import replace
 import json
 import os
 from pathlib import Path
 import threading
 import time
 from typing import Any, Generic, TypeVar
 
+from dimos_lcm.std_msgs import Bool
 from langchain_core.messages import AIMessage, HumanMessage
 from langchain_core.messages.base import BaseMessage
 from reactivex.disposable import Disposable
@@ -50,6 +52,16 @@
 # ponytail: fixed steering gains; make them config if a robot needs different ones.
 SLOW_WITHIN_M = 1.5
 TURN_FULL_AT_DEG = 45.0
+TASK = (
+    "You are a mobile robot in a room. Each tick you receive this JSON: `goal` (what to do), "
+    "`robot` (your position, heading and last motion), `objects` (things in the room with their "
+    "world position, size, `distance` from you to their nearest edge, and `bearing`), and "
+    "`room.sectors` (the nearest obstacle in each direction around you). Drive toward the object "
+    "named in `goal`, around obstacles. Coordinates in `goal` only say which object is meant; "
+    "you cannot stand on an object's centre, so never compare them with your own position. The "
+    "task is finished when that object's `distance` is touching, or near with the robot stopped "
+    "as close as it can get: then report finished."
+)
 
 
 def typesafe_api_key() -> str | None:
@@ -80,7 +92,6 @@ class TypeSafeAgentConfig(ModuleConfig):
     angular_speed: float = 0.8
     linear_accel: float = 0.8
     angular_accel: float = 1.6
-    min_confidence: float = 0.5
     stop_threshold: float = 0.7
     reached_m: float = 0.5
     give_up_s: float = 5.0  # goal clears after this long without motion
@@ -102,6 +113,7 @@ class TypeSafeAgent(Module):
     cmd_vel: Out[Twist]
     agent: Out[BaseMessage]
     agent_idle: Out[bool]
+    finished: Out[Bool]
 
     def __init__(self, **kwargs: Any) -> None:
         super().__init__(**kwargs)
@@ -165,6 +177,8 @@ def _on_odometry(self, o: Odometry) -> None:
     @rpc
     def set_goal(self, goal: str | None) -> None:
         goal = (goal or "").strip() or None
+        if goal:
+            goal = goal.splitlines()[-1].strip()  # a briefing may precede the goal
         with self._lock:
             self._goal = goal
             self._target = self._current = ZERO
@@ -180,6 +194,13 @@ def set_goal(self, goal: str | None) -> None:
     def goal(self) -> str | None:
         return self._goal
 
+    @rpc
+    def set_trace_dir(self, path: str | None) -> None:
+        """One request/response pair per model call under *path*; None turns it off."""
+        with self._lock:
+            self.config.trace_dir = Path(path) if path is not None else None
+            self._seq = 0
+
     def _say(self, text: str) -> None:
         if text != self._last_message:
             self._last_message = text
@@ -210,6 +231,7 @@ def _tick(self) -> None:
         state = build_world_state(
             goal,
             pose,
+            task=TASK,
             detections_3d=det3d,
             detections_2d=det2d,
             lidar=self._lidar.get(stale),
@@ -218,11 +240,11 @@ def _tick(self) -> None:
             lidar_band=self.config.lidar_band,
         )
         qs = questions(tuple(dict.fromkeys(o["label"] for o in state["objects"])))
+        started, t0 = time.time(), time.monotonic()
         answers = self._client(state, qs)
-        self._trace(state, qs, answers)
+        self._trace(state, qs, answers, started, time.monotonic() - t0)
         drive = decode(
             answers,
-            min_confidence=self.config.min_confidence,
             stop_threshold=self.config.stop_threshold,
         )
         self._steer(state, drive)
@@ -234,7 +256,7 @@ def _steer(self, state: WorldState, drive: Drive) -> None:
         if target is not None and "distance_m" in target:
             dist, err = target["distance_m"], abs(target.get("bearing_deg", 0.0))
             if dist <= self.config.reached_m:
-                drive = Drive(0.0, 0.0, 0.0, True, drive.confidence, drive.labels, drive.target)
+                drive = replace(drive, x=0.0, y=0.0, yaw=0.0, stop=True)
             lin *= min(1.0, max(0.3, dist / SLOW_WITHIN_M))
             ang *= min(1.0, max(0.25, err / TURN_FULL_AT_DEG))
         self._set_target((drive.x * lin, drive.y * lin, drive.yaw * ang), immediate=drive.stop)
@@ -250,7 +272,11 @@ def _steer(self, state: WorldState, drive: Drive) -> None:
         self._say(
             f"drive {'/'.join(drive.labels)} stop={drive.stop} target={drive.target} confidence={drive.confidence:.2f}"
         )
-        if gave_up:
+        if drive.finished:
+            self.finished.publish(Bool(True))
+            self.set_goal(None)
+            self._say(f"finished at the target {drive.target}")
+        elif gave_up:
             self.set_goal(None)
             self._say("goal reached or unreachable; stopped")
 
@@ -290,13 +316,32 @@ def _publish_loop(self) -> None:
                 )
             self._stop_event.wait(dt)
 
-    def _trace(self, state: WorldState, qs: dict[str, Question], answers: Answers) -> None:
-        if self.config.trace_dir is None:
+    def _trace(
+        self,
+        state: WorldState,
+        qs: dict[str, Question],
+        answers: Answers,
+        started_at: float,
+        latency_s: float,
+    ) -> None:
+        """Same layout as ``dimos.agents.llm_trace`` so eval adapters read both."""
+        if self.config.trace_dir is None or self._client is None:
             return
         d = Path(self.config.trace_dir)
         d.mkdir(parents=True, exist_ok=True)
         self._seq += 1
         (d / f"{self._seq}-request.json").write_text(
-            json.dumps({"body": {"state": state, "questions": qs}})
+            json.dumps({"started_at": started_at, "body": {"state": state, "questions": qs}})
+        )
+        (d / f"{self._seq}-response.json").write_text(
+            json.dumps(
+                {
+                    "latency_s": latency_s,
+                    "body": {
+                        "model": self._client.last_model,
+                        "answers": answers,
+                        "usage": self._client.last_usage,
+                    },
+                }
+            )
         )
-        (d / f"{self._seq}-response.json").write_text(json.dumps({"body": {"answers": answers}}))

dimos/agents/typesafe/client.py

@@ -78,6 +78,8 @@ def __init__(
         self._timeout_s = timeout_s
         self._session = requests.Session()
         self._session.headers["Authorization"] = f"Bearer {api_key}"
+        self.last_usage: dict[str, int] = {}
+        self.last_model = ""
 
     def __call__(self, state: object, questions: Mapping[str, Question]) -> Answers:
         body = {"state": state, "model": self._model, "questions": questions}
@@ -88,7 +90,10 @@ def __call__(self, state: object, questions: Mapping[str, Question]) -> Answers:
                 continue
             if resp.status_code >= 400:
                 raise RuntimeError(f"TypeSafe {resp.status_code}: {resp.text[:300]}")
-            answers: Answers = resp.json()["answers"]
+            data = resp.json()
+            self.last_usage = data.get("usage") or {}
+            self.last_model = data.get("model", "")
+            answers: Answers = data["answers"]
             return answers
         raise AssertionError("unreachable")
 

dimos/agents/typesafe/demo_objects.py

@@ -11,10 +11,17 @@
 # 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.
-"""Publish fixed world-frame objects as Detection3DArray: a stand-in for a 3D detector."""
+"""Publish fixed world-frame objects as Detection3DArray: a stand-in for a 3D detector.
+
+``scene_json`` replaces ``objects`` with a ground-truth snapshot in the
+``detection3d_array_to_dict`` layout: ``{"detections": [{"label", "center_xyz", "size_xyz"}]}``.
+"""
 
 from __future__ import annotations
 
+import json
+from pathlib import Path
+import re
 import threading
 import time
 from typing import Any
@@ -42,9 +49,25 @@ class DemoObjectsConfig(ModuleConfig):
     objects: list[tuple[str, float, float, float]] = Field(
         default_factory=lambda: [("chair", 1.2, 2.0, 0.4)]
     )
+    scene_json: Path | None = None
+    exclude: str = "^wall"  # labels matching this regex are not published (walls crowd the list)
+    size: tuple[float, float, float] = (0.5, 0.5, 0.9)  # for ``objects``, which carry none
     rate_hz: float = 2.0
 
 
+Object = tuple[str, tuple[float, float, float], tuple[float, float, float]]  # label, center, size
+
+
+def load_scene_objects(path: Path, exclude: str = "") -> list[Object]:
+    raw = json.loads(Path(path).expanduser().read_text())
+    skip = re.compile(exclude) if exclude else None
+    return [
+        (str(d["label"]), tuple(map(float, d["center_xyz"])), tuple(map(float, d["size_xyz"])))  # type: ignore[misc]
+        for d in raw["detections"]
+        if skip is None or not skip.search(str(d["label"]))
+    ]
+
+
 class DemoObjects(Module):
     config: DemoObjectsConfig
     detections_3d: Out[Detection3DArray]
@@ -53,10 +76,16 @@ def __init__(self, **kwargs: Any) -> None:
         super().__init__(**kwargs)
         self._stop_event = threading.Event()
         self._thread: threading.Thread | None = None
+        self._objects: list[Object] = []
 
     @rpc
     def start(self) -> None:
         super().start()
+        self._objects = (
+            load_scene_objects(self.config.scene_json, self.config.exclude)
+            if self.config.scene_json is not None
+            else [(label, (x, y, z), self.config.size) for label, x, y, z in self.config.objects]
+        )
         self._stop_event.clear()
         self._thread = threading.Thread(target=self._publish_loop, name="DemoObjects", daemon=True)
         self._thread.start()
@@ -72,14 +101,14 @@ def stop(self) -> None:
     def _message(self) -> Detection3DArray:
         now = time.time()
         dets = []
-        for label, x, y, z in self.config.objects:
+        for label, center, size in self._objects:
             d = Detection3D()
             d.header = Header(now, "world")
             d.results = [
                 ObjectHypothesisWithPose(hypothesis=ObjectHypothesis(class_id=label, score=0.95))
             ]
             d.results_length = 1
-            d.bbox = BoundingBox3D(center=Pose(position=(x, y, z)), size=Vector3(0.5, 0.5, 0.9))
+            d.bbox = BoundingBox3D(center=Pose(position=center), size=Vector3(*size))
             dets.append(d)
         return Detection3DArray(
             detections_length=len(dets), header=Header(now, "world"), detections=dets