62 lines
1.4 KiB
Python
62 lines
1.4 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
|
||
|
|
import rclpy
|
||
|
|
from rclpy.node import Node
|
||
|
|
|
||
|
|
from sensor_msgs.msg import Image
|
||
|
|
from cv_bridge import CvBridge
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
|
||
|
|
|
||
|
|
class BlackflyViewer(Node):
|
||
|
|
def __init__(self):
|
||
|
|
super().__init__('blackfly_viewer')
|
||
|
|
|
||
|
|
# Change this topic if your driver publishes elsewhere
|
||
|
|
image_topic = '/right_camera/image_raw'
|
||
|
|
|
||
|
|
self.bridge = CvBridge()
|
||
|
|
|
||
|
|
self.subscription = self.create_subscription(
|
||
|
|
Image,
|
||
|
|
image_topic,
|
||
|
|
self.image_callback,
|
||
|
|
10
|
||
|
|
)
|
||
|
|
|
||
|
|
self.get_logger().info(f'Subscribed to {image_topic}')
|
||
|
|
|
||
|
|
def image_callback(self, msg: Image):
|
||
|
|
try:
|
||
|
|
# Convert ROS Image -> OpenCV image
|
||
|
|
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
|
||
|
|
|
||
|
|
cv2.imshow('Blackfly Camera', frame)
|
||
|
|
|
||
|
|
# Needed for OpenCV window events
|
||
|
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||
|
|
self.get_logger().info('Shutting down viewer')
|
||
|
|
rclpy.shutdown()
|
||
|
|
|
||
|
|
except Exception as e:
|
||
|
|
self.get_logger().error(f'Image conversion failed: {e}')
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
rclpy.init()
|
||
|
|
node = BlackflyViewer()
|
||
|
|
|
||
|
|
try:
|
||
|
|
rclpy.spin(node)
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
pass
|
||
|
|
finally:
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
node.destroy_node()
|
||
|
|
rclpy.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|