fix(scene-context): keep the published snapshot fresh and name real camera topics

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)

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'],
     },

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