File size: 1,321 Bytes
15b8951 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 | """RosFrameSource: subscribe /camera/image_raw -> latest BGR frame."""
def _waiting_frame(text="Dang cho camera /camera/image_raw..."):
import numpy as np
import cv2
frame = np.full((480, 640, 3), 245, dtype=np.uint8)
cv2.putText(frame, text, (30, 240),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (60, 60, 60), 2)
return frame
class RosFrameSource:
def __init__(self, node, image_topic="/camera/image_raw"):
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
# Camera của Go2 publish bằng QoS BEST_EFFORT (sensor data); subscriber
# phải khớp, nếu dùng RELIABLE mặc định sẽ không nhận được frame nào.
from rclpy.qos import qos_profile_sensor_data
self.node = node
self.bridge = CvBridge()
self._latest = _waiting_frame() # placeholder để web không quay vô tận
self._got = False
self.sub = node.create_subscription(
Image, image_topic, self._on_image, qos_profile_sensor_data
)
def _on_image(self, msg):
self._latest = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
self._got = True
@property
def is_connected(self):
return self._got
def get_latest_frame(self):
return self._latest
|