📷 第23课：摄像头与图像

📋 课程目标

• 理解ROS2图像消息格式与传输
• 掌握cv_bridge实现ROS2与OpenCV互操作
• 学会image_pipeline图像处理管线
• 实现图像发布/订阅节点
• 掌握相机标定与图像压缩

🧠 ROS2图像系统

ROS2的图像系统基于sensor_msgs/Image和sensor_msgs/CompressedImage消息，通过image_transport实现高效的图像传输，配合cv_bridge实现与OpenCV的无缝对接。

📐 Image消息结构

# sensor_msgs/msg/Image
std_msgs/Header header
uint32 height                # 图像高度(行数)
uint32 width                 # 图像宽度(列数)
string encoding              # 编码: rgb8, bgr8, mono8, 16UC1...
uint8 is_bigendian
uint32 step                  # 一行字节数
uint8[] data                 # 像素数据

# sensor_msgs/msg/CompressedImage
std_msgs/Header header
string format                # jpeg, png
uint8[] data                 # 压缩数据

🐍 Python：图像发布节点

#!/usr/bin/env python3
# camera_publisher.py - 摄像头图像发布节点
# ✅ Docker验证通过
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
import cv2
import numpy as np

class CameraPublisher(Node):
    def __init__(self):
        super().__init__('camera_publisher')
        self.declare_parameter('camera_device', 0)
        self.declare_parameter('frame_id', 'camera_frame')
        self.declare_parameter('publish_rate', 30.0)
        self.declare_parameter('width', 640)
        self.declare_parameter('height', 480)
        self.declare_parameter('compress_quality', 80)
        
        device = self.get_parameter('camera_device').value
        self.frame_id = self.get_parameter('frame_id').value
        rate = self.get_parameter('publish_rate').value
        self.width = self.get_parameter('width').value
        self.height = self.get_parameter('height').value
        
        self.bridge = CvBridge()
        self.cap = cv2.VideoCapture(device)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)
        
        # 发布原始图像和压缩图像
        self.image_pub = self.create_publisher(Image, '/camera/image_raw', 10)
        self.compressed_pub = self.create_publisher(
            CompressedImage, '/camera/image_raw/compressed', 10)
        
        self.timer = self.create_timer(1.0/rate, self._capture)
        self.frame_count = 0
        self.get_logger().info(f'📷 摄像头发布器启动 - {self.width}x{self.height} @ {rate}Hz')
    
    def _capture(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warn('摄像头读取失败')
            return
        
        # 发布原始图像
        img_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
        img_msg.header.stamp = self.get_clock().now().to_msg()
        img_msg.header.frame_id = self.frame_id
        self.image_pub.publish(img_msg)
        
        # 发布压缩图像
        compress = CompressedImage()
        compress.header = img_msg.header
        compress.format = 'jpeg'
        encode_param = [cv2.IMWRITE_JPEG_QUALITY, 
                       self.get_parameter('compress_quality').value]
        _, encoded = cv2.imencode('.jpg', frame, encode_param)
        compress.data = encoded.tobytes()
        self.compressed_pub.publish(compress)
        
        self.frame_count += 1
        if self.frame_count % 300 == 0:
            self.get_logger().info(f'已发布 {self.frame_count} 帧')

def main(args=None):
    rclpy.init(args=args); rclpy.spin(CameraPublisher()); rclpy.shutdown()

🐍 Python：图像处理节点

#!/usr/bin/env python3
# image_processor.py - 图像处理节点(边缘检测+轮廓)
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class ImageProcessor(Node):
    def __init__(self):
        super().__init__('image_processor')
        self.declare_parameter('canny_threshold1', 50)
        self.declare_parameter('canny_threshold2', 150)
        self.declare_parameter('blur_kernel', 5)
        self.declare_parameter('min_contour_area', 100)
        
        self.bridge = CvBridge()
        self.t1 = self.get_parameter('canny_threshold1').value
        self.t2 = self.get_parameter('canny_threshold2').value
        self.blur = self.get_parameter('blur_kernel').value
        self.min_area = self.get_parameter('min_contour_area').value
        
        self.sub = self.create_subscription(Image, '/camera/image_raw', self._cb, 10)
        self.edge_pub = self.create_publisher(Image, '/image/edges', 10)
        self.contour_pub = self.create_publisher(Image, '/image/contours', 10)
        self.get_logger().info('🖼️ 图像处理器已启动')
    
    def _cb(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        
        # 灰度 + 高斯模糊
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        blurred = cv2.GaussianBlur(gray, (self.blur, self.blur), 0)
        
        # Canny边缘检测
        edges = cv2.Canny(blurred, self.t1, self.t2)
        edge_msg = self.bridge.cv2_to_imgmsg(edges, encoding='mono8')
        edge_msg.header = msg.header
        self.edge_pub.publish(edge_msg)
        
        # 轮廓检测
        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        contour_img = frame.copy()
        count = 0
        for cnt in contours:
            if cv2.contourArea(cnt) > self.min_area:
                cv2.drawContours(contour_img, [cnt], -1, (0,255,0), 2)
                count += 1
        
        c_msg = self.bridge.cv2_to_imgmsg(contour_img, encoding='bgr8')
        c_msg.header = msg.header
        self.contour_pub.publish(c_msg)

def main(args=None):
    rclpy.init(args=args); rclpy.spin(ImageProcessor()); rclpy.shutdown()

🐍 Python：色彩检测器

#!/usr/bin/env python3
# color_detector.py - 颜色检测节点
import rclpy, numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
import cv2

class ColorDetector(Node):
    def __init__(self):
        super().__init__('color_detector')
        # HSV色彩范围参数
        self.declare_parameter('h_min', 0)
        self.declare_parameter('h_max', 10)    # 红色范围
        self.declare_parameter('s_min', 100)
        self.declare_parameter('s_max', 255)
        self.declare_parameter('v_min', 100)
        self.declare_parameter('v_max', 255)
        self.declare_parameter('min_area', 500)
        
        self.bridge = CvBridge()
        self.sub = self.create_subscription(Image, '/camera/image_raw', self._cb, 10)
        self.mask_pub = self.create_publisher(Image, '/image/color_mask', 10)
        self.center_pub = self.create_publisher(Point, '/detected_color_center', 10)
        self.get_logger().info('🎨 色彩检测器已启动')
    
    def _cb(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
        # HSV阈值过滤
        h_min = self.get_parameter('h_min').value
        h_max = self.get_parameter('h_max').value
        lower = np.array([h_min, self.get_parameter('s_min').value,
                         self.get_parameter('v_min').value])
        upper = np.array([h_max, self.get_parameter('s_max').value,
                         self.get_parameter('v_max').value])
        mask = cv2.inRange(hsv, lower, upper)
        
        # 形态学操作去噪
        kernel = np.ones((5,5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        
        # 找最大轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        min_area = self.get_parameter('min_area').value
        if contours:
            largest = max(contours, key=cv2.contourArea)
            if cv2.contourArea(largest) > min_area:
                M = cv2.moments(largest)
                if M["m00"] > 0:
                    cx = M["m10"]/M["m00"]
                    cy = M["m01"]/M["m00"]
                    pt = Point()
                    pt.x = cx / frame.shape[1]  # 归一化
                    pt.y = cy / frame.shape[0]
                    pt.z = float(cv2.contourArea(largest))
                    self.center_pub.publish(pt)
        
        mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding='mono8')
        mask_msg.header = msg.header
        self.mask_pub.publish(mask_msg)

def main(args=None):
    rclpy.init(args=args); rclpy.spin(ColorDetector()); rclpy.shutdown()

🔧 C++：cv_bridge图像处理

// image_processor_cpp.cpp
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>

class ImageProcessorCpp : public rclcpp::Node {
public:
    ImageProcessorCpp() : Node("image_processor_cpp") {
        sub_ = create_subscription<sensor_msgs::msg::Image>(
            "/camera/image_raw", 10,
            [this](sensor_msgs::msg::Image::SharedPtr msg) {
                auto cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
                cv::Mat gray;
                cv::cvtColor(cv_ptr->image, gray, cv::COLOR_BGR2GRAY);
                cv::Mat edges;
                cv::Canny(gray, edges, 50, 150);
                auto edge_msg = cv_bridge::CvImage(
                    msg->header, "mono8", edges).toImageMsg();
                pub_->publish(*edge_msg);
            });
        pub_ = create_publisher<sensor_msgs::msg::Image>("/image/edges_cpp", 10);
        RCLCPP_INFO(get_logger(), "C++图像处理器已启动");
    }
private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
};

🎯 练习题

🏆 成就解锁