From 7714c71cd23ce3286d18ea397f15b9c756908b66 Mon Sep 17 00:00:00 2001
From: Junhan Chang <changjh@dp.tech>
Date: Sun, 29 Jun 2025 17:35:32 +0800
Subject: [PATCH] add camera and dependency (#56)

---
 test/experiments/camera.json          | 45 ++++++++++++++++++++
 unilabos-linux-64.yaml                |  3 +-
 unilabos-osx-arm64.yaml               |  3 +-
 unilabos-win64.yaml                   |  3 +-
 unilabos/registry/devices/camera.yaml | 10 +++++
 unilabos/ros/nodes/presets/camera.py  | 61 +++++++++++++++++++++++++++
 6 files changed, 122 insertions(+), 3 deletions(-)
 create mode 100644 test/experiments/camera.json
 create mode 100644 unilabos/registry/devices/camera.yaml
 create mode 100644 unilabos/ros/nodes/presets/camera.py

diff --git a/test/experiments/camera.json b/test/experiments/camera.json
new file mode 100644
index 0000000..2d20845
--- /dev/null
+++ b/test/experiments/camera.json
@@ -0,0 +1,45 @@
+{
+    "nodes": [
+        {
+            "id": "Camera",
+            "name": "摄像头",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "camera",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "camera_index": 0,
+                "period": 0.05
+            },
+            "data": {
+            }
+        },
+        {
+            "id": "Gripper1",
+            "name": "假夹爪",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "gripper.mock",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+            },
+            "data": {
+            }
+        }
+    ],
+    "links": [
+        
+    ]
+}
\ No newline at end of file
diff --git a/unilabos-linux-64.yaml b/unilabos-linux-64.yaml
index aeac636..d2f6df1 100644
--- a/unilabos-linux-64.yaml
+++ b/unilabos-linux-64.yaml
@@ -48,8 +48,9 @@ dependencies:
   - ros-humble-ros2-control
   - ros-humble-robot-state-publisher
   - ros-humble-joint-state-publisher
-  # web
+  # web and visualization
   - ros-humble-rosbridge-server
+  - ros-humble-cv-bridge
   # geometry & motion planning
   - ros-humble-tf2
   - ros-humble-moveit
diff --git a/unilabos-osx-arm64.yaml b/unilabos-osx-arm64.yaml
index 83ffe1b..391e818 100644
--- a/unilabos-osx-arm64.yaml
+++ b/unilabos-osx-arm64.yaml
@@ -50,8 +50,9 @@ dependencies:
   - ros-humble-ros2-control
   - ros-humble-robot-state-publisher
   - ros-humble-joint-state-publisher
-  # web
+  # web and visualization
   - ros-humble-rosbridge-server
+  - ros-humble-cv-bridge
   # geometry & motion planning
   - ros-humble-tf2
   - ros-humble-moveit
diff --git a/unilabos-win64.yaml b/unilabos-win64.yaml
index 9ae2243..93f6332 100644
--- a/unilabos-win64.yaml
+++ b/unilabos-win64.yaml
@@ -48,8 +48,9 @@ dependencies:
   - ros-humble-ros2-control
   - ros-humble-robot-state-publisher
   - ros-humble-joint-state-publisher
-  # web
+  # web and visualization
   - ros-humble-rosbridge-server
+  - ros-humble-cv-bridge
   # geometry & motion planning
   - ros-humble-tf2
   - ros-humble-moveit
diff --git a/unilabos/registry/devices/camera.yaml b/unilabos/registry/devices/camera.yaml
new file mode 100644
index 0000000..241576e
--- /dev/null
+++ b/unilabos/registry/devices/camera.yaml
@@ -0,0 +1,10 @@
+camera:
+  class:
+    action_value_mappings: {}
+    module: unilabos.ros.nodes.presets.camera:VideoPublisher
+    status_types: {}
+    type: ros2
+  description: ''
+  handles: []
+  icon: ''
+  init_param_schema: {}
diff --git a/unilabos/ros/nodes/presets/camera.py b/unilabos/ros/nodes/presets/camera.py
new file mode 100644
index 0000000..e161671
--- /dev/null
+++ b/unilabos/ros/nodes/presets/camera.py
@@ -0,0 +1,61 @@
+import rclpy
+from rclpy.node import Node
+import cv2
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
+
+class VideoPublisher(BaseROS2DeviceNode):
+    def __init__(self, device_id='video_publisher', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
+        # 初始化BaseROS2DeviceNode，使用自身作为driver_instance
+        BaseROS2DeviceNode.__init__(
+            self,
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface="camera",
+            print_publish=False,
+            resource_tracker=resource_tracker,
+        )
+        # 创建一个发布者，发布到 /video 话题，消息类型为 sensor_msgs/Image，队列长度设为 10
+        self.publisher_ = self.create_publisher(Image, f'/{device_id}/video', 10)
+        # 初始化摄像头（默认设备索引为 0）
+        self.cap = cv2.VideoCapture(camera_index)
+        if not self.cap.isOpened():
+            self.get_logger().error("无法打开摄像头")
+        # 用于将 OpenCV 的图像转换为 ROS 图像消息
+        self.bridge = CvBridge()
+        # 设置定时器，10 Hz 发布一次
+        timer_period = period  # 单位：秒
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        ret, frame = self.cap.read()
+        if not ret:
+            self.get_logger().error("读取视频帧失败")
+            return
+        # 将 OpenCV 图像转换为 ROS Image 消息，注意图像编码需与摄像头数据匹配，这里使用 bgr8
+        img_msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
+        self.publisher_.publish(img_msg)
+        # self.get_logger().info("已发布视频帧")
+
+    def destroy_node(self):
+        # 释放摄像头资源
+        if self.cap.isOpened():
+            self.cap.release()
+        super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VideoPublisher()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()