From 37efb87a2bfdc3241836d6fa4ea5a65bbcc72da6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?=
 =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= <grabardm@ml-dev.ru>
Date: Wed, 11 Nov 2026 01:35:00 +1000
Subject: [PATCH] fix(scene-context): keep the published snapshot fresh and
 name real camera topics

---
 .../iiwa_scene_context/node.py                |  9 ++++++-
 .../iiwa_scene_context/snapshot.py            |  4 +--
 src/iiwa_scene_context/test/test_node.py      | 25 +++++++++++++++++++
 3 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/src/iiwa_scene_context/iiwa_scene_context/node.py b/src/iiwa_scene_context/iiwa_scene_context/node.py
index b4ee9f8..45a7b49 100644
--- a/src/iiwa_scene_context/iiwa_scene_context/node.py
+++ b/src/iiwa_scene_context/iiwa_scene_context/node.py
@@ -138,6 +138,7 @@ class SceneContextNode(Node):
             freshness_sec=float(self.get_parameter('freshness_sec').value),
         )
         self._last_published: tuple[int, int] | None = None
+        self._last_publish_time = -1.0
         self._ready_client = self.create_client(Trigger, READY_SERVICE)
         self._ready_request_pending = False
         self._service = self.create_service(
@@ -199,9 +200,15 @@ class SceneContextNode(Node):
         if envelope is None:
             return
         key = (int(envelope['scene_revision']), int(envelope['state_version']))
-        if key == self._last_published:
+        now = self._now()
+        period = max(
+            0.05, float(self.get_parameter('publish_period_sec').value),
+        )
+        changed = key != self._last_published
+        if not changed and now - self._last_publish_time < period:
             return
         self._last_published = key
+        self._last_publish_time = now
         message = String()
         message.data = json.dumps(envelope, ensure_ascii=False)
         self._publisher.publish(message)
diff --git a/src/iiwa_scene_context/iiwa_scene_context/snapshot.py b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py
index f081be5..b140650 100644
--- a/src/iiwa_scene_context/iiwa_scene_context/snapshot.py
+++ b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py
@@ -23,12 +23,12 @@ from .catalog import (
 
 DEFAULT_CAMERAS = {
     'top': {
-        'topic': '/d455_top/image_raw',
+        'topic': '/d455_top/image_raw/image_color',
         'role': 'overview',
         'required_for': ['detect'],
     },
     'hand': {
-        'topic': '/camera/color',
+        'topic': '/camera/color/image_color',
         'role': 'close_inspection',
         'required_for': ['detect', 'capture'],
     },
diff --git a/src/iiwa_scene_context/test/test_node.py b/src/iiwa_scene_context/test/test_node.py
index 67299f4..4cb01be 100644
--- a/src/iiwa_scene_context/test/test_node.py
+++ b/src/iiwa_scene_context/test/test_node.py
@@ -102,3 +102,28 @@ def test_snapshot_scene_is_the_catalog_scene_not_the_state():
     expected = build_scene(store.spec, store.state, store.catalog)
     assert envelope['scene_json'] == expected
     assert 'pose' not in envelope['scene_json']
+
+
+def test_scene_topic_is_republished_to_keep_the_timestamp_fresh():
+    store = make_store()
+    store.set_scene_ready(True)
+    store.update(state(1), received_at=10.0)
+    node = object.__new__(SceneContextNode)
+    node.store = store
+    node._last_published = None
+    node._last_publish_time = -1.0
+    node._publisher = type('Publisher', (), {
+        'messages': [],
+        'publish': lambda self, message: self.messages.append(message),
+    })()
+    clock = {'now': 10.1}
+    node._now = lambda: clock['now']
+    node.get_parameter = lambda name: type('P', (), {
+        'value': 0.5 if name == 'publish_period_sec' else None,
+    })()
+    SceneContextNode._publish_if_new(node)
+    SceneContextNode._publish_if_new(node)
+    assert len(node._publisher.messages) == 1
+    clock['now'] = 10.7
+    SceneContextNode._publish_if_new(node)
+    assert len(node._publisher.messages) == 2