塔克创新ros小车红绿灯识别寻线代码

以下是一份基于ROS的塔克创新小车红绿灯识别寻线代码，你可以根据需要进行修改：

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

import cv2
import numpy as np
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import String


class LineDetector:
    def __init__(self):
        rospy.init_node('LineDetector', anonymous=True)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.callback)
        self.line_pub = rospy.Publisher('/line_info', String, queue_size=1)

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

        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        lower_yellow = np.array([20, 100, 100], dtype=np.uint8)
        upper_yellow = np.array([30, 255, 255], dtype=np.uint8)
        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

        h, w, d = cv_image.shape
        search_top = 3*h/4  # 需要寻找线的区域的顶部
        search_bot = search_top + 20  # 需要寻找线的区域的底部
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0

        M = cv2.moments(mask)
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            self.line_pub.publish(str(cx) + "," + str(cy))


if __name__ == '__main__':
    ld = LineDetector()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

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

import cv2
import numpy as np
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import String


class TrafficLightDetector:
    def __init__(self):
        rospy.init_node('TrafficLightDetector', anonymous=True)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.callback)
        self.light_pub = rospy.Publisher('/traffic_light', String, queue_size=1)

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

        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        lower_red = np.array([0, 100, 100], dtype=np.uint8)
        upper_red = np.array([10, 255, 255], dtype=np.uint8)
        mask1 = cv2.inRange(hsv, lower_red, upper_red)
        lower_red = np.array([160, 100, 100], dtype=np.uint8)
        upper_red = np.array([179, 255, 255], dtype=np.uint8)
        mask2 = cv2.inRange(hsv, lower_red, upper_red)
        mask = mask1 + mask2

        h, w, d = cv_image.shape
        search_top = 3*h/4  # 需要寻找交通灯的区域的顶部
        search_bot = search_top + 20  # 需要寻找交通灯的区域的底部
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0

        M = cv2.moments(mask)
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            self.light_pub.publish(str(cx) + "," + str(cy))


if __name__ == '__main__':
    tld = TrafficLightDetector()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")