diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md
index 9587251..4ecef98 100644
--- a/doc/lwc-doc/docs/getting-started/configuration.en.md
+++ b/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.
diff --git a/doc/lwc-doc/docs/getting-started/configuration.md b/doc/lwc-doc/docs/getting-started/configuration.md
index 5a2acc4..5a7fc7d 100644
--- a/doc/lwc-doc/docs/getting-started/configuration.md
+++ b/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).
diff --git a/src/iiwa_config/config/cameras/d455_top.yaml b/src/iiwa_config/config/cameras/d455_top.yaml
index 55803be..416460f 100644
--- a/src/iiwa_config/config/cameras/d455_top.yaml
+++ b/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
\ No newline at end of file
+ update_rate: 30
diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro
index 8ecb474..81ad785 100644
--- a/src/iiwa_description/urdf/iiwa7.urdf.xacro
+++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro
@@ -17,6 +17,7 @@
+
@@ -26,6 +27,7 @@
+ false
/camera/color
30
True
@@ -35,6 +37,7 @@
+ false
/camera/depth
30
True
@@ -42,6 +45,23 @@
+
+
+
+ camera_rgb
+ /camera/color
+ 30
+ camera_optical_frame
+
+
+ camera_depth
+ true
+ /camera/depth
+ 30
+ camera_optical_frame
+
+
+
diff --git a/src/iiwa_description/urdf/tools/patron.xacro b/src/iiwa_description/urdf/tools/patron.xacro
index 59448e4..312f8a4 100644
--- a/src/iiwa_description/urdf/tools/patron.xacro
+++ b/src/iiwa_description/urdf/tools/patron.xacro
@@ -1,6 +1,8 @@
+
+
@@ -134,7 +136,8 @@
- 1.3089969
+
+ 1.3894765524
640
480
@@ -146,7 +149,7 @@
gaussian
- 0.005
+ 0.0
@@ -159,18 +162,20 @@
480
- 0.52
+ 0.01
6.0
- 0.52
+
+ 0.4
6.0
0.001
gaussian
- 0.02
+
+ 0.0
diff --git a/src/iiwa_utils/iiwa_utils/camera_spawner.py b/src/iiwa_utils/iiwa_utils/camera_spawner.py
index 6b09c96..ad7b0d9 100644
--- a/src/iiwa_utils/iiwa_utils/camera_spawner.py
+++ b/src/iiwa_utils/iiwa_utils/camera_spawner.py
@@ -178,12 +178,19 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
devices.append(
f' \n'
f' \n'
+ f' false\n'
f' {ros.topic}\n'
f' {ros.update_rate}\n'
f' True\n'
f' {cfg.name}_link\n'
f' \n'
- f' '
+ f' \n'
+ f' \n'
+ f' {cfg.name}\n'
+ f' {ros.topic}\n'
+ f' {ros.update_rate}\n'
+ f' {cfg.name}_link\n'
+ f' '
)
if cfg.range_finder is not None:
@@ -191,12 +198,20 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
devices.append(
f' \n'
f' \n'
+ f' false\n'
f' {ros.topic}\n'
f' {ros.update_rate}\n'
f' True\n'
f' {cfg.name}_link\n'
f' \n'
- f' '
+ f' \n'
+ f' \n'
+ f' {cfg.name}_depth\n'
+ f' true\n'
+ f' {ros.topic}\n'
+ f' {ros.update_rate}\n'
+ f' {cfg.name}_link\n'
+ f' '
)
devices_str = "\n".join(devices)
diff --git a/src/iiwa_utils/iiwa_utils/webots_camera.py b/src/iiwa_utils/iiwa_utils/webots_camera.py
new file mode 100644
index 0000000..f8ac4f6
--- /dev/null
+++ b/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)
diff --git a/src/iiwa_utils/package.xml b/src/iiwa_utils/package.xml
index ac9a617..f81c4ed 100644
--- a/src/iiwa_utils/package.xml
+++ b/src/iiwa_utils/package.xml
@@ -11,6 +11,9 @@
xacro
ament_index_python
webots_ros2_msgs
+ webots_ros2_driver
+ sensor_msgs
+ builtin_interfaces
python3-yaml
iiwa_msgs
python3-numpy
diff --git a/src/iiwa_utils/test/test_camera_timing.py b/src/iiwa_utils/test/test_camera_timing.py
new file mode 100644
index 0000000..284124f
--- /dev/null
+++ b/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)
diff --git a/src/iiwa_utils/test/test_webots_camera.py b/src/iiwa_utils/test/test_webots_camera.py
new file mode 100644
index 0000000..bd47ee1
--- /dev/null
+++ b/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()