[FEATURE] Add draw_debug_frustum and draw_debug_trajectory to scene API

genesis/engine/scene.py

@@ -42,6 +42,7 @@
 from genesis.utils.misc import tensor_to_array, sanitize_index
 from genesis.vis import Visualizer
 from genesis.utils.warnings import warn_once
+import genesis.utils.mesh as mu
 
 if TYPE_CHECKING:
     from genesis.engine.entities.base_entity import Entity
@@ -1250,6 +1251,54 @@ def draw_debug_points(self, poss, colors=(1.0, 0.0, 0.0, 0.5)):
         with self._visualizer.viewer_lock:
             return self._visualizer.context.draw_debug_points(poss, colors)
 
+    @gs.assert_built
+    def draw_debug_frustum(self, camera, color=(1.0, 1.0, 1.0, 0.3)):
+        """
+        Draws a camera frustum in the scene for visualization.
+
+        Parameters
+        ----------
+        camera : Camera
+            The camera object whose frustum will be visualized. Works for any
+            camera including sensor cameras.
+        color : array_like, shape (4,), optional
+            The color of the frustum in RGBA format.
+
+        Returns
+        -------
+        node : genesis.ext.pyrender.mesh.Mesh
+            The created debug object.
+        """
+        with self._visualizer.viewer_lock:
+            mesh = mu.create_camera_frustum(camera, color)
+            return self._visualizer.context.draw_debug_mesh(mesh)
+
+    @gs.assert_built
+    def draw_debug_trajectory(self, poss, radius=0.002, color=(1.0, 0.5, 0.0, 0.8)):
+        """
+        Draws a trajectory as a series of connected lines in the scene for visualization.
+
+        Parameters
+        ----------
+        poss : array_like, shape (N, 3)
+            The positions of the trajectory points.
+        radius : float, optional
+            The radius of the trajectory lines.
+        color : array_like, shape (4,), optional
+            The color of the trajectory in RGBA format.
+
+        Returns
+        -------
+        nodes : list
+            List of created debug line objects.
+        """
+        nodes = []
+        with self._visualizer.viewer_lock:
+            for i in range(len(poss) - 1):
+                node = self._visualizer.context.draw_debug_line(poss[i], poss[i + 1], radius, color)
+                nodes.append(node)
+        return nodes
+
     @gs.assert_built
     def draw_debug_path(self, qposs, entity, link_idx=-1, density=0.3, frame_scaling=1.0):
         """

tests/test_render.py

@@ -1041,6 +1041,70 @@ def test_draw_debug(renderer, show_viewer):
     assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS)
 
 
+@pytest.mark.required
+@pytest.mark.parametrize("renderer_type", [RENDERER_TYPE.RASTERIZER])
+def test_draw_debug_frustum_and_trajectory(renderer, show_viewer):
+    """Test that draw_debug_frustum and draw_debug_trajectory render visible content."""
+    if "GS_DISABLE_OFFSCREEN_MARKERS" in os.environ:
+        pytest.skip("Offscreen rendering of markers is forcibly disabled. Skipping...")
+
+    scene = gs.Scene(
+        renderer=renderer,
+        show_viewer=show_viewer,
+        show_FPS=False,
+    )
+    cam = scene.add_camera(
+        pos=(3.5, 0.5, 2.5),
+        lookat=(0.0, 0.0, 0.5),
+        res=(640, 640),
+        debug=True,
+        GUI=show_viewer,
+    )
+    # Add sensor_cam BEFORE build
+    sensor_cam = scene.add_camera(
+        res=(640, 480),
+        pos=(1.0, 0.0, 1.0),
+        lookat=(0.0, 0.0, 0.0),
+        fov=45,
+        GUI=False,
+    )
+    scene.build()
+
+    # Verify scene is initially blank
+    rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False)
+    assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS)
+
+    # Test draw_debug_frustum
+    scene.draw_debug_frustum(sensor_cam, color=(0.0, 1.0, 0.0, 0.5))
+    scene.visualizer.update()
+
+    rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False)
+    rgb_array_flat = rgb_array.reshape((-1, 3)).astype(np.int32)
+    assert (np.std(rgb_array_flat, axis=0) > 10.0).any()
+
+    # Clear and verify blank again
+    scene.clear_debug_objects()
+    scene.visualizer.update()
+    rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False)
+    assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS)
+
+    # Test draw_debug_trajectory
+    t = np.linspace(0, 2 * np.pi, 50)
+    positions = np.column_stack([np.cos(t), np.sin(t), np.ones_like(t) * 0.5])
+    scene.draw_debug_trajectory(positions, radius=0.01, color=(1.0, 0.5, 0.0, 1.0))
+    scene.visualizer.update()
+
+    rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False)
+    rgb_array_flat = rgb_array.reshape((-1, 3)).astype(np.int32)
+    assert (np.std(rgb_array_flat, axis=0) > 10.0).any()
+
+    # Clear and verify blank again
+    scene.clear_debug_objects()
+    scene.visualizer.update()
+    rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False)
+    assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS)
+
+
 @pytest.mark.slow  # ~150s
 @pytest.mark.required
 @pytest.mark.parametrize("n_envs", [0, 2])