From cd450a3476e439e4349ae93ceb1e4647151bff58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Mon, 13 Apr 2026 17:37:24 +0300 Subject: [PATCH] Add camera spawning functionality and update world configuration --- src/iiwa_bringup/launch/iiwa.launch.py | 34 ++++++++++++ src/iiwa_config/config/cameras/d455_top.yaml | 4 ++ src/iiwa_config/config/gazebo/gz_bridge.yaml | 29 ---------- src/iiwa_config/config/setting.yaml | 3 +- src/iiwa_description/urdf/iiwa7.urdf.xacro | 9 ++++ src/iiwa_description/worlds/simple_world.wbt | 15 +++++- src/iiwa_utils/iiwa_utils/camera_spawner.py | 56 ++++++++++++++++++++ 7 files changed, 118 insertions(+), 32 deletions(-) create mode 100644 src/iiwa_config/config/cameras/d455_top.yaml delete mode 100644 src/iiwa_config/config/gazebo/gz_bridge.yaml create mode 100644 src/iiwa_utils/iiwa_utils/camera_spawner.py diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 7bbb205..050767d 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -125,6 +125,40 @@ def _runtime_setup(context, *args, **kwargs): "rotation": str(settings.digital_twin.webots.rotation), "controller_timer": str(settings.digital_twin.webots.controller_timer), } + + # TODO пример работы, необходимо правильно интегрировать в структуру проекта и удалить из-за избыточности + +# example_camera_urdf = """ +# +# +# +# +# +# /example_camera/image_raw +# 30 +# True +# example_camera_link +# +# +# +# +# """ + +# 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] + else: controller_args = { "robot_name": settings.robot.name, diff --git a/src/iiwa_config/config/cameras/d455_top.yaml b/src/iiwa_config/config/cameras/d455_top.yaml new file mode 100644 index 0000000..323005b --- /dev/null +++ b/src/iiwa_config/config/cameras/d455_top.yaml @@ -0,0 +1,4 @@ +# Пример использования параметров камеры +name: D455_TOP +translation: "-0.25 0 0.79" +rotation: "0 0 1 0" \ No newline at end of file diff --git a/src/iiwa_config/config/gazebo/gz_bridge.yaml b/src/iiwa_config/config/gazebo/gz_bridge.yaml deleted file mode 100644 index 4f23c22..0000000 --- a/src/iiwa_config/config/gazebo/gz_bridge.yaml +++ /dev/null @@ -1,29 +0,0 @@ -# Формат каждой записи: -# ros_topic_name: имя топика в ROS2 -# gz_topic_name: имя топика в Gazebo (можно опустить если совпадает) -# ros_type_name: тип сообщения ROS2 -# gz_type_name: тип сообщения Gazebo -# direction: GZ_TO_ROS / ROS_TO_GZ / BIDIRECTIONAL - -- ros_topic_name: /clock - gz_topic_name: /clock - ros_type_name: rosgraph_msgs/msg/Clock - gz_type_name: gz.msgs.Clock - direction: GZ_TO_ROS - lazy: false - -# Состояния джойнтов из Gazebo в ROS2 -- ros_topic_name: /joint_states - gz_topic_name: /world/empty/model/iiwa7/joint_state - ros_type_name: sensor_msgs/msg/JointState - gz_type_name: gz.msgs.Model - direction: GZ_TO_ROS - lazy: false - -# TF из Gazebo в ROS2 (поза модели в мире) -- ros_topic_name: /tf - gz_topic_name: /model/iiwa7/pose - ros_type_name: tf2_msgs/msg/TFMessage - gz_type_name: gz.msgs.Pose_V - direction: GZ_TO_ROS - lazy: false \ No newline at end of file diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 39ff7e8..83ed47f 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -8,7 +8,8 @@ robot: digital_twin: webots: - world: pkg://iiwa_description/worlds/iiwa.wbt + # world: pkg://iiwa_description/worlds/iiwa.wbt + world: pkg://iiwa_description/worlds/simple_world.wbt transform: "-0.25 0 0.79" rotation: "0 0 1 0" controller_timer: "50" diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 2b4df9c..cefddf7 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -32,6 +32,15 @@ + + + /camera_example/color + 30 + True + example_camera + + + /camera/depth diff --git a/src/iiwa_description/worlds/simple_world.wbt b/src/iiwa_description/worlds/simple_world.wbt index 2271a17..9bad8ce 100644 --- a/src/iiwa_description/worlds/simple_world.wbt +++ b/src/iiwa_description/worlds/simple_world.wbt @@ -7,8 +7,8 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/project WorldInfo { } Viewpoint { - orientation -0.20135401485081894 0.14705590432610455 0.9684168119700772 1.9104172436184537 - position 2.126114183750861 -6.172386590547907 2.2530769187603243 + orientation -0.28913430413481667 0.20676337530925004 0.9346926023047372 1.96317584288795 + position 2.0700764329273156 -5.621201880809646 3.39425616182994 } TexturedBackground { } @@ -17,3 +17,14 @@ 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 new file mode 100644 index 0000000..7340fe4 --- /dev/null +++ b/src/iiwa_utils/iiwa_utils/camera_spawner.py @@ -0,0 +1,56 @@ +from dataclasses import dataclass + +import rclpy +from rclpy.node import Node +from rclpy.task import Future +from webots_ros2_msgs.srv import SpawnNodeFromString +from rclpy.executors import ExternalShutdownException + + +@dataclass(frozen=True) +class CameraSpawnParams: + camera_name: str + translation: str + rotation: str + + + + +class CameraSpawner(Node): + + def __init__(self, camera_params: CameraSpawnParams): + super().__init__("camera_spawner") + + self._camera_params = camera_params + self._urdf_template = self._generate_camera_urdf() + self._proto_template = self._generate_camera_proto() + + + + # 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..." + # ) + + # 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