From 4cc3dbae73fe1edb22a27a560103e1bd8132b591 Mon Sep 17 00:00:00 2001 From: 23-yoshikawa Date: Thu, 1 Aug 2024 00:51:20 +0900 Subject: [PATCH 1/5] =?UTF-8?q?=E7=94=BB=E5=83=8F=E3=82=92=E5=9C=A7?= =?UTF-8?q?=E7=B8=AE=E3=81=97=E3=81=A6=E9=80=81=E5=8F=97=E4=BF=A1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/imshow/imshow/imshow.py | 18 +++++++++++++----- device/camera_reader/camera_reader/camera.py | 8 +++++--- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/app/imshow/imshow/imshow.py b/app/imshow/imshow/imshow.py index 2eb3749..0e3b9d6 100644 --- a/app/imshow/imshow/imshow.py +++ b/app/imshow/imshow/imshow.py @@ -2,14 +2,16 @@ import rclpy from cv_bridge import CvBridge from rclpy.node import Node -from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage +import numpy as np + class Imshow(Node): def __init__(self): super().__init__("imshow") self.subscription = self.create_subscription( - Image, + CompressedImage, "camera_image", self.listener_callback, 10, @@ -17,9 +19,15 @@ def __init__(self): self.bridge = CvBridge() def listener_callback(self, msg): - frame = self.bridge.imgmsg_to_cv2(msg, "bgr8") - cv2.imshow("Camera", frame) - cv2.waitKey(1) + # CompressedImageデータをNumPy配列に変換 + np_arr = np.frombuffer(msg.data, np.uint8) + # NumPy配列から画像をデコード + frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if frame is not None: + cv2.imshow("Camera", frame) + cv2.waitKey(1) + else: + self.get_logger().error("Failed to decode image") def main(args=None): diff --git a/device/camera_reader/camera_reader/camera.py b/device/camera_reader/camera_reader/camera.py index 4d1bf74..920b17b 100644 --- a/device/camera_reader/camera_reader/camera.py +++ b/device/camera_reader/camera_reader/camera.py @@ -2,14 +2,14 @@ import rclpy from cv_bridge import CvBridge from rclpy.node import Node -from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage from std_msgs.msg import Header class Camera(Node): def __init__(self): super().__init__("camera") - self.publisher_ = self.create_publisher(Image, "camera_image", 10) + self.publisher_ = self.create_publisher(CompressedImage, "camera_image", 10) self.timer = self.create_timer(0.1, self.timer_callback) self.cap = cv2.VideoCapture(0) self.bridge = CvBridge() @@ -27,7 +27,9 @@ def _generate_header(self) -> Header: def timer_callback(self): ret, frame = self.cap.read() if ret: - msg = self.bridge.cv2_to_imgmsg(frame, "bgr8", header=self._generate_header()) + self.get_logger().info("read image") + msg = self.bridge.cv2_to_compressed_imgmsg(frame) + msg.header=self._generate_header() self.publisher_.publish(msg) else: self.get_logger().error("Failed to capture image") From 1033aeb03635f72f6459ced0c9486f350bc51e78 Mon Sep 17 00:00:00 2001 From: 23-yoshikawa Date: Sat, 3 Aug 2024 09:25:45 +0900 Subject: [PATCH 2/5] =?UTF-8?q?=E3=83=87=E3=83=90=E3=83=83=E3=82=AF?= =?UTF-8?q?=E7=94=A8=E3=83=A1=E3=83=83=E3=82=BB=E3=83=BC=E3=82=B8=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- device/camera_reader/camera_reader/camera.py | 1 - 1 file changed, 1 deletion(-) diff --git a/device/camera_reader/camera_reader/camera.py b/device/camera_reader/camera_reader/camera.py index f35dda2..8817014 100644 --- a/device/camera_reader/camera_reader/camera.py +++ b/device/camera_reader/camera_reader/camera.py @@ -32,7 +32,6 @@ def _generate_header(self) -> Header: def timer_callback(self): ret, frame = self.cap.read() if ret: - self.get_logger().info("read image") msg = self.bridge.cv2_to_compressed_imgmsg(frame) msg.header=self._generate_header() self.publisher_.publish(msg) From a928960eb2f55e02cd66ab3087b680bd84110783 Mon Sep 17 00:00:00 2001 From: 23-yoshikawa Date: Sat, 3 Aug 2024 18:54:43 +0900 Subject: [PATCH 3/5] =?UTF-8?q?=E3=82=A4=E3=83=B3=E3=83=9D=E3=83=BC?= =?UTF-8?q?=E3=83=88=E9=A0=86=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/imshow/imshow/imshow.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/app/imshow/imshow/imshow.py b/app/imshow/imshow/imshow.py index 0e3b9d6..76f968d 100644 --- a/app/imshow/imshow/imshow.py +++ b/app/imshow/imshow/imshow.py @@ -1,10 +1,9 @@ import cv2 +import numpy as np import rclpy from cv_bridge import CvBridge from rclpy.node import Node from sensor_msgs.msg import CompressedImage -import numpy as np - class Imshow(Node): From e126d9a7424993f9bf8ba0726bedc58472ae424c Mon Sep 17 00:00:00 2001 From: 23-yoshikawa <136178082+23-yoshikawa@users.noreply.github.com> Date: Sun, 4 Aug 2024 17:12:45 +0900 Subject: [PATCH 4/5] Update device/camera_reader/camera_reader/camera.py Co-authored-by: H1rono_K <54711422+H1rono@users.noreply.github.com> --- device/camera_reader/camera_reader/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/device/camera_reader/camera_reader/camera.py b/device/camera_reader/camera_reader/camera.py index 8817014..1d66974 100644 --- a/device/camera_reader/camera_reader/camera.py +++ b/device/camera_reader/camera_reader/camera.py @@ -33,7 +33,7 @@ def timer_callback(self): ret, frame = self.cap.read() if ret: msg = self.bridge.cv2_to_compressed_imgmsg(frame) - msg.header=self._generate_header() + msg.header = self._generate_header() self.publisher_.publish(msg) else: self.get_logger().error("Failed to capture image") From 648e38444a8573fdb21656ee5b3f2c25598f2a72 Mon Sep 17 00:00:00 2001 From: 23-yoshikawa <136178082+23-yoshikawa@users.noreply.github.com> Date: Sun, 4 Aug 2024 17:44:29 +0900 Subject: [PATCH 5/5] Update camera.py --- device/camera_reader/camera_reader/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/device/camera_reader/camera_reader/camera.py b/device/camera_reader/camera_reader/camera.py index 1d66974..4707acb 100644 --- a/device/camera_reader/camera_reader/camera.py +++ b/device/camera_reader/camera_reader/camera.py @@ -10,7 +10,7 @@ class Camera(Node): def __init__(self): super().__init__("camera") self.publisher_ = self.create_publisher(CompressedImage, "camera_image", 10) - self.timer = self.create_timer(0.1, self.timer_callback) + self.timer = self.create_timer(0.01, self.timer_callback) param = self.declare_parameter("camera_id", 0) camera_id = ( self.get_parameter_or(param.name, param).get_parameter_value().integer_value