编写一段ROS订阅摄像头消息的色块识别程序

以下是基于ROS Kinetic和Python编写的订阅摄像头消息的色块识别程序：

#!/usr/bin/env python

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

class ColorBlockDetector:
    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.image_callback)

    def image_callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)

        # Define range of blue color in HSV
        lower_blue = np.array([110,50,50])
        upper_blue = np.array([130,255,255])

        # Threshold the HSV image to get only blue colors
        mask = cv2.inRange(hsv, lower_blue, upper_blue)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(cv_image,cv_image, mask= mask)

        # Find contours
        contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        # Draw contours
        cv2.drawContours(cv_image, contours, -1, (0,255,0), 3)

        # Show result
        cv2.imshow('image',cv_image)
        cv2.imshow('mask',mask)
        cv2.imshow('res',res)
        cv2.waitKey(1)

if __name__ == '__main__':
    rospy.init_node('color_block_detector', anonymous=True)
    cbd = ColorBlockDetector()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
    cv2.destroyAllWindows()

这个程序订阅`/camera/image_raw`话题，使用`cv_bridge`将ROS图像消息转换为OpenCV图像格式。随后，它将图像从BGR颜色空间转换为HSV颜色空间，并使用`cv2.inRange`函数通过指定颜色的HSV范围来创建掩膜。然后，程序使用`cv2.findContours`函数查找掩膜中的轮廓，使用`cv2.drawContours`函数绘制轮廓线，并使用`cv2.imshow`函数显示结果图像。

注意，这个程序仅仅是一个简单的示例，可能需要根据你的具体应用场景进行修改。