Add InteractiveScene camera debug helpers (S5)

CHANGELOG.md

@@ -54,6 +54,15 @@ All notable changes to GeneLab are recorded here.
   time, so robots whose actuator parameters live in Python config —
   rather than in an MJCF file — can use the same named knobs without
   hand-translating to PD gains.
+- **Camera debug helpers.** Added
+  `InteractiveScene.draw_camera_frustums(camera_names=None, color=...)`
+  and `InteractiveScene.draw_camera_trajectory(positions, radius, color)`
+  wrapping Genesis 1.0's `scene.draw_debug_frustum` /
+  `scene.draw_debug_trajectory`. The frustum helper resolves names
+  against the scene's `CameraSensor` set; the trajectory helper passes
+  positions through verbatim. Added a public `gs_camera` property on
+  `CameraSensor` so the scene helper can reach the Genesis handle
+  without poking private state.
 
 ### Changed
 

docs/concepts/scene.en.md

@@ -31,6 +31,23 @@ Genesis APIs and Isaac Lab-style task code use different vocabulary. Wrappers is
 difference: MDP terms read stable GeneLab properties while the backend integration handles Genesis
 details.
 
+## Camera debug helpers
+
+`InteractiveScene` exposes two thin wrappers around Genesis 1.0's debug-draw
+API for inspecting attached cameras:
+
+- `draw_camera_frustums(camera_names=None, color=(1, 1, 1, 0.3))` draws the view
+  frustum of every (or named) `CameraSensor` on the scene. Returns the number of
+  frustums drawn. Raises if a name does not resolve to a `CameraSensor`.
+- `draw_camera_trajectory(positions, radius=0.002, color=(1, 0.5, 0, 0.8))`
+  draws a polyline through `positions` (one row per world-frame point). The
+  caller owns the buffer — typically a recorded camera path — so the scene
+  keeps no history.
+
+Both helpers must be called after `InteractiveScene.build()`; calling pre-build
+raises `RuntimeError`. Use them from a play-time hook to visualise where the
+RGB-D sensors are looking and how their pose evolved over a rollout.
+
 ## Where to continue
 
 - [Module Map](../reference/module-map.md)

docs/concepts/scene.zh.md

@@ -27,6 +27,22 @@ entity 表访问，例如用 `env.articulations["robot"].data` 读取 `RobotStat
 
 Genesis API 和 Isaac Lab 风格任务代码使用的词汇不同。wrapper 隔离这种差异：MDP term 读取稳定的 GeneLab 属性，后端集成负责 Genesis 细节。
 
+## 相机调试辅助
+
+`InteractiveScene` 提供两个对 Genesis 1.0 debug-draw API 的薄封装，用于检查
+挂载在场景上的相机：
+
+- `draw_camera_frustums(camera_names=None, color=(1, 1, 1, 0.3))` 绘制场景上
+  每一个（或指定名字的）`CameraSensor` 的视锥。返回实际绘制的视锥数。指定
+  的名字若没有对应 `CameraSensor` 会抛错。
+- `draw_camera_trajectory(positions, radius=0.002, color=(1, 0.5, 0, 0.8))`
+  在 `positions`（每行一个世界系坐标点）上画一条折线。调用方持有 buffer —— 
+  通常是录制下来的相机路径 —— 所以 scene 不维护任何历史。
+
+两个辅助方法都必须在 `InteractiveScene.build()` 之后调用；build 之前调用会抛
+`RuntimeError`。一般是从 play 阶段的 hook 里调用，用来可视化 RGB-D 传感器
+所看的视野以及一次 rollout 中位姿的变化。
+
 ## 继续阅读
 
 - [模块地图](../reference/module-map.md)

src/genelab/scene/interactive_scene.py

@@ -27,6 +27,9 @@
 from genelab.sensor import Sensor
 from genelab.terrains import TerrainImporter
 
+if TYPE_CHECKING:
+    from genelab.sensor.camera import CameraSensor
+
 
 _pyrender_save_patch_applied = False
 
@@ -340,6 +343,70 @@ def reset(self, env_ids: torch.Tensor) -> None:
         for art in self.articulations.values():
             art.reset(env_ids)
 
+    def draw_camera_frustums(
+        self,
+        *,
+        camera_names: tuple[str, ...] | None = None,
+        color: tuple[float, float, float, float] = (1.0, 1.0, 1.0, 0.3),
+    ) -> int:
+        """Draw the view frustum of one or more :class:`~genelab.sensor.CameraSensor` s.
+
+        Wraps Genesis 1.0's ``scene.draw_debug_frustum(camera, color=...)``. When
+        ``camera_names`` is ``None`` every ``CameraSensor`` on the scene is drawn;
+        otherwise only the named ones (a name that doesn't resolve to a
+        ``CameraSensor`` raises). Returns the number of frustums drawn so callers
+        can detect mis-named selections without inspecting the viewer state.
+
+        Cameras allocate their Genesis-side handle in
+        :meth:`~genelab.sensor.CameraSensor.pre_build_genesis`; calling this before
+        :meth:`build` raises a ``RuntimeError``.
+        """
+        if not self._built:
+            raise RuntimeError("draw_camera_frustums called before InteractiveScene.build()")
+        selected = self._select_camera_sensors(camera_names)
+        for sensor in selected:
+            self._gs_scene.draw_debug_frustum(sensor.gs_camera, color=color)
+        return len(selected)
+
+    def draw_camera_trajectory(
+        self,
+        positions: Any,
+        *,
+        radius: float = 0.002,
+        color: tuple[float, float, float, float] = (1.0, 0.5, 0.0, 0.8),
+    ) -> None:
+        """Draw a polyline through ``positions`` (one row per world-frame point).
+
+        Thin wrapper over Genesis 1.0's
+        ``scene.draw_debug_trajectory(poss, radius=…, color=…)``. The caller owns
+        the positions buffer (typically a recorded camera path or an inspection
+        waypoint sequence) — no history is maintained on the scene side.
+        """
+        if not self._built:
+            raise RuntimeError("draw_camera_trajectory called before InteractiveScene.build()")
+        self._gs_scene.draw_debug_trajectory(positions, radius=radius, color=color)
+
+    def _select_camera_sensors(self, names: tuple[str, ...] | None) -> "list[CameraSensor]":
+        from genelab.sensor.camera import CameraSensor
+
+        if names is None:
+            return [s for s in self._sensors.values() if isinstance(s, CameraSensor)]
+        selected: list[CameraSensor] = []
+        for n in names:
+            sensor = self._sensors.get(n)
+            if sensor is None:
+                raise KeyError(
+                    f"draw_camera_frustums: no sensor named {n!r} on this scene "
+                    f"(have: {sorted(self._sensors)})"
+                )
+            if not isinstance(sensor, CameraSensor):
+                raise TypeError(
+                    f"draw_camera_frustums: sensor {n!r} is not a CameraSensor "
+                    f"(got {type(sensor).__name__})"
+                )
+            selected.append(sensor)
+        return selected
+
     def close(self) -> None:
         scene = self._gs_scene
         if scene is None:

src/genelab/sensor/camera.py

@@ -151,6 +151,16 @@ def _validate_cfg(self) -> None:
                 f"render_rgb / render_depth / render_segmentation must be True"
             )
 
+    @property
+    def gs_camera(self) -> object | None:
+        """The underlying Genesis camera handle, or ``None`` before ``pre_build_genesis``.
+
+        Exposed for scene-level debug helpers (e.g.
+        :meth:`~genelab.scene.InteractiveScene.draw_camera_frustums`) that need the
+        handle to call Genesis's ``draw_debug_frustum`` API directly.
+        """
+        return self._cam
+
     def _allocate_camera(self, gs_scene: object, link: object) -> object:
         cam = gs_scene.add_camera(  # type: ignore[attr-defined]
             res=(self._cfg_typed.width, self._cfg_typed.height),

tests/test_scene_camera_debug.py

@@ -0,0 +1,149 @@
+"""Unit tests for ``InteractiveScene.draw_camera_frustums`` / ``draw_camera_trajectory``.
+
+The scene helpers wrap Genesis 1.0's ``scene.draw_debug_frustum(camera, color=...)``
+and ``scene.draw_debug_trajectory(poss, ...)``. The tests use a fake gs_scene that
+records every debug-draw call so we can assert (a) the right camera handle is
+forwarded for each selection and (b) selection errors are raised with useful
+messages — no real Genesis viewer needed.
+"""
+
+from typing import Any
+
+import pytest
+
+torch = pytest.importorskip("torch")
+
+
+class _FakeCameraHandle:
+    """Stand-in for the gs.Camera handle attached by ``CameraSensor._allocate_camera``."""
+
+    def __init__(self, name: str) -> None:
+        self.name = name
+
+
+class _FakeCameraSensor:
+    """Minimum subset of CameraSensor that ``_select_camera_sensors`` reads.
+
+    Subclasses the real ``CameraSensor`` class symbol so the ``isinstance`` check
+    inside the helper still passes — the helper only ever touches ``gs_camera``.
+    """
+
+    def __init__(self, name: str) -> None:
+        self._name = name
+        self._cam = _FakeCameraHandle(name)
+
+    @property
+    def gs_camera(self) -> Any:
+        return self._cam
+
+
+class _FakeGsScene:
+    """Records every debug-draw call so tests can assert on them."""
+
+    def __init__(self) -> None:
+        self.frustum_calls: list[tuple[Any, tuple[float, float, float, float]]] = []
+        self.trajectory_calls: list[tuple[Any, float, tuple[float, float, float, float]]] = []
+
+    def draw_debug_frustum(self, camera: Any, color: tuple[float, float, float, float]) -> None:
+        self.frustum_calls.append((camera, color))
+
+    def draw_debug_trajectory(
+        self, poss: Any, radius: float, color: tuple[float, float, float, float]
+    ) -> None:
+        self.trajectory_calls.append((poss, radius, color))
+
+
+def _build_scene(sensors: dict[str, Any], *, built: bool = True) -> Any:
+    """Construct just enough ``InteractiveScene`` surface for the debug helpers."""
+    from genelab.scene.interactive_scene import InteractiveScene
+
+    scene = InteractiveScene.__new__(InteractiveScene)
+    scene._gs_scene = _FakeGsScene()
+    scene._sensors = sensors
+    scene._built = built
+    return scene
+
+
+def _patch_camera_sensor_isinstance(monkeypatch: pytest.MonkeyPatch) -> None:
+    """``_select_camera_sensors`` uses ``isinstance(sensor, CameraSensor)``; route
+    the import to a class the fakes inherit from so the test fakes pass the check
+    without needing a real Genesis env to construct a real ``CameraSensor``."""
+    import genelab.sensor.camera as camera_mod
+
+    monkeypatch.setattr(camera_mod, "CameraSensor", _FakeCameraSensor)
+
+
+def test_draw_camera_frustums_default_draws_every_camera(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    _patch_camera_sensor_isinstance(monkeypatch)
+    sensors = {"front": _FakeCameraSensor("front"), "side": _FakeCameraSensor("side")}
+    scene = _build_scene(sensors)
+
+    n = scene.draw_camera_frustums()
+
+    assert n == 2
+    drawn = [call[0].name for call in scene._gs_scene.frustum_calls]
+    assert sorted(drawn) == ["front", "side"]
+
+
+def test_draw_camera_frustums_named_selection(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    _patch_camera_sensor_isinstance(monkeypatch)
+    sensors = {"front": _FakeCameraSensor("front"), "side": _FakeCameraSensor("side")}
+    scene = _build_scene(sensors)
+
+    n = scene.draw_camera_frustums(camera_names=("side",), color=(0.1, 0.2, 0.3, 0.4))
+
+    assert n == 1
+    cam, color = scene._gs_scene.frustum_calls[0]
+    assert cam.name == "side"
+    assert color == (0.1, 0.2, 0.3, 0.4)
+
+
+def test_draw_camera_frustums_unknown_name_raises(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    _patch_camera_sensor_isinstance(monkeypatch)
+    sensors = {"front": _FakeCameraSensor("front")}
+    scene = _build_scene(sensors)
+
+    with pytest.raises(KeyError, match="no sensor named 'ghost'"):
+        scene.draw_camera_frustums(camera_names=("ghost",))
+
+
+def test_draw_camera_frustums_non_camera_sensor_raises(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    _patch_camera_sensor_isinstance(monkeypatch)
+    sensors = {"front": _FakeCameraSensor("front"), "imu": object()}
+    scene = _build_scene(sensors)
+
+    with pytest.raises(TypeError, match="not a CameraSensor"):
+        scene.draw_camera_frustums(camera_names=("imu",))
+
+
+def test_draw_camera_frustums_pre_build_raises() -> None:
+    scene = _build_scene({}, built=False)
+    with pytest.raises(RuntimeError, match="before InteractiveScene.build"):
+        scene.draw_camera_frustums()
+
+
+def test_draw_camera_trajectory_forwards_args() -> None:
+    scene = _build_scene({})
+    poss = torch.tensor([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0]])
+
+    scene.draw_camera_trajectory(poss, radius=0.005, color=(0.0, 1.0, 0.0, 1.0))
+
+    assert len(scene._gs_scene.trajectory_calls) == 1
+    forwarded, radius, color = scene._gs_scene.trajectory_calls[0]
+    assert torch.equal(forwarded, poss)
+    assert radius == pytest.approx(0.005)
+    assert color == (0.0, 1.0, 0.0, 1.0)
+
+
+def test_draw_camera_trajectory_pre_build_raises() -> None:
+    scene = _build_scene({}, built=False)
+    with pytest.raises(RuntimeError, match="before InteractiveScene.build"):
+        scene.draw_camera_trajectory([[0.0, 0.0, 0.0]])