60 lines
1.8 KiB
Python
60 lines
1.8 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
|
|
import time
|
|
from datetime import datetime
|
|
|
|
class CameraSnapshot(Node):
|
|
def __init__(self):
|
|
super().__init__('camera_snapshot')
|
|
self.bridge = CvBridge()
|
|
self.saved = False
|
|
self.start_time = time.time()
|
|
|
|
# 订阅RGB图像话题
|
|
self.subscription = self.create_subscription(
|
|
Image,
|
|
'/image_right', # 根据实际命名空间调整，如'/stereo_camera/image_rgb'
|
|
self.image_callback,
|
|
10)
|
|
self.get_logger().info('等待相机图像...')
|
|
|
|
def image_callback(self, msg):
|
|
current_time = time.time()
|
|
elapsed = current_time - self.start_time
|
|
|
|
# 第5秒时保存图像
|
|
if elapsed >= 5.0 and not self.saved:
|
|
try:
|
|
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
|
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
|
filename = f'rgb_snapshot_{timestamp}.jpg'
|
|
cv2.imwrite(filename, cv_image)
|
|
self.get_logger().info(f'已保存截图: {filename}')
|
|
self.saved = True
|
|
|
|
# 显示保存的图像（可选）
|
|
cv2.imshow('Saved Image', cv_image)
|
|
cv2.waitKey(3000) # 显示3秒
|
|
cv2.destroyAllWindows()
|
|
|
|
except Exception as e:
|
|
self.get_logger().error(f'图像保存失败: {str(e)}')
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = CameraSnapshot()
|
|
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
if __name__ == '__main__':
|
|
main() |