diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 050767d..82cc42d 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/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 = """ -# -# -# -# -# -# /example_camera/image_raw -# 30 -# True -# example_camera_link -# -# -# -# -# """ + # WebotsController для каждой камеры + for cam_path in settings.digital_twin.webots.cameras: + cam_cfg = load_camera_config(cam_path) + urdf = build_ros_urdf(cam_cfg) -# from webots_ros2_driver.webots_controller import WebotsController - -# example_camera_driver = WebotsController( -# robot_name="example_camera_robot", -# parameters=[ -# { -# "robot_description": example_camera_urdf, -# "use_sim_time": True, -# "set_robot_state_publisher": False, -# } -# ], -# respawn=True, -# ) -# setup += [example_camera_driver] + camera_controller = WebotsController( + robot_name=f"{cam_cfg.name}_robot", + parameters=[{ + "robot_description": urdf, + "use_sim_time": True, + "set_robot_state_publisher": False, + }], + respawn=True, + ) + setup.append(camera_controller) else: controller_args = { diff --git a/src/iiwa_config/config/cameras/d455_top.yaml b/src/iiwa_config/config/cameras/d455_top.yaml index 323005b..86c76c4 100644 --- a/src/iiwa_config/config/cameras/d455_top.yaml +++ b/src/iiwa_config/config/cameras/d455_top.yaml @@ -1,4 +1,52 @@ -# Пример использования параметров камеры -name: D455_TOP -translation: "-0.25 0 0.79" -rotation: "0 0 1 0" \ No newline at end of file +# Имя камеры - используется как: +# - имя Robot-ноды в Webots: "{name}_robot" +# - имя Camera-девайса: "{name}" +# - имя 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 \ No newline at end of file diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 83ed47f..6fd4037 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 diff --git a/src/iiwa_description/worlds/iiwa_world.sdf b/src/iiwa_description/worlds/iiwa_world.sdf deleted file mode 100644 index c8d3b70..0000000 --- a/src/iiwa_description/worlds/iiwa_world.sdf +++ /dev/null @@ -1,106 +0,0 @@ - - - - 0.001 - 1 - 1000 - - - - - - - - - Kuka iiwa world - floating - - - - - - - true - false - false - - - 0 0 -9.8000000000000007 - 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 - - - 0.400000006 0.400000006 0.400000006 1 - 0.699999988 0.699999988 0.699999988 1 - true - - - true - - - - - 0 0 1 - 100 100 - - - - - - - - - - - - - - 0 0 1 - 100 100 - - - - 0.800000012 0.800000012 0.800000012 1 - 0.800000012 0.800000012 0.800000012 1 - 0.800000012 0.800000012 0.800000012 1 - - - 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 1 - - 1 - 0 - 0 - 1 - 0 - 1 - - - false - - 0 0 0 0 0 0 - false - - - - 0 0 10 0 0 0 - true - 1 - -0.5 0.10000000000000001 -0.90000000000000002 - 0.800000012 0.800000012 0.800000012 1 - 0.200000003 0.200000003 0.200000003 1 - - 1000 - 0.01 - 0.90000000000000002 - 0.001 - - - 0 - 0 - 0 - - - - diff --git a/src/iiwa_description/worlds/simple_world.wbt b/src/iiwa_description/worlds/simple_world.wbt index 9bad8ce..a8c7737 100644 --- a/src/iiwa_description/worlds/simple_world.wbt +++ b/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 "" -} diff --git a/src/iiwa_utils/iiwa_utils/camera_spawner.py b/src/iiwa_utils/iiwa_utils/camera_spawner.py index 7340fe4..6b09c96 100644 --- a/src/iiwa_utils/iiwa_utils/camera_spawner.py +++ b/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: - camera_name: str +class RosCfg: + 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 ""' + 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' \n' + f' \n' + f' {ros.topic}\n' + f' {ros.update_rate}\n' + f' True\n' + f' {cfg.name}_link\n' + f' \n' + f' ' + ) + + if cfg.range_finder is not None: + ros = cfg.range_finder.ros + devices.append( + f' \n' + f' \n' + f' {ros.topic}\n' + f' {ros.update_rate}\n' + f' True\n' + f' {cfg.name}_link\n' + f' \n' + f' ' + ) + + devices_str = "\n".join(devices) + + return ( + f'\n' + f'\n' + f' \n' + f' \n' + f'{devices_str}\n' + f' \n' + f'' + ) + 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._urdf_template = self._generate_camera_urdf() - self._proto_template = self._generate_camera_proto() + self.declare_parameter("camera_configs", "[]") + configs_json = ( + 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( - # SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string" - # ) +def main(args=None): + 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 }' - # req = SpawnNodeFromString.Request(data=data, check_fields=True) - # 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: - ... \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index e0cfd2d..0ed66fc 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/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") diff --git a/src/iiwa_utils/setup.py b/src/iiwa_utils/setup.py index d782b25..b799e4b 100644 --- a/src/iiwa_utils/setup.py +++ b/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', - "motion_planning_test = iiwa_utils.motion_planing_test:main" + "object_spawner = iiwa_utils.object_spawner:main", + "motion_planning_test = iiwa_utils.motion_planing_test:main", + "camera_spawner = iiwa_utils.camera_spawner:main", ], }, )