Update camera settings

doc/lwc-doc/docs/getting-started/configuration.en.md

@@ -74,6 +74,50 @@ digital_twin:
 | `webots.cameras` | List of YAML configurations for connected cameras |
 | `rviz.config` | Path to the RViz configuration |
 
+### D455 cameras in Webots
+
+The overhead camera (`config/cameras/d455_top.yaml`) and the `patron` tool camera
+use **640×480 at 30 FPS** for RGB and depth. Nominal VGA depth horizontal FOV
+is 75°; 87° applies to HD. RGB approximates a central 4:3 crop of a 1280×800
+sensor with a 90° horizontal FOV, resulting in about 79.61°. Use the individual
+camera's 640×480 calibration for an exact RGB FOV.
+
+The selected simulation depth range is 0.4–6 m with 1 mm quantization.
+These are not accuracy guarantees or the exact VGA minimum distance of a D455.
+This ideal model omits random noise, stereo matching, the IR projector,
+distortion and the physical RGB/depth offset. Webots derives vertical FOV from
+horizontal FOV and aspect ratio; it cannot be set independently.
+Matching an individual device requires its intrinsics and extrinsics.
+
+The `iiwa_utils.webots_camera.WebotsCamera` plugin maintains an average 30 Hz
+cadence in simulation time without accumulating timestep rounding errors.
+With `basicTimeStep: 32`, frame intervals are 32 or 64 ms; uniform 33.33 ms
+intervals are impossible on this grid. Wall-clock FPS depends on simulation
+speed. The world's physics timestep does not need changing.
+
+Existing image topic names are preserved:
+
+| Camera | RGB (`bgra8`) | Depth (`32FC1`, metres) |
+|---|---|---|
+| Overhead | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Tool | `/camera/color/image_color` | `/camera/depth/image` |
+
+Each stream also publishes `camera_info` with the virtual camera matrix and
+matching timestamp. Depth offers `point_cloud` when subscribed: XYZ in metres
+using optical axes (X right, Y down, Z forward), with NaN for missing points.
+Frame IDs remain `d455_top_link` and `camera_optical_frame`;
+the plugin neither broadcasts TF nor aligns depth to RGB.
+
+Smoke test: start `cobot run local` → **Webots simulator**, subscribe to the four
+image topics, check image dimensions, matching `CameraInfo` timestamps, and the
+average rate from frame timestamps over 10 seconds of simulation time.
+`ros2 topic hz /camera/depth/image` measures wall-clock delivery, so it can report
+less than 30 when the simulation runs slowly.
+
+Sources: [D400/D455 datasheet](https://dev.realsenseai.com/download/42003/),
+[D455 RGB modes](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
 ### TCP gizmo in Webots
 
 Select the TCP marker, translate or rotate it with the native Webots gizmo, and release the left mouse button. Holding the button only changes the preview. Releasing it submits one complete trajectory. Another drag is accepted after that trajectory finishes. An unreachable target or a colliding trajectory resets the marker to the current TCP.

doc/lwc-doc/docs/getting-started/configuration.md

@@ -76,6 +76,50 @@ digital_twin:
 | `webots.cameras` | Список YAML-конфигов подключённых камер |
 | `rviz.config` | Путь к конфигурации RViz |
 
+### Камеры D455 в Webots
+
+Верхняя камера (`config/cameras/d455_top.yaml`) и камера инструмента `patron`
+используют **640×480, 30 кадров/с** для RGB и глубины. Горизонтальный угол глубины
+для VGA — 75°; 87° относится к HD. RGB использует приближение центральной обрезки
+сенсора 1280×800 с горизонтальным углом 90° до формата 4:3: около 79,61°.
+Точный угол RGB следует получить из калибровки конкретной камеры для 640×480.
+
+Глубина ограничена выбранным диапазоном симуляции 0,4–6 м, с шагом 1 мм.
+Это не гарантия точности и не точное значение минимальной дистанции D455 в VGA.
+Модель идеальная: без случайного шума, стереосопоставления, ИК-проектора,
+дисторсии и реального смещения между RGB и depth. Вертикальный угол Webots
+вычисляет из горизонтального угла и соотношения сторон; он не задаётся независимо.
+Для воспроизведения конкретного экземпляра нужны его intrinsics и extrinsics.
+
+Плагин `iiwa_utils.webots_camera.WebotsCamera` поддерживает среднюю частоту
+30 Гц по времени симуляции, не накапливая округление до шага Webots.
+При `basicTimeStep: 32` интервалы составляют 32 или 64 мс; равномерные интервалы
+33,33 мс на такой сетке невозможны. Частота по настенным часам зависит от скорости
+симуляции. Физический шаг мира менять не требуется.
+
+Имена топиков изображений сохранены:
+
+| Камера | RGB (`bgra8`) | Глубина (`32FC1`, метры) |
+|---|---|---|
+| Верхняя | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Инструмент | `/camera/color/image_color` | `/camera/depth/image` |
+
+Рядом публикуется `camera_info` с матрицей виртуальной камеры и той же временной
+меткой. Для глубины доступен `point_cloud` при наличии подписчиков: XYZ в метрах
+в оптических осях (X вправо, Y вниз, Z вперёд), отсутствующие точки — NaN.
+Сохранены идентификаторы кадров `d455_top_link` и `camera_optical_frame`;
+плагин не публикует TF и не выполняет совмещение depth с RGB.
+
+Проверка после `cobot run local` → **Webots simulator**: подписаться на четыре
+топика, проверить размеры `Image`, совпадение временных меток с `CameraInfo`
+и среднюю частоту по меткам кадров за 10 секунд модельного времени.
+`ros2 topic hz /camera/depth/image` измеряет доставку по настенным часам,
+поэтому при медленной симуляции значение может быть ниже 30.
+
+Источники: [спецификация D400/D455](https://dev.realsenseai.com/download/42003/),
+[режимы RGB D455](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
 ### Перемещение за TCP в Webots
 
 Порядок действий приведён в [руководстве по симуляции](concepts/simulation.md).

src/iiwa_config/config/cameras/d455_top.yaml

@@ -17,7 +17,9 @@ rotation: "0 1 0 1.5708"
 camera:
   width: 640
   height: 480
-  fieldOfView: 1.047        # ~60° в радианах
+  # VGA RGB: приближение центрального 4:3 crop сенсора 1280x800 с HFOV 90°.
+  # 2*atan(tan(90°/2)*(4/3)/(1280/800)) = 79.61°; уточнить по калибровке.
+  fieldOfView: 1.3894765524
   near: 0.1
   far: 0.0                  # 0 = без ограничений
   exposure: 1.0
@@ -40,13 +42,13 @@ camera:
 range_finder:
   width: 640
   height: 480
-  fieldOfView: 1.047
-  minRange: 0.1
-  maxRange: 5.0
-  resolution: -1.0          # -1 = без ограничений по дискретизации
+  fieldOfView: 1.308996939  # D455 VGA depth: HFOV 75°, не HD 87°
+  minRange: 0.4            # Выбранное ограничение симуляции, не точный Min-Z VGA
+  maxRange: 6.0
+  resolution: 0.001        # Шаг глубины 1 мм, не погрешность измерения
   noise: 0.0
 
   # Параметры публикации в ROS2
   ros:
     topic: /d455_top/depth/image_raw
-    update_rate: 15
+    update_rate: 30

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -17,6 +17,7 @@
     <xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
     <xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
 
+    <xacro:property name="tool_has_camera" value="false"/>
     <xacro:include filename="$(find iiwa_description)/urdf/tools/tool_active.xacro"/>
 
     <xacro:if value="$(arg simulate)">
@@ -26,6 +27,7 @@
 
             <device reference="camera_rgb" type="Camera">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/color</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -35,6 +37,7 @@
 
             <device reference="camera_depth" type="RangeFinder">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/depth</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -42,6 +45,23 @@
                 </ros>
             </device>
 
+            <!-- Keep the 30 Hz deadlines independent of Webots' 32 ms steps. -->
+            <xacro:if value="${tool_has_camera}">
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_rgb</device>
+                    <topicName>/camera/color</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_depth</device>
+                    <depth>true</depth>
+                    <topicName>/camera/depth</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+            </xacro:if>
+
         </webots>
 
         <ros2_control name="iiwaGazeboControl" type="system">

src/iiwa_description/urdf/tools/patron.xacro

@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_patron">
 
+    <xacro:property name="tool_has_camera" value="true"/>
+
     <link name="camera_holder">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -134,7 +136,8 @@
     <gazebo reference="camera_optical_frame">
         <sensor type="camera" name="camera_rgb">
             <camera>
-                <horizontal_fov>1.3089969</horizontal_fov>
+                <!-- VGA RGB central-crop approximation: 79.61 deg; calibrate per device. -->
+                <horizontal_fov>1.3894765524</horizontal_fov>
                 <image>
                     <width>640</width>
                     <height>480</height>
@@ -146,7 +149,7 @@
                 </clip>
                 <noise>
                     <type>gaussian</type>
-                    <stddev>0.005</stddev>
+                    <stddev>0.0</stddev>
                 </noise>
             </camera>
         </sensor>
@@ -159,18 +162,20 @@
                     <height>480</height>
                 </image>
                 <clip>
-                    <near>0.52</near>
+                    <near>0.01</near>
                     <far>6.0</far>
                 </clip>
             </camera>
             <range>
-                <min>0.52</min>
+                <!-- Conservative simulation cutoff, not calibrated VGA Min-Z. -->
+                <min>0.4</min>
                 <max>6.0</max>
                 <resolution>0.001</resolution>
             </range>
             <noise>
                 <type>gaussian</type>
-                <stddev>0.02</stddev>
+                <!-- No arbitrary Gaussian noise: this is an ideal depth model. -->
+                <stddev>0.0</stddev>
             </noise>
         </sensor>
     </gazebo> 

src/iiwa_utils/iiwa_utils/camera_spawner.py

@@ -178,12 +178,19 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}" type="Camera">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}</device>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     if cfg.range_finder is not None:
@@ -191,12 +198,20 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}_depth" type="RangeFinder">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}_depth</device>\n'
+            f'      <depth>true</depth>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     devices_str = "\n".join(devices)

src/iiwa_utils/iiwa_utils/webots_camera.py

@@ -0,0 +1,114 @@
+"""Publish Webots camera frames without accumulating timestep rounding drift."""
+import array
+import math
+import sys
+
+import numpy as np
+import rclpy
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+from builtin_interfaces.msg import Time
+from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
+
+
+class FrameSchedule:
+    """Keep deadlines on the requested cadence, with at most one frame per step."""
+
+    def __init__(self, rate, now):
+        if not math.isfinite(rate) or rate <= 0:
+            raise ValueError('Camera update rate must be finite and positive')
+        self.period = 1.0 / rate
+        self.last_time = now
+        self.next_time = now + self.period
+
+    def due(self, now):
+        if now < self.last_time:
+            self.next_time = now + self.period
+        self.last_time = now
+        if now + 1e-9 < self.next_time:
+            return False
+        elapsed = max(0, math.floor((now - self.next_time + 1e-9) / self.period))
+        self.next_time += (elapsed + 1) * self.period
+        return True
+
+
+class WebotsCamera:
+    """One RGB or depth stream; preserves the stock driver's topic suffixes."""
+
+    def init(self, webots_node, properties):
+        self._robot = webots_node.robot
+        name = properties['device']
+        self._device = self._robot.getDevice(name)
+        if self._device is None:
+            raise ValueError(f'Camera device not found: {name}')
+        self._depth = properties.get('depth', 'false') == 'true'
+        rate = float(properties.get('updateRate', '30'))
+        self._schedule = FrameSchedule(rate, self._robot.getTime())
+        timestep = int(self._robot.getBasicTimeStep())
+        if timestep <= 0 or rate * timestep > 1000:
+            raise ValueError('Camera rate exceeds the Webots simulation step rate')
+        self._device.enable(timestep)
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        self._node = rclpy.create_node(f'{name}_publisher')
+        qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.RELIABLE)
+        topic = properties['topicName']
+        suffix = '/image' if self._depth else '/image_color'
+        self._publisher = self._node.create_publisher(Image, topic + suffix, qos)
+        self._info_publisher = self._node.create_publisher(CameraInfo, topic + '/camera_info', qos)
+        self._info = CameraInfo()
+        self._info.header.frame_id = properties['frameName']
+        self._info.width = self._device.getWidth()
+        self._info.height = self._device.getHeight()
+        focal = self._info.width / (2 * math.tan(self._device.getFov() / 2))
+        cx, cy = self._info.width / 2, self._info.height / 2
+        self._info.distortion_model = 'plumb_bob'
+        self._info.d = [0.0] * 5
+        self._info.k = [focal, 0.0, cx, 0.0, focal, cy, 0.0, 0.0, 1.0]
+        self._info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+        self._info.p = [focal, 0.0, cx, 0.0, 0.0, focal, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
+        if self._depth:
+            self._cloud_publisher = self._node.create_publisher(
+                PointCloud2, topic + '/point_cloud', qos)
+            u, v = np.meshgrid(np.arange(self._info.width), np.arange(self._info.height))
+            self._rays = np.stack(((u - cx) / focal, (v - cy) / focal,
+                                   np.ones_like(u)), axis=-1).astype(np.float32)
+
+    def step(self):
+        now = self._robot.getTime()
+        if not self._schedule.due(now):
+            return
+        if self._depth:
+            data = array.array('f', self._device.getRangeImage()).tobytes()
+        else:
+            data = self._device.getImage()
+        if data is None:
+            return
+        nanoseconds = round(now * 1_000_000_000)
+        self._info.header.stamp = Time(
+            sec=nanoseconds // 1_000_000_000, nanosec=nanoseconds % 1_000_000_000)
+        message = Image()
+        message.header = self._info.header
+        message.width, message.height = self._info.width, self._info.height
+        message.encoding = '32FC1' if self._depth else 'bgra8'
+        message.is_bigendian = int(sys.byteorder == 'big') if self._depth else 0
+        message.step = message.width * 4
+        message.data = data
+        self._publisher.publish(message)
+        self._info_publisher.publish(self._info)
+        if self._depth and self._cloud_publisher.get_subscription_count():
+            depth = np.frombuffer(data, dtype=np.float32).reshape(message.height, message.width)
+            # Optical coordinates: x right, y down, z forward, all in metres.
+            valid = np.isfinite(depth) & (depth > 0)
+            points = self._rays * np.where(valid, depth, 0.0)[..., None]
+            points[~valid] = np.nan
+            cloud = PointCloud2()
+            cloud.header = message.header
+            cloud.width, cloud.height = message.width, message.height
+            cloud.fields = [PointField(name=name, offset=4 * i,
+                                       datatype=PointField.FLOAT32, count=1)
+                            for i, name in enumerate(('x', 'y', 'z'))]
+            cloud.is_bigendian = bool(message.is_bigendian)
+            cloud.point_step, cloud.row_step = 12, message.width * 12
+            cloud.is_dense = bool(valid.all())
+            cloud.data = points.tobytes()
+            self._cloud_publisher.publish(cloud)

src/iiwa_utils/package.xml

@@ -11,6 +11,9 @@
   <depend>xacro</depend>
   <depend>ament_index_python</depend>
   <depend>webots_ros2_msgs</depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
   <depend>python3-yaml</depend>
   <depend>iiwa_msgs</depend>
   <depend>python3-numpy</depend>

src/iiwa_utils/test/test_camera_timing.py

@@ -0,0 +1,33 @@
+"""Camera cadence must not halve when 30 Hz is sampled on 32 ms steps."""
+import pytest
+
+from iiwa_utils.webots_camera import FrameSchedule
+
+
+def test_30_hz_on_webots_32_ms_steps():
+    schedule = FrameSchedule(30, 0.0)
+    stamps = [tick * 0.032 for tick in range(1, 3126)
+              if schedule.due(tick * 0.032)]
+    assert len(stamps) == 3000
+    assert all(b > a for a, b in zip(stamps, stamps[1:]))
+    assert max(b - a for a, b in zip(stamps, stamps[1:])) <= 0.064001
+
+
+def test_delayed_step_does_not_publish_catchup_burst():
+    schedule = FrameSchedule(30, 0.0)
+    assert schedule.due(1.0)
+    assert not schedule.due(1.0)
+    assert not schedule.due(1.001)
+    assert schedule.due(1.034)
+
+
+def test_time_reset_restarts_schedule():
+    schedule = FrameSchedule(30, 10.0)
+    assert not schedule.due(0.0)
+    assert schedule.due(0.034)
+
+
+@pytest.mark.parametrize('rate', [0, -1, float('nan'), float('inf')])
+def test_invalid_rates_rejected(rate):
+    with pytest.raises(ValueError):
+        FrameSchedule(rate, 0.0)

src/iiwa_utils/test/test_webots_camera.py

@@ -0,0 +1,66 @@
+"""Check published pixels, units and calibration with a small device adapter."""
+import array
+import copy
+import math
+from types import SimpleNamespace
+
+import pytest
+import numpy as np
+
+from iiwa_utils import webots_camera as camera
+
+
+@pytest.mark.parametrize('depth', [False, True])
+def test_messages_match_device_and_share_timestamp(monkeypatch, depth):
+    messages = {}
+    now = [0.0]
+    device = SimpleNamespace(
+        enable=lambda period: None,
+        getWidth=lambda: 2, getHeight=lambda: 1,
+        getFov=lambda: math.pi / 2,
+        getImage=lambda: bytes([1, 2, 3, 255, 4, 5, 6, 255]),
+        getRangeImage=lambda: [1.25, float('inf')],
+    )
+    robot = SimpleNamespace(getDevice=lambda name: device,
+                            getBasicTimeStep=lambda: 32, getTime=lambda: now[0])
+
+    def publisher(message_type, topic, qos):
+        messages[topic] = []
+        return SimpleNamespace(publish=lambda msg: messages[topic].append(copy.deepcopy(msg)),
+                               get_subscription_count=lambda: 1)
+
+    monkeypatch.setattr(camera.rclpy, 'ok', lambda: True)
+    monkeypatch.setattr(camera.rclpy, 'create_node',
+                        lambda name: SimpleNamespace(create_publisher=publisher))
+    plugin = camera.WebotsCamera()
+    plugin.init(SimpleNamespace(robot=robot), dict(
+        device='test_camera', depth=str(depth).lower(), updateRate='30',
+        topicName='/test', frameName='test_optical_frame'))
+    for tick in range(1, 101):
+        now[0] = tick * 0.032
+        plugin.step()
+    images = messages['/test/image' if depth else '/test/image_color']
+    infos = messages['/test/camera_info']
+    assert len(images) == len(infos) == 96
+    image, info = images[-1], infos[-1]
+    assert image.header == info.header
+    assert image.header.frame_id == 'test_optical_frame'
+    assert (image.width, image.height, image.step) == (2, 1, 8)
+    assert image.header.stamp.sec == 3
+    assert image.header.stamp.nanosec == 200_000_000
+    assert info.k[0] == pytest.approx(1.0)
+    assert info.k[2] == 1.0
+    assert info.k[5] == 0.5
+    if depth:
+        assert image.encoding == '32FC1'
+        values = array.array('f', image.data.tobytes())
+        assert values[0] == 1.25
+        assert math.isinf(values[1])
+        cloud = messages['/test/point_cloud'][-1]
+        assert cloud.header == image.header
+        xyz = np.frombuffer(cloud.data, dtype=np.float32).reshape(-1, 3)
+        np.testing.assert_allclose(xyz[0], [-1.25, -0.625, 1.25])
+        assert np.isnan(xyz[1]).all()
+    else:
+        assert image.encoding == 'bgra8'
+        assert image.data.tobytes() == device.getImage()