compress_camera.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImageResizer:
    def __init__(self):
        rospy.init_node('image_resizer', anonymous=True)
        self.bridge = CvBridge()
        
        # 发布缩小后的图像，仍然是Image格式
        self.pub = rospy.Publisher('/hikrobot_camera/rgb_small', Image, queue_size=1)
        self.sub = rospy.Subscriber('/hikrobot_camera/rgb', Image, self.callback, queue_size=1)
        
        # 缩放参数
        self.scale_factor = rospy.get_param('~scale_factor', 0.05)  # 缩放到1/4
        self.target_width = int(3072 * self.scale_factor)   # 768
        self.target_height = int(2048 * self.scale_factor)  # 512
        
        rospy.loginfo("图像缩放器启动:")
        rospy.loginfo("原始尺寸: 3072x2048")
        rospy.loginfo("目标尺寸: %dx%d", self.target_width, self.target_height)
        rospy.loginfo("缩放因子: %.2f", self.scale_factor)
        
    def callback(self, msg):
        try:
            # 转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
            
            # 缩放图像
            resized = cv2.resize(cv_image, (self.target_width, self.target_height), 
                               interpolation=cv2.INTER_AREA)  # INTER_AREA适合缩小
            
            # 转换回ROS Image消息
            resized_msg = self.bridge.cv2_to_imgmsg(resized, encoding='bgr8')
            resized_msg.header = msg.header  # 保持原有header信息
            
            # 发布
            self.pub.publish(resized_msg)
            
        except Exception as e:
            rospy.logerr("图像处理失败: %s", str(e))

if __name__ == '__main__':
    try:
        resizer = ImageResizer()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("图像缩放器已停止")