Add camera spawning functionality and update configuration files

src/iiwa_bringup/launch/iiwa.launch.py

@@ -1,3 +1,4 @@
+import json
 from dataclasses import asdict
 
 from launch import LaunchDescription
@@ -16,8 +17,10 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from moveit_configs_utils import MoveItConfigsBuilder
+from webots_ros2_driver.webots_controller import WebotsController
 
 from iiwa_utils import converter, setting_loader
+from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type: ignore
 
 
 def _foxglove_params(fg, use_sim_time: bool) -> dict:
@@ -112,7 +115,7 @@ def _runtime_setup(context, *args, **kwargs):
         )
 
         setup += [webots_launch]
-    
+
     # Controller launch
     if simulate:
         controller_args = {
@@ -126,38 +129,34 @@ def _runtime_setup(context, *args, **kwargs):
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
         }
 
-        # TODO пример работы, необходимо правильно интегрировать в структуру проекта и удалить из-за избыточности
+        if settings.digital_twin.webots.cameras:
+            # Спавн камеры
+            camera_spawner_node = Node(
+                package="iiwa_utils",
+                executable="camera_spawner",
+                name="camera_spawner",
+                output="screen",
+                parameters=[{
+                    "camera_configs": json.dumps(settings.digital_twin.webots.cameras)
+                }],
+            )
+            setup.append(camera_spawner_node)
 
-#         example_camera_urdf = """<?xml version="1.0"?>
+            # WebotsController для каждой камеры
-# <robot name="example_camera_robot">
+            for cam_path in settings.digital_twin.webots.cameras:
-#     <link name="example_camera_link"/>
+                cam_cfg = load_camera_config(cam_path)
-#     <webots>
+                urdf = build_ros_urdf(cam_cfg)
-#         <device reference="example_camera" type="Camera">
-#             <ros>
-#                 <topicName>/example_camera/image_raw</topicName>
-#                 <updateRate>30</updateRate>
-#                 <alwaysOn>True</alwaysOn>
-#                 <frameName>example_camera_link</frameName>
-#             </ros>
-#         </device>
-#     </webots>
-# </robot>
-# """
 
-#         from webots_ros2_driver.webots_controller import WebotsController
+                camera_controller = WebotsController(
-
+                    robot_name=f"{cam_cfg.name}_robot",
-#         example_camera_driver = WebotsController(
+                    parameters=[{
-#         robot_name="example_camera_robot",
+                        "robot_description": urdf,
-#         parameters=[
+                        "use_sim_time": True,
-#             {
+                        "set_robot_state_publisher": False,
-#                 "robot_description": example_camera_urdf,
+                    }],
-#                 "use_sim_time": True,
+                    respawn=True,
-#                 "set_robot_state_publisher": False,
+                )
-#             }
+                setup.append(camera_controller)
-#         ],
-#         respawn=True,
-#     )  
-#         setup += [example_camera_driver]
 
     else:
         controller_args = {

src/iiwa_config/config/cameras/d455_top.yaml

@@ -1,4 +1,52 @@
-# Пример использования параметров камеры
+# Имя камеры - используется как:
-name: D455_TOP
+#   - имя Robot-ноды в Webots: "{name}_robot"
-translation: "-0.25 0 0.79"
+#   - имя Camera-девайса: "{name}"
-rotation: "0 0 1 0"
+#   - имя RangeFinder-девайса: "{name}_depth"
+#   - префикс ROS2-топиков
+
+name: d455_top
+# Положение в сцене (x y z)
+translation: "-0.25 0 1.5"
+# Ориентация (ось_x ось_y ось_z угол_рад)
+rotation: "0 0 1 0"
+
+# Блок camera
+# Если блок отсутствует — Camera-девайс не создаётся
+# Поля соответствуют документации Webots:
+# https://cyberbotics.com/doc/reference/camera
+camera:
+  width: 640
+  height: 480
+  fieldOfView: 1.047        # ~60° в радианах
+  near: 0.1
+  far: 0.0                  # 0 = без ограничений
+  exposure: 1.0
+  antiAliasing: false
+  ambientOcclusionRadius: 0.0
+  bloomThreshold: -1.0      # -1 = отключён
+  noise: 0.0
+  motionBlur: 0.0
+
+  # Параметры публикации в ROS2 (не часть Webots-ноды, обрабатываются отдельно)
+  ros:
+    topic: /d455_top/image_raw
+    update_rate: 30
+
+
+# Блок range_finder
+# Если блок отсутствует - RangeFinder-девайс не создаётся
+# Поля соответствуют документации Webots:
+# https://cyberbotics.com/doc/reference/rangefinder
+# range_finder:
+#   width: 640
+#   height: 480
+#   fieldOfView: 1.047
+#   minRange: 0.1
+#   maxRange: 5.0
+#   resolution: -1.0          # -1 = без ограничений по дискретизации
+#   noise: 0.0
+
+#   # Параметры публикации в ROS2
+#   ros:
+#     topic: /d455_top/depth/image_raw
+#     update_rate: 15

src/iiwa_config/config/setting.yaml

@@ -14,6 +14,9 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    cameras:
+      - pkg://iiwa_config/config/cameras/d455_top.yaml
+
   rviz:
     config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz
 

src/iiwa_description/worlds/iiwa_world.sdf

@@ -1,106 +0,0 @@
-<sdf version='1.10'>
-  <world name='empty'>
-    <physics name='1ms' type='ignored'>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <real_time_update_rate>1000</real_time_update_rate>
-    </physics>
-    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
-    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
-    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
-    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
-
-    <plugin filename="WorldControl" name="World control">
-      <gz-gui>
-          <title>Kuka iiwa world</title>
-          <property type="string" key="state">floating</property>
-          <anchors target="3D View">
-          <line own="left" target="left"/>
-          <line own="bottom" target="bottom"/>
-          </anchors>
-      </gz-gui>
-
-      <play_pause>true</play_pause>
-      <step>false</step>
-      <start_paused>false</start_paused>
-    </plugin>
-
-    <gravity>0 0 -9.8000000000000007</gravity>
-    <magnetic_field>5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05</magnetic_field>
-    <atmosphere type='adiabatic'/>
-    <scene>
-      <ambient>0.400000006 0.400000006 0.400000006 1</ambient>
-      <background>0.699999988 0.699999988 0.699999988 1</background>
-      <shadows>true</shadows>
-    </scene>
-    <model name='ground_plane'>
-      <static>true</static>
-      <link name='link'>
-        <collision name='collision'>
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <ode/>
-            </friction>
-            <bounce/>
-            <contact/>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.800000012 0.800000012 0.800000012 1</ambient>
-            <diffuse>0.800000012 0.800000012 0.800000012 1</diffuse>
-            <specular>0.800000012 0.800000012 0.800000012 1</specular>
-          </material>
-        </visual>
-        <pose>0 0 0 0 0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>1</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>1</iyy>
-            <iyz>0</iyz>
-            <izz>1</izz>
-          </inertia>
-        </inertial>
-        <enable_wind>false</enable_wind>
-      </link>
-      <pose>0 0 0 0 0 0</pose>
-      <self_collide>false</self_collide>
-    </model>
-    
-    <light name='sun' type='directional'>
-      <pose>0 0 10 0 0 0</pose>
-      <cast_shadows>true</cast_shadows>
-      <intensity>1</intensity>
-      <direction>-0.5 0.10000000000000001 -0.90000000000000002</direction>
-      <diffuse>0.800000012 0.800000012 0.800000012 1</diffuse>
-      <specular>0.200000003 0.200000003 0.200000003 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <linear>0.01</linear>
-        <constant>0.90000000000000002</constant>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <spot>
-        <inner_angle>0</inner_angle>
-        <outer_angle>0</outer_angle>
-        <falloff>0</falloff>
-      </spot>
-    </light>
-  </world>
-</sdf>

src/iiwa_description/worlds/simple_world.wbt

@@ -17,14 +17,3 @@ TexturedBackgroundLight {
 RectangleArena {
   floorSize 5 5
 }
-
-Robot {
-  translation -1.26 0 0.77
-  children [
-    Camera {
-      name "example_camera"
-    }
-  ]
-  name "example_camera_robot"
-  controller "<extern>"
-}

src/iiwa_utils/iiwa_utils/camera_spawner.py

@@ -1,56 +1,306 @@
-from dataclasses import dataclass
+"""
+Документация Webots:
+  Camera:      https://cyberbotics.com/doc/reference/camera
+  RangeFinder: https://cyberbotics.com/doc/reference/rangefinder
+"""
 
+import json
+import os
+from dataclasses import dataclass
+from typing import Any, Dict, List, Optional
+
+import yaml
 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 rclpy.executors import ExternalShutdownException
+
+
+_SKIP_FIELDS = {"ros"}
+_CAMERA_BOOL_FIELDS = {"antiAliasing"}
+_RANGE_FINDER_BOOL_FIELDS: set = set()
 
 
 @dataclass(frozen=True)
-class CameraSpawnParams:
+class RosCfg:
-    camera_name: str
+    topic: str
+    update_rate: int
+
+
+@dataclass(frozen=True)
+class CameraDeviceCfg:
+    webots_fields: Dict[str, Any]
+    ros: RosCfg
+
+
+@dataclass(frozen=True)
+class RangeFinderDeviceCfg:
+    webots_fields: Dict[str, Any]
+    ros: RosCfg
+
+
+@dataclass(frozen=True)
+class CameraConfig:
+    name: str
     translation: str
     rotation: str
-    
+    camera: Optional[CameraDeviceCfg]
+    range_finder: Optional[RangeFinderDeviceCfg]
 
 
+def _parse_device_block(
+    raw: Optional[Dict[str, Any]],
+    bool_fields: set,
+    default_topic: str,
+    default_rate: int,
+) -> Optional[Dict[str, Any]]:
+    """
+    Парсит блок camera или range_finder из YAML.
+    Возвращает None если блок отсутствует.
+    """
+    if raw is None:
+        return None
+
+    ros_raw = raw.get("ros", {})
+    ros = RosCfg(
+        topic=str(ros_raw.get("topic", default_topic)),
+        update_rate=int(ros_raw.get("update_rate", default_rate)),
+    )
+
+    webots_fields: Dict[str, Any] = {}
+    for key, value in raw.items():
+        if key in _SKIP_FIELDS:
+            continue
+        if key in bool_fields:
+            webots_fields[key] = "TRUE" if value else "FALSE"
+        else:
+            webots_fields[key] = value
+
+    return {"webots_fields": webots_fields, "ros": ros}
+
+
+def load_camera_config(path: str) -> CameraConfig:
+    """Загружает camera YAML и возвращает CameraConfig."""
+    path = os.path.abspath(path)
+    if not os.path.exists(path):
+        raise FileNotFoundError(f"Camera config not found: {path}")
+
+    with open(path, "r", encoding="utf-8") as f:
+        raw: Dict[str, Any] = yaml.safe_load(f) or {}
+
+    name = str(raw["name"])
+    translation = str(raw["translation"])
+    rotation = str(raw["rotation"])
+
+    cam_raw = _parse_device_block(
+        raw.get("camera"),
+        _CAMERA_BOOL_FIELDS,
+        default_topic=f"/{name}/image_raw",
+        default_rate=30,
+    )
+    rf_raw = _parse_device_block(
+        raw.get("range_finder"),
+        _RANGE_FINDER_BOOL_FIELDS,
+        default_topic=f"/{name}/depth/image_raw",
+        default_rate=15,
+    )
+
+    camera = (
+        CameraDeviceCfg(webots_fields=cam_raw["webots_fields"], ros=cam_raw["ros"])
+        if cam_raw is not None else None
+    )
+    range_finder = (
+        RangeFinderDeviceCfg(webots_fields=rf_raw["webots_fields"], ros=rf_raw["ros"])
+        if rf_raw is not None else None
+    )
+
+    return CameraConfig(
+        name=name,
+        translation=translation,
+        rotation=rotation,
+        camera=camera,
+        range_finder=range_finder,
+    )
+
+
+
+def _build_device_proto(device_type: str, device_name: str, fields: Dict[str, Any]) -> str:
+    """Строит PROTO-строку для Camera или RangeFinder."""
+    parts = [f'name "{device_name}"']
+    for key, value in fields.items():
+        if isinstance(value, str) and value not in ("TRUE", "FALSE"):
+            parts.append(f'{key} "{value}"')
+        else:
+            parts.append(f'{key} {value}')
+    return f'{device_type} {{ {" ".join(parts)} }}'
+
+
+def build_robot_proto(cfg: CameraConfig) -> str:
+    """
+    Строит Webots PROTO-строку Robot-ноды, содержащей Camera и/или RangeFinder.
+    Результат передаётся в SpawnNodeFromString.Request.data.
+    """
+    children: List[str] = []
+
+    if cfg.camera is not None:
+        children.append(
+            _build_device_proto("Camera", cfg.name, cfg.camera.webots_fields)
+        )
+
+    if cfg.range_finder is not None:
+        children.append(
+            _build_device_proto("RangeFinder", f"{cfg.name}_depth", cfg.range_finder.webots_fields)
+        )
+
+    children_str = " ".join(children)
+
+    return (
+        f'Robot {{'
+        f' name "{cfg.name}_robot"'
+        f' translation {cfg.translation}'
+        f' rotation {cfg.rotation}'
+        f' children [ {children_str} ]'
+        f' controller "<extern>"'
+        f' }}'
+    )
+
+
+def build_ros_urdf(cfg: CameraConfig) -> str:
+    """
+    Строит URDF-строку для WebotsController.
+    Описывает ROS2-интерфейс Camera и/или RangeFinder.
+    """
+    devices: List[str] = []
+
+    if cfg.camera is not None:
+        ros = cfg.camera.ros
+        devices.append(
+            f'    <device reference="{cfg.name}" type="Camera">\n'
+            f'      <ros>\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>'
+        )
+
+    if cfg.range_finder is not None:
+        ros = cfg.range_finder.ros
+        devices.append(
+            f'    <device reference="{cfg.name}_depth" type="RangeFinder">\n'
+            f'      <ros>\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>'
+        )
+
+    devices_str = "\n".join(devices)
+
+    return (
+        f'<?xml version="1.0"?>\n'
+        f'<robot name="{cfg.name}_robot">\n'
+        f'  <link name="{cfg.name}_link"/>\n'
+        f'  <webots>\n'
+        f'{devices_str}\n'
+        f'  </webots>\n'
+        f'</robot>'
+    )
+
 
 class CameraSpawner(Node):
+    """
+    Спавнит камеры в Webots через /Ros2Supervisor/spawn_node_from_string.
 
-    def __init__(self, camera_params: CameraSpawnParams):
+    Параметры ROS2 ноды:
+        camera_configs (string): JSON-массив абсолютных путей до camera YAML файлов.
+    """
+
+    def __init__(self):
         super().__init__("camera_spawner")
 
-        self._camera_params = camera_params
+        self.declare_parameter("camera_configs", "[]")
-        self._urdf_template = self._generate_camera_urdf()
+        configs_json = (
-        self._proto_template = self._generate_camera_proto()
+            self.get_parameter("camera_configs")
+            .get_parameter_value()
+            .string_value
+        )
+        config_paths: List[str] = json.loads(configs_json)
+
+        self._pending: List[CameraConfig] = []
+        for path in config_paths:
+            try:
+                cfg = load_camera_config(path)
+                self._pending.append(cfg)
+                self.get_logger().info(f"Loaded camera config: '{cfg.name}' from {path}")
+            except Exception as e:
+                self.get_logger().error(f"Failed to load camera config '{path}': {e}")
+
+        if not self._pending:
+            self.get_logger().info("No cameras to spawn.")
+            return
+
+        self._in_flight = False
+
+        self._client = self.create_client(
+            SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
+        )
+
+        self.get_logger().info("Waiting for /Ros2Supervisor/spawn_node_from_string...")
+        while not self._client.wait_for_service(timeout_sec=10.0):
+            self.get_logger().warning(
+                "Service /Ros2Supervisor/spawn_node_from_string not available, retrying..."
+            )
+
+        self._timer = self.create_timer(0.1, self._tick)
+
+    def _tick(self):
+        if not self._pending:
+            self.get_logger().info("All cameras spawned.")
+            self._timer.cancel()
+            return
+
+        if self._in_flight:
+            return
+
+        cfg = self._pending[0]
+        proto = build_robot_proto(cfg)
+
+        self.get_logger().info(f"Spawning camera '{cfg.name}'...")
+        self.get_logger().debug(f"Proto:\n{proto}")
+
+        req = SpawnNodeFromString.Request(data=proto, check_fields=True)
+        self._in_flight = True
+        future = self._client.call_async(req)
+        future.add_done_callback(lambda f: self._on_spawned(f, cfg))
+
+    def _on_spawned(self, future: Future, cfg: CameraConfig):
+        try:
+            response = future.result()
+            self.get_logger().info(f"Camera '{cfg.name}' spawned: {response}")
+        except Exception as e:
+            self.get_logger().error(f"Failed to spawn camera '{cfg.name}': {e}")
+        finally:
+            self._pending.pop(0)
+            self._in_flight = False
 
 
 
-        # self.cli = self.create_client(
+def main(args=None):
-        #     SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
+    try:
-        # )
+        rclpy.init(args=args)
+        node = CameraSpawner()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
 
-        # while not self.cli.wait_for_service(timeout_sec=10):
-        #     self.get_logger().warning(
-        #         "service /Ros2Supervisor/spawn_node_from_string  not available, waiting again..."
-        #     )
 
-        # data = 'Camera { name "camera" translation 0 0 1.5 rotation 1 0 0 -1.5708 }'
+if __name__ == "__main__":
-        # req = SpawnNodeFromString.Request(data=data, check_fields=True)
+    main()
-        # self.get_logger().info(f"Calling spawn service for camera")
-        # 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("Camera spawned successfully")
-    #     except Exception as e:
-    #         self.get_logger().error(f"Service call failed: {e}")
-
-    def _generate_camera_proto(self) -> str:
-        ...
-
-    def _generate_camera_urdf(self) -> str:
-        ...

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -23,6 +23,7 @@ class WebotsCfg:
     transform: str
     rotation: str
     controller_timer: str
+    cameras: List[str]
 
 
 @dataclass(frozen=True)
@@ -240,13 +241,17 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
     # digital_twin
     dt_raw = require(raw, "digital_twin")
     webots_raw = require(dt_raw, "webots")
+    cameras_raw = webots_raw.get("cameras", [])
     rviz_raw = require(dt_raw, "rviz")
 
+    cameras = [resolve_path(str(c), settings_dir) for c in cameras_raw]
+
     webots = WebotsCfg(
         world=resolve_path(str(require(webots_raw, "world")), settings_dir),
         transform=str(require(webots_raw, "transform")),
         rotation=str(require(webots_raw, "rotation")),
         controller_timer=str(int(require(webots_raw, "controller_timer"))),
+        cameras=cameras,
     )
     rviz = RvizCfg(
         config=resolve_path(str(require(rviz_raw, "config")), settings_dir)
@@ -282,6 +287,9 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
     )
 
     if check_files:
+        for i, cam_path in enumerate(s.digital_twin.webots.cameras):
+            assert_file(cam_path, f"digital_twin.webots.cameras[{i}]")
+
         assert_file(s.robot.description, "robot.description")
         assert_file(s.digital_twin.webots.world, "digital_twin.webots.world")
         assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config")

src/iiwa_utils/setup.py

@@ -19,8 +19,9 @@ setup(
     license='Apache-2.0',
     entry_points={
         'console_scripts': [
-            'object_spawner = iiwa_utils.object_spawner:main',
+            "object_spawner = iiwa_utils.object_spawner:main",
-            "motion_planning_test = iiwa_utils.motion_planing_test:main"
+            "motion_planning_test = iiwa_utils.motion_planing_test:main",
+            "camera_spawner = iiwa_utils.camera_spawner:main",
         ],
     },
 )