append spawn objects

cobot-setting.yaml

@@ -22,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 1                    # точное общее количество; модели могут повторяться
+      models:
+        # - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+        #   rotation_deg: [0, 90, 0]  # X, Y, Z в градусах, относительно робота
+        # - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+        #   rotation_deg: [90, 0, 0]
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [90, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+        #   rotation_deg: [1.57, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+        #   rotation_deg: [1.57, 0, 0]
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
     - pkg://iiwa_config/config/cameras/d455_top.yaml
 

doc/lwc-doc/docs/getting-started/concepts/simulation.en.md

@@ -39,3 +39,96 @@ Set `digital_twin.tcp_gizmo.enabled: false` and restart to disable this mode. Th
 - Motion still obeys configured joint and TCP limits; simulation does not reproduce all physical robot protections.
 - Physics is approximate, including inertia, friction, and elasticity.
 - FRI is not used; communication goes through the Webots ROS 2 driver.
+
+## Spawning textured objects
+
+Set `digital_twin.webots.objects` in `cobot-setting.yaml`:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+Webots spawns exactly `count` instances once at startup, independently choosing
+from `models` with replacement. This list specifies allowed models, not individual
+quantities. An absent section, `enabled: false`, or `count: 0` disables spawning.
+Paths support `pkg://`, absolute paths and paths relative to the settings file.
+Currently only Wavefront OBJ is supported.
+
+Bounds are in metres in the robot base frame. The spawner uses the same
+`webots.transform` and `webots.rotation` (axis and angle in radians) as the robot:
+`p_world = t_robot + R_robot * p_local`. They constrain initial spawn positions;
+physics may move the objects afterwards. Moving the robot with the mouse after
+startup does not relocate existing objects.
+
+The spawn anchor is the bounding-box XY centre and bottom Z. Exported vertex
+offsets are compensated without modifying the original files, UVs, scale
+(1 unit = 1 metre); orientation is configured per model. Bounds constrain the anchor, not the entire
+object. Random placements may overlap; choose a sufficiently large area.
+
+`CadShape` loads OBJ materials through `mtllib` and textures through MTL. Preserve
+relative asset paths: for example, `hammer1.mtl` references
+`hammer1_basecolor.png`. The complete `objects` directory is installed with
+`iiwa_description`. Missing models, materials or textures fail before the first
+spawn request. Physics uses an approximate bounding Box and a mass of 0.5 kg,
+not calibrated hammer inertia or grasp geometry. Objects are not automatically
+added to the MoveIt Planning Scene.
+
+The `object_spawner` node calls `/Ros2Supervisor/spawn_node_from_string`
+(`webots_ros2_msgs/srv/SpawnNodeFromString`) once per object. A rejected request
+aborts remaining work. Timeouts are not retried to avoid duplicates. The node
+exits after success and only runs in simulation.
+
+### Simulation smoke test
+
+1. Build with `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Run `source install/setup.bash`, then `cobot run local`, selecting Webots.
+3. Inspect the five nodes `spawned_object_0000` through `spawned_object_0004`, their
+   textures and the `All 5 objects spawned` log.
+4. Change `transform` and `rotation`, restart and confirm the spawn area translates
+   and rotates with the robot base. Pause before objects fall to inspect initial positions.
+5. Check `count: 0`, operation without cameras and rejection of missing models.
+
+Reference: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Per-model rotation
+
+Each `models` entry accepts `path` and `rotation_deg: [X, Y, Z]`.
+Angles are fixed degrees: apply X, then Y, then Z about the robot base axes,
+`R_model = Rz * Ry * Rx` and `R_world = R_robot * R_model`. For example:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+The supplied OBJ hammers have their long axis along Z. `[0, 0, 0]` gives an
+initial vertical orientation, `[90, 0, 0]` lays the hammer sideways and
+`[0, 0, 45]` turns it around the vertical axis. Both configuration files now
+explicitly set `[0, 0, 0]` for each model; adjust each independently.
+Legacy path-only entries remain supported and imply zero rotation.
+
+Both the visible model and its collision Box rotate. Height is recalculated:
+`area.z` locates the bottom of the rotated collision bounds in the robot frame;
+X/Y locate their centre. Model rotation does not rotate the spawn area.
+Changes take effect on the next launch.
+
+This sets the initial orientation, not a constraint. Gravity and contact can
+still topple the hammer. Pause simulation at spawn to inspect it. Smoke-test
+two models with different angles, check the Box stays above `area.z`, and check
+that changing `webots.rotation` preserves each model's relative orientation.

doc/lwc-doc/docs/getting-started/concepts/simulation.md

@@ -39,3 +39,101 @@
 - Действуют настроенные ограничения суставов и TCP; симуляция не воспроизводит все защиты физического робота
 - Физика не идеальна — инерция, трение и упругость приближённые
 - FRI-протокол не задействован — связь идёт через Webots-драйвер ROS 2
+
+## Спавн объектов с текстурами
+
+В `cobot-setting.yaml` (секция `digital_twin.webots`) задайте:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+При запуске Webots создаётся ровно `count` объектов: каждая модель независимо
+выбирается из `models`, повторения разрешены. `enabled: false`, `count: 0` или
+отсутствие секции отключает спавн. Список — разрешённые модели, а не количество
+экземпляров каждой модели. Пути могут быть `pkg://`, абсолютными или относительно
+файла настроек. Пока поддерживается Wavefront OBJ.
+
+Область задаётся в метрах в системе основания робота: применяется та же пара
+`webots.transform` / `webots.rotation` (ось и угол в радианах), что и при спавне
+робота. Формула: `p_world = t_robot + R_robot * p_local`. Это начальная область,
+а не ограничитель движения: физика затем может перемещать объекты. Перемещение
+робота мышью после запуска не переносит уже созданные предметы.
+
+Точка спавна — центр габаритов по X/Y и нижняя точка по Z. Смещение вершин
+в экспортированном OBJ компенсируется автоматически; исходные файлы, UV,
+масштаб (1 единица = 1 метр) сохраняется; ориентация задаётся отдельно для каждой модели. Диапазон ограничивает
+эту точку, а не весь габарит. Пересечения объектов при случайном размещении
+не исключаются: выбирайте достаточно просторную область.
+
+`CadShape` загружает материалы из `mtllib` и текстуры из MTL. Сохраняйте OBJ,
+MTL и PNG в исходной структуре каталогов: например, `hammer1.mtl` ссылается на
+`hammer1_basecolor.png`. Весь каталог `objects` устанавливается вместе с
+`iiwa_description`. Отсутствующие модели, MTL и текстуры вызывают ошибку до
+первого запроса спавна. Для физики используется упрощённый габаритный Box и
+масса 0.5 кг; это не точная модель инерции или захвата молотка. В MoveIt Planning
+Scene эти объекты автоматически не добавляются.
+
+Нода `object_spawner` вызывает
+`/Ros2Supervisor/spawn_node_from_string` (`webots_ros2_msgs/srv/SpawnNodeFromString`)
+однократно для каждого предмета. Отказ сервиса прерывает оставшийся спавн;
+тайм-аут не повторяется, чтобы не создавать дубликаты. Нода завершается после
+успеха. На физическом роботе она не запускается.
+
+### Проверка в симуляции
+
+1. Пересоберите пакеты:
+   `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Выполните `source install/setup.bash`, затем `cobot run local` и выберите Webots.
+3. Проверьте пять узлов `spawned_object_0000` … `spawned_object_0004` в дереве сцены,
+   текстуры молотков и сообщение `All 5 objects spawned`.
+4. Измените `transform` и `rotation`, перезапустите: область должна переместиться
+   и повернуться вместе с основанием робота. Для проверки начальных координат
+   используйте паузу симулятора до падения объектов.
+5. Проверьте `count: 0`, запуск без камер и ошибку при несуществующей модели.
+
+Справочник: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Индивидуальный поворот модели
+
+Каждая запись `models` может содержать `path` и `rotation_deg: [X, Y, Z]`.
+Углы фиксированные, в градусах; сначала применяется поворот вокруг X, затем Y,
+затем Z в системе основания робота: `R_model = Rz * Ry * Rx`,
+`R_world = R_robot * R_model`. Например:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+В предоставленных OBJ длинная ось молотка направлена по Z: `[0, 0, 0]`
+задаёт вертикальную начальную ориентацию, `[90, 0, 0]` кладёт молоток набок,
+а `[0, 0, 45]` поворачивает его вокруг вертикальной оси. В обоих конфигурационных
+файлах для всех моделей явно указано `[0, 0, 0]`; изменяйте каждую независимо.
+Записи только с путём остаются совместимыми и означают нулевой поворот.
+
+Поворачиваются и видимая модель, и её коллизионный Box. После поворота
+пересчитывается высота: `area.z` задаёт нижнюю точку повёрнутого коллизионного
+габарита в системе робота, а X/Y — его центр. Поворот модели не поворачивает
+саму область спавна. Изменения применяются при следующем запуске.
+
+Это начальная ориентация, а не фиксация: после появления гравитация и контакты
+могут опрокинуть молоток. Для проверки поставьте симуляцию на паузу при спавне.
+Проверьте разные углы у двух моделей, отсутствие погружения Box ниже `area.z`
+и сохранение локальных углов при изменении `webots.rotation`.

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_bringup/launch/supported/simulation_nodes.py

@@ -1,4 +1,5 @@
 import json
+from dataclasses import asdict
 
 from launch_ros.actions import Node
 from webots_ros2_driver.webots_controller import WebotsController
@@ -7,12 +8,23 @@ from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type
 
 
 def make_simulation_nodes(settings) -> list:
-    """Возвращает ноды камер для симуляции: [camera_spawner, *camera_controllers].
+    """Create the configured object spawner and camera nodes for Webots."""
-
+    nodes = []
-    Если камеры не заданы в настройках — возвращает пустой список.
+    webots = settings.digital_twin.webots
-    """
+    if webots.objects.enabled and webots.objects.count:
+        nodes.append(Node(
+            package="iiwa_utils",
+            executable="object_spawner",
+            name="object_spawner",
+            output="screen",
+            parameters=[{
+                "objects_config": json.dumps(asdict(webots.objects)),
+                "robot_translation": webots.transform,
+                "robot_rotation": webots.rotation,
+            }],
+        ))
     if not settings.digital_twin.webots.cameras:
-        return []
+        return nodes
 
     camera_spawner = Node(
         package="iiwa_utils",
@@ -37,4 +49,4 @@ def make_simulation_nodes(settings) -> list:
             respawn=True,
         ))
 
-    return [camera_spawner, *camera_controllers]
+    return [*nodes, camera_spawner, *camera_controllers]

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
+  fieldOfView: 1.308996939  # D455 VGA depth: HFOV 75°, не HD 87°
-  minRange: 0.1
+  minRange: 0.4            # Выбранное ограничение симуляции, не точный Min-Z VGA
-  maxRange: 5.0
+  maxRange: 6.0
-  resolution: -1.0          # -1 = без ограничений по дискретизации
+  resolution: 0.001        # Шаг глубины 1 мм, не погрешность измерения
   noise: 0.0
 
   # Параметры публикации в ROS2
   ros:
     topic: /d455_top/depth/image_raw
-    update_rate: 15
+    update_rate: 30

src/iiwa_config/config/setting.yaml

@@ -22,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 5                    # точное общее количество; модели могут повторяться
+      models:
+        - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
       - pkg://iiwa_config/config/cameras/d455_top.yaml
 

src/iiwa_description/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY resource worlds urdf
+  DIRECTORY resource worlds urdf objects
   DESTINATION share/${PROJECT_NAME}
 )
 

src/iiwa_description/objects/hammer/hammer1.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 01_|_circular_ground_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_exposed_cut_wood
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_honey_hickory_longitudinal_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_satin_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png

src/iiwa_description/objects/hammer/hammer2.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 02_|_black_oxide_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_blue_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_graphite_moulded_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_machined_steel_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png

src/iiwa_description/objects/hammer/hammer3.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 03_|_ash_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_blue_tempered_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_bright_milled_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_pale_ash_straight_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png

src/iiwa_description/objects/hammer/hammer4.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 04_|_dark_walnut_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_polished_ball_and_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_walnut_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_warm_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png

src/iiwa_description/objects/hammer/hammer5.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 05_|_charcoal_anti_vibration_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_dark_textured_forged_coating
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_machined_impact_faces
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_orange_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png

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/object_scene.py

@@ -0,0 +1,196 @@
+"""Validated object configuration and Webots scene generation (metres/radians)."""
+import json
+import math
+import random
+import shlex
+from dataclasses import dataclass, field
+from pathlib import Path
+
+from scipy.spatial.transform import Rotation
+
+
+@dataclass(frozen=True)
+class ObjectModelCfg:
+    path: str
+    rotation_deg: tuple[float, float, float] = (0.0, 0.0, 0.0)
+
+
+@dataclass(frozen=True)
+class ObjectsCfg:
+    enabled: bool = False
+    count: int = 0
+    models: list[ObjectModelCfg] = field(default_factory=list)
+    area: dict[str, list[float]] = field(default_factory=dict)
+
+
+def parse_objects(raw, resolve_path) -> ObjectsCfg:
+    if raw is None:
+        return ObjectsCfg()
+    if not isinstance(raw, dict):
+        raise ValueError('digital_twin.webots.objects must be a mapping')
+    enabled = raw.get('enabled', False)
+    count = raw.get('count', 0)
+    models = raw.get('models', [])
+    area = raw.get('area', {})
+    if type(enabled) is not bool:
+        raise ValueError('objects.enabled must be true or false')
+    if type(count) is not int or count < 0:
+        raise ValueError('objects.count must be an integer >= 0')
+    if not isinstance(models, list):
+        raise ValueError('objects.models must be a list of paths or model mappings')
+    if enabled and count and not models:
+        raise ValueError('objects.models must not be empty when count > 0')
+    if not isinstance(area, dict):
+        raise ValueError('objects.area must be a mapping')
+    parsed_area = {}
+    if area or (enabled and count):
+        for axis in 'xyz':
+            bounds = area.get(axis)
+            if (not isinstance(bounds, (list, tuple)) or len(bounds) != 2
+                    or any(type(v) not in (int, float) or not math.isfinite(v) for v in bounds)
+                    or bounds[0] > bounds[1]):
+                raise ValueError(f'objects.area.{axis} must be [min, max], finite metres, min <= max')
+            parsed_area[axis] = list(map(float, bounds))
+    resolved = []
+    for index, entry in enumerate(models):
+        if isinstance(entry, str):
+            entry = {'path': entry}
+        if not isinstance(entry, dict) or entry.keys() - {'path', 'rotation_deg'}:
+            raise ValueError(f'objects.models[{index}] requires path and optional rotation_deg')
+        path = entry.get('path')
+        angles = entry.get('rotation_deg', (0, 0, 0))
+        if not isinstance(path, str) or not path.strip():
+            raise ValueError(f'objects.models[{index}].path must be a nonempty path')
+        if (not isinstance(angles, (list, tuple)) or len(angles) != 3
+                or any(type(v) not in (int, float) or not math.isfinite(v) for v in angles)):
+            raise ValueError(f'objects.models[{index}].rotation_deg must be finite [X, Y, Z] degrees')
+        resolved.append(ObjectModelCfg(resolve_path(path), tuple(map(float, angles))))
+    if any(Path(model.path).suffix.lower() != '.obj' for model in resolved):
+        raise ValueError('objects.models currently supports Wavefront .obj files')
+    return ObjectsCfg(enabled, count, resolved, parsed_area)
+
+
+def robot_pose(translation, rotation):
+    try:
+        position = tuple(map(float, translation.split()))
+        orient = tuple(map(float, rotation.split()))
+    except (ValueError, AttributeError) as exc:
+        raise ValueError('Robot transform/rotation must be numeric strings') from exc
+    if len(position) != 3 or len(orient) != 4 or not all(map(math.isfinite, position + orient)):
+        raise ValueError('Robot transform needs 3 and rotation 4 finite numbers')
+    length = math.sqrt(sum(v*v for v in orient[:3]))
+    if length == 0:
+        raise ValueError('Robot rotation axis must not be zero')
+    return position, tuple(v / length for v in orient[:3]) + (orient[3],)
+
+
+def world_position(local, translation, rotation):
+    position, orient = robot_pose(translation, rotation)
+    axis, angle = orient[:3], orient[3]
+    c, s = math.cos(angle), math.sin(angle)
+    dot = sum(a*b for a, b in zip(axis, local))
+    cross = (axis[1]*local[2] - axis[2]*local[1],
+             axis[2]*local[0] - axis[0]*local[2],
+             axis[0]*local[1] - axis[1]*local[0])
+    return tuple(position[i] + local[i]*c + cross[i]*s + axis[i]*dot*(1-c) for i in range(3))
+
+
+@dataclass(frozen=True)
+class ObjAsset:
+    path: Path
+    offset: tuple
+    size: tuple
+    textures: tuple
+
+
+def _require_file(path):
+    if not path.is_file():
+        raise ValueError(f'Object asset file does not exist: {path}')
+
+
+def load_obj(path) -> ObjAsset:
+    """Keep original OBJ/MTL/UV data; derive a centred, bottom-aligned origin."""
+    path = Path(path).resolve()
+    _require_file(path)
+    low, high = [math.inf]*3, [-math.inf]*3
+    materials, textures = [], []
+    for line in path.read_text(encoding='utf-8').splitlines():
+        parts = line.split()
+        if not parts:
+            continue
+        if parts[0] == 'v':
+            vertex = tuple(map(float, parts[1:4]))
+            if len(vertex) != 3 or not all(map(math.isfinite, vertex)):
+                raise ValueError(f'Invalid vertex in {path}')
+            low = [min(a, b) for a, b in zip(low, vertex)]
+            high = [max(a, b) for a, b in zip(high, vertex)]
+        elif parts[0] == 'mtllib':
+            materials.extend(path.parent / p for p in shlex.split(line)[1:])
+    size = tuple(b-a for a, b in zip(low, high))
+    if any(not math.isfinite(v) or v <= 0 for v in size):
+        raise ValueError(f'OBJ must have finite, nonzero 3D bounds: {path}')
+    for material in materials:
+        _require_file(material)
+        for line in material.read_text(encoding='utf-8').splitlines():
+            parts = shlex.split(line, comments=True)
+            if parts and (parts[0].lower().startswith('map_') or parts[0].lower() in ('bump', 'disp', 'decal', 'norm')):
+                # Wavefront map options precede the final texture filename.
+                texture = material.parent / parts[-1]
+                _require_file(texture)
+                textures.append(texture)
+    offset = (-(low[0]+high[0])/2, -(low[1]+high[1])/2, -low[2])
+    return ObjAsset(path, offset, size, tuple(textures))
+
+
+def _vector(values):
+    return ' '.join(format(v, '.12g') for v in values)
+
+
+def model_rotation(rotation_deg):
+    """Fixed-axis XYZ degrees: Rz(z) @ Ry(y) @ Rx(x), relative to robot base."""
+    rotvec = Rotation.from_euler('xyz', rotation_deg, degrees=True).as_rotvec()
+    angle = math.sqrt(sum(v*v for v in rotvec))
+    if angle < 1e-12:
+        return (0.0, 0.0, 1.0, 0.0)
+    return tuple(float(v / angle) for v in rotvec) + (angle,)
+
+
+def build_objects(config, translation, rotation, rng=None):
+    if not config.enabled or config.count == 0:
+        return []
+    _, orient = robot_pose(translation, rotation)
+    # Validate every allowed asset before any service request is sent.
+    assets = [(load_obj(model.path), model.rotation_deg) for model in config.models]
+    rng = rng or random.Random()
+    nodes = []
+    for index in range(config.count):
+        asset, angles = rng.choice(assets)
+        model_orient = model_rotation(angles)
+        matrix = Rotation.from_euler('xyz', angles, degrees=True).as_matrix()
+        # Lift the rotated collision box above the spawn anchor. Keep the
+        # original oriented box, rather than inflating it into a world AABB.
+        half_height = sum(abs(matrix[2, i]) * asset.size[i] / 2 for i in range(3))
+        center = (-asset.offset[0], -asset.offset[1],
+                  -asset.offset[2] + asset.size[2] / 2)
+        rotated_center = matrix @ center
+        visual_offset = tuple(-rotated_center[i] + (half_height if i == 2 else 0)
+                              for i in range(3))
+        local = tuple(rng.uniform(*config.area[axis]) for axis in 'xyz')
+        world = world_position(local, translation, rotation)
+        nodes.append(f'''Solid {{
+  name "spawned_object_{index:04d}"
+  translation {_vector(world)}
+  rotation {_vector(orient)}
+  children [ Pose {{
+    translation {_vector(visual_offset)}
+    rotation {_vector(model_orient)}
+    children [ CadShape {{ url [ {json.dumps(str(asset.path))} ] }} ]
+  }} ]
+  boundingObject Pose {{
+    translation 0 0 {half_height:.12g}
+    rotation {_vector(model_orient)}
+    children [ Box {{ size {_vector(asset.size)} }} ]
+  }}
+  physics Physics {{ density -1 mass 0.5 }}
+}}''')
+    return nodes

src/iiwa_utils/iiwa_utils/object_spawner.py

@@ -1,88 +1,50 @@
-# /Ros2Supervisor/spawn_node_from_string / webots_ros2_msgs/srv/SpawnNodeFromString
+"""Spawn the configured batch once through the Webots supervisor service."""
-# /Ros2Supervisor/spawn_urdf_robot
+import json
-
-
-from random import randint
 
 import rclpy
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
-from rclpy.task import Future
 from webots_ros2_msgs.srv import SpawnNodeFromString
 
+from iiwa_utils.object_scene import build_objects, parse_objects
 
-class ObjectSpawner(Node):
-    def __init__(self):
-        super().__init__("object_spawner")
 
-        # параметры передаваемые
+def spawn_objects(node):
-        self.declare_parameter('objects_path', '')
+    raw = json.loads(node.declare_parameter('objects_config', '{}').value)
-
+    translation = node.declare_parameter('robot_translation', '0 0 0').value
-        # переменные
+    rotation = node.declare_parameter('robot_rotation', '0 0 1 0').value
-        self._object_count: int = randint(1, 5)
+    config = parse_objects(raw, lambda path: path)
-        self._spawned_count: int = 0
+    objects = build_objects(config, translation, rotation)
-        self._call_in_progress: bool = False
+    if not objects:
-
+        node.get_logger().info('Object spawning disabled or count is zero')
-        # Реализация
-        self.cli = self.create_client(
-            SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
-        )
-
-        while not self.cli.wait_for_service(timeout_sec=10):
-            self.get_logger().warning(
-                "service /Ros2Supervisor/spawn_node_from_string  not available, waiting again..."
-            )
-
-        self._timer = self.create_timer(0.1, self.timer_callback)
-
-    # TODO: переделать процесс спавна, должен сформировать все proto файлы, после чего заспавнить все объекты
-    def timer_callback(self):
-        # TODO: здесь должен получать объекты
-        if self._spawned_count >= self._object_count:
-            self.get_logger().info(f"All {self._object_count} objects spawned")
-            self._timer.cancel()
         return
-
+    client = node.create_client(SpawnNodeFromString, '/Ros2Supervisor/spawn_node_from_string')
-        if self._call_in_progress:
+    if not client.wait_for_service(timeout_sec=60.0):
-            return
+        raise RuntimeError('Webots supervisor unavailable; no objects spawned')
-
+    for index, data in enumerate(objects):
-        data = 'Solid { name "test_box2" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }'
+        future = client.call_async(SpawnNodeFromString.Request(data=data))
-        data = self._create_group(children=[data])
+        rclpy.spin_until_future_complete(node, future, timeout_sec=30.0)
-
+        if not future.done():
-        req = SpawnNodeFromString.Request(data=data, check_fields=True)
+            # A timeout does not prove rejection: retrying could create duplicates.
-        self.get_logger().info(
+            raise RuntimeError(f'Object {index + 1}: spawn timed out; result unknown, no retry')
-            f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}"
-        )
-        self._call_in_progress = True
-        future = self.cli.call_async(req)
-        future.add_done_callback(self._response_callback)
-
-    def _response_callback(self, future: Future):
-        try:
         response = future.result()
-            self.get_logger().info(f"Spawn service response: {response}")
+        if response is None or not response.success:
-        except Exception as e:
+            raise RuntimeError(f'Object {index + 1} rejected; {index}/{len(objects)} spawned')
-            self.get_logger().error(f"Service call field: {e}")
+    node.get_logger().info(f'All {len(objects)} objects spawned')
-        finally:
-            self._call_in_progress = False
-            self._spawned_count += 1
-
-    # Вспомогательные методы формирования кода для спавна объектов
-    def _create_group(self, children: list[str]) -> str:
-        return f"DEF OBJECTS Group {{children [{" ".join(children)}]}}"
 
 
 def main(args=None):
-    try:
     rclpy.init(args=args)
-        node = ObjectSpawner()
+    node = Node('object_spawner')
-        rclpy.spin(node=node)
+    try:
+        spawn_objects(node)
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
         node.destroy_node()
+        if rclpy.ok():
             rclpy.shutdown()
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -1,11 +1,13 @@
 import math
 import os
-from dataclasses import dataclass, fields, is_dataclass
+from dataclasses import dataclass, field, fields, is_dataclass
 from typing import Any, Dict, List, Optional, Type, TypeVar
 
 import yaml
 from ament_index_python.packages import get_package_share_directory
 
+from iiwa_utils.object_scene import ObjectsCfg, load_obj, parse_objects, robot_pose
+
 T = TypeVar("T")
 
 
@@ -26,6 +28,7 @@ class WebotsCfg:
     rotation: str
     controller_timer: str
     cameras: List[str]
+    objects: ObjectsCfg = field(default_factory=ObjectsCfg)
 
 
 @dataclass(frozen=True)
@@ -339,7 +342,14 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         rotation=str(require(webots_raw, "rotation")),
         controller_timer=str(int(require(webots_raw, "controller_timer"))),
         cameras=cameras,
+        objects=parse_objects(webots_raw.get("objects"),
+                              lambda p: resolve_path(p, settings_dir)),
     )
+    if webots.objects.enabled and webots.objects.count:
+        robot_pose(webots.transform, webots.rotation)
+        if check_files:
+            for model in webots.objects.models:
+                load_obj(model.path)
     rviz = RvizCfg(
         config=resolve_path(str(require(rviz_raw, "config")), settings_dir)
     )

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_object_spawner_service.py

@@ -0,0 +1,30 @@
+"""Exercise our service requests with the installed Jazzy message schema."""
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+
+from iiwa_utils import object_spawner
+
+
+@pytest.mark.parametrize('success,completed', [(True, True), (False, True), (True, False)])
+def test_service_schema_and_failure_stop(monkeypatch, success, completed):
+    node = Mock()
+    node.declare_parameter.side_effect = [SimpleNamespace(value='{}'),
+                                         SimpleNamespace(value='0 0 0'),
+                                         SimpleNamespace(value='0 0 1 0')]
+    monkeypatch.setattr(object_spawner, 'build_objects', lambda *args: ['Solid {}', 'Solid {}'])
+    monkeypatch.setattr(object_spawner.rclpy, 'spin_until_future_complete', lambda *a, **kw: None)
+    client = node.create_client.return_value
+    client.wait_for_service.return_value = True
+    future = client.call_async.return_value
+    future.done.return_value = completed
+    future.result.return_value = SimpleNamespace(success=success)
+    if success and completed:
+        object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 2
+        assert client.call_async.call_args.args[0].data == 'Solid {}'
+    else:
+        with pytest.raises(RuntimeError):
+            object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 1

src/iiwa_utils/test/test_object_spawning.py

@@ -0,0 +1,121 @@
+"""Configuration, model placement and textured asset regressions."""
+import math
+import random
+from pathlib import Path
+
+import pytest
+
+from iiwa_utils import object_scene as scene
+
+
+@pytest.fixture
+def model(tmp_path):
+    (tmp_path / 'texture.png').write_bytes(b'texture')
+    (tmp_path / 'hammer.mtl').write_text('newmtl wood\nmap_Kd texture.png\n')
+    path = tmp_path / 'hammer.obj'
+    path.write_text('mtllib hammer.mtl\nv 10 20 30\nv 12 24 36\nf 1 2 1\n')
+    return path
+
+
+def config(model, **updates):
+    raw = dict(enabled=True, count=5, models=[str(model)],
+               area=dict(x=[1, 1], y=[2, 2], z=[3, 3]))
+    raw.update(updates)
+    return scene.parse_objects(raw, lambda p: p)
+
+
+def test_default_disabled():
+    assert not scene.parse_objects(None, lambda p: p).enabled
+
+
+@pytest.mark.parametrize('updates', [
+    {'count': -1}, {'count': True}, {'count': 1.5}, {'enabled': 'true'},
+    {'models': []}, {'models': 'hammer.obj'},
+    {'area': {'x': [2, 1], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [0, math.nan], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [False, 1], 'y': [0, 1], 'z': [0, 1]}},
+])
+def test_reject_bad_configuration(model, updates):
+    with pytest.raises(ValueError):
+        config(model, **updates)
+
+
+def test_rotation_about_translated_robot():
+    assert scene.world_position((1, 2, 3), '10 20 30', f'0 0 2 {math.pi/2}') == pytest.approx((8, 21, 33))
+    assert scene.world_position((0, 1, 0), '0 0 0', f'1 0 0 {math.pi/2}') == pytest.approx((0, 0, 1))
+
+
+@pytest.mark.parametrize('translation,rotation', [('0 0', '0 0 1 0'), ('0 nan 0', '0 0 1 0'), ('0 0 0', '0 0 0 1')])
+def test_bad_robot_pose(translation, rotation):
+    with pytest.raises(ValueError):
+        scene.world_position((0, 0, 0), translation, rotation)
+
+
+def test_exact_count_repeats_and_centered_geometry(model):
+    nodes = scene.build_objects(config(model), '10 20 30', '0 0 1 0', random.Random(1))
+    assert len(nodes) == 5
+    assert len(set(nodes)) == 5
+    for node in nodes:
+        assert 'translation 11 22 33' in node
+        assert 'CadShape' in node and str(model) in node
+        assert 'translation -11 -22 -30' in node  # XY centered, bottom at local z=0
+        assert 'size 2 4 6' in node
+    assert scene.build_objects(config(model, count=0), '0 0 0', '0 0 1 0') == []
+
+
+def test_missing_texture_fails_before_spawning(model):
+    (model.parent / 'texture.png').unlink()
+    with pytest.raises(ValueError, match='texture'):
+        scene.build_objects(config(model), '0 0 0', '0 0 1 0')
+
+
+def test_repository_models_have_materials_and_textures():
+    root = Path(__file__).resolve().parents[2] / 'iiwa_description/objects/hammer'
+    models = sorted(root.glob('*.obj'))
+    assert len(models) == 5
+    for path in models:
+        asset = scene.load_obj(path)
+        assert all(size > 0 for size in asset.size)
+        assert asset.textures
+
+
+def test_per_model_rotation_and_serialization(model):
+    from dataclasses import asdict
+    cfg = config(model, models=[str(model), {'path': str(model), 'rotation_deg': [90, 0, 180]}])
+    assert cfg.models[0].rotation_deg == (0.0, 0.0, 0.0)
+    assert cfg.models[1].rotation_deg == (90.0, 0.0, 180.0)
+    assert scene.parse_objects(asdict(cfg), lambda p: p) == cfg
+
+
+@pytest.mark.parametrize('entry', [
+    {}, {'path': 12}, {'path': 'a.obj', 'rotation_deg': [0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [True, 0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [0, float('inf'), 0]},
+    {'path': 'a.obj', 'rotation_deg': '90 0 0'},
+    {'path': 'a.obj', 'rotaton_deg': [90, 0, 0]},
+])
+def test_invalid_model_orientation(model, entry):
+    with pytest.raises(ValueError):
+        config(model, models=[entry])
+
+
+def test_quarter_turn_repositions_geometry_and_collision(model):
+    cfg = config(model, count=1, models=[{'path': str(model), 'rotation_deg': [90, 0, 0]}])
+    node = scene.build_objects(cfg, '10 20 30', f'0 0 1 {math.pi/2}')[0]
+    # Anchor (1,2,3) rotates about the ROBOT, not about the model's X axis.
+    assert 'translation 8 21 33' in node
+    # Raw centre (11,22,33) -> (11,-33,22); rotated half-height = 2.
+    assert 'translation -11 33 -20' in node
+    assert 'translation 0 0 2' in node
+    # Rendering AND the original 2x4x6 collision box receive the model rotation.
+    assert node.count('rotation 1 0 0 1.57079632679') == 2
+    assert 'size 2 4 6' in node
+
+
+def test_rotation_order_xyz():
+    # Apply Rx(90), then Ry(0), then Rz(90): x -> y, y -> z, z -> x.
+    orient = scene.model_rotation([90, 0, 90])
+    encoded = ' '.join(map(str, orient))
+    assert scene.world_position((1, 0, 0), '0 0 0', encoded) == pytest.approx((0, 1, 0))
+    assert scene.world_position((0, 1, 0), '0 0 0', encoded) == pytest.approx((0, 0, 1))
+    assert scene.world_position((0, 0, 1), '0 0 0', encoded) == pytest.approx((1, 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()