<ROS1> Opencv로 신호등,횡단보도 검출

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import rospy
import time
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import String

src = None  # 초기값을 None으로 설정
bridge = CvBridge()
previous_red_time = 0
previous_green_time = 0

def rotate_image_90_right(image):
    # 이미지 전치 (행과 열을 교환)
    transposed = cv2.transpose(image)
    # 전치된 이미지를 수평으로 플립하여 90도 오른쪽으로 회전
    rotated = cv2.flip(transposed, 1)
    return rotated

def perspective_transform(image, src_points, dst_points):
    matrix = cv2.getPerspectiveTransform(np.float32(src_points), np.float32(dst_points))
    warped_image = cv2.warpPerspective(image, matrix, (300, 300))
    return warped_image

def video_callback(data):
    global src
    try: 
        src = bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e: 
        rospy.logerr("cvBridgeError: %s", e)

def preprocess_mask_green(mask):
    #mask = cv2.GaussianBlur(mask, (3, 3), 0)
    kernel = np.ones((5, 5), np.uint8)
    #mask = cv2.erode(mask, kernel, iterations=6)
    mask = cv2.dilate(mask, kernel, iterations=5)
    return mask

def preprocess_mask(mask):
    mask = cv2.GaussianBlur(mask, (3, 3), 0)
    kernel = np.ones((5, 5), np.uint8)
    mask = cv2.erode(mask, kernel, iterations=6)
    mask = cv2.dilate(mask, kernel, iterations=5)
    return mask

def preprocess_mask_bodo(mask):
    mask = cv2.GaussianBlur(mask, (5, 5), 0)
    kernel = np.ones((5, 5), np.uint8)
    #mask = cv2.erode(mask, kernel, iterations=6)
    #mask = cv2.dilate(mask, kernel, iterations=5)
    return mask

# 빨간색과 초록색 HSV 범위 정의
lower_red1 = np.array([0, 130, 130])
upper_red1 = np.array([18, 255, 255])
lower_red2 = np.array([170, 130, 130])
upper_red2 = np.array([180, 255, 255])

lower_green = np.array([40, 55, 170])
upper_green = np.array([90, 255, 255])

MIN_AREA_THRESHOLD = 2500

rospy.init_node('traffic_light_detector', anonymous=True)
rospy.Subscriber('/opencv_topic', Image, video_callback)
red_light_pub = rospy.Publisher('traffic_light_status', String, queue_size=10)
green_light_pub = rospy.Publisher('traffic_light_status', String, queue_size=10)
crosswalk_pub = rospy.Publisher('crosswalk_detected', Image, queue_size=10)

def detect_crosswalk(image):
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    gray = preprocess_mask_bodo(gray)
    #cv2.imshow('그레이 프레임에 전처리만 적용한 프레임', gray)
    
    edges = cv2.Canny(gray, 50, 100) #100 150     50 130   50 100 이 최적의 파라미터이다.
    cv2.imshow('케니 엣지 적용한 프레임', edges)  
    
    contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    visualized_image = image.copy()
    all_points = []

    # 컨투어에서 평행사변형 검출
    for contour in contours:
        area = cv2.contourArea(contour)
        print(area)
        if area < 2300 or area > 5000:
            continue
        
        epsilon = 0.03 * cv2.arcLength(contour, True)  #0.02
        approx = cv2.approxPolyDP(contour, epsilon, True)
        
        if len(approx) == 4:  # 사각형으로 판별됨
            x, y, w, h = cv2.boundingRect(approx)
            aspect_ratio = float(h) / w
            if 0.3 < aspect_ratio < 8.0:
                cv2.drawContours(visualized_image, [approx], -1, (255, 0, 255), 2)
                for point in approx:
                    x, y = point.ravel()
                    cv2.circle(visualized_image, (int(x), int(y)), 10, (255, 0, 0), -1)
                all_points.append(approx)
    
    # 컨벡스 헐에서 사각형 검출
    if all_points:
        all_points = np.vstack(all_points)
        final_hull = cv2.convexHull(all_points)
        
        epsilon = 0.02 * cv2.arcLength(final_hull, True)
        approx = cv2.approxPolyDP(final_hull, epsilon, True)
        
        if len(approx) == 4:
            cv2.drawContours(visualized_image, [approx], -1, (0, 255, 0), 2)
            for point in approx:
                x, y = point.ravel()
                cv2.circle(visualized_image, (int(x), int(y)), 10, (0, 0, 255), -1)
            
            return visualized_image, approx #approx가 실제 컨벡스헐이고, visualized_image는 보여주기용 프레임

    # 컨투어가 감지되지 않은 경우
    return visualized_image, None



def main():
    global previous_red_time, previous_green_time
    while not rospy.is_shutdown():
        if src is None:
            continue

        hsv = cv2.cvtColor(src, cv2.COLOR_BGR2HSV)

        # 빨간색과 초록색 마스크 생성
        mask_red1 = cv2.inRange(hsv, lower_red1, upper_red1)
        mask_red2 = cv2.inRange(hsv, lower_red2, upper_red2)
        mask_red = cv2.bitwise_or(mask_red1, mask_red2)
        mask_red = preprocess_mask(mask_red)
        #cv2.imshow('빨간 신호등 검출', mask_red)

        mask_green = cv2.inRange(hsv, lower_green, upper_green)
        mask_green = preprocess_mask_green(mask_green)
        cv2.imshow('초록 신호등 검출', mask_green)

        contours_red, _ = cv2.findContours(mask_red, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        contours_green, _ = cv2.findContours(mask_green, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        current_time = time.time()

        # 빨간불 감지
        for contour in contours_red:
            area = cv2.contourArea(contour)
            if area > MIN_AREA_THRESHOLD:
                x, y, w, h = cv2.boundingRect(contour)
                aspect_ratio = float(w) / h
                if 0.8 < aspect_ratio < 2.0:
                    cv2.drawContours(src, [contour], 0, (0, 0, 255), 5)
                    if (current_time - previous_red_time) > 10:
                        red_light_pub.publish("redlight")
                        rospy.loginfo("빨간불을 감지했고, 토픽을 퍼블리시 했습니다.")
                        previous_red_time = current_time
                    else:
                        rospy.loginfo('빨간불을 감지했지만 10초가 지나지 않아 퍼블리시하지 않았습니다.')

        # 초록불 감지
        for contour in contours_green:
            area = cv2.contourArea(contour)
            if area > MIN_AREA_THRESHOLD:
                x, y, w, h = cv2.boundingRect(contour)
                aspect_ratio = float(w) / h
                if 0.8 < aspect_ratio < 2.0:
                    cv2.drawContours(src, [contour], 0, (0, 255, 0), 5)
                    if (current_time - previous_green_time) > 10:
                        green_light_pub.publish("greenlight")
                        rospy.loginfo("초록불을 감지했고, 토픽을 퍼블리시 했습니다.")
                        previous_green_time = current_time
                    else:
                        rospy.loginfo('초록불을 감지했지만 10초가 지나지 않아 퍼블리시하지 않았습니다.')

        # 횡단보도 감지 및 퍼스펙티브 변환
        visualized_image, src_points = detect_crosswalk(src)
        if src_points is not None and src_points.size == 8:  # src_points가 유효한지 검사
            dst_points = [
                (0, 0),
                (300, 0),
                (300, 300),
                (0, 300)
            ]

            warped_image = perspective_transform(src, src_points, dst_points)
            rotated_image = rotate_image_90_right(warped_image)
            cv2.imshow('횡단보도 검출', rotated_image)
            crosswalk_pub.publish(bridge.cv2_to_imgmsg(warped_image, "bgr8"))

        cv2.imshow('횡단보도 검출 컨벡스헐 처리 결과', visualized_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

1. 정지표지판은 Jetson inference를 활용하여 구분. 신호등과 정지표지판 모양이 비슷하여, 정지표지판을 보고 빨간신호등 신호도 같이 잡힘 -> 타원의 성질 이용 (너비/높이) or 외접원과 컨투어의 넓이 비교

2. 횡단보도를 검출시 HSV이용하니 주변환경에 따라 너무 결과가 달라짐 -> canny edge detection이용 (임계값 튜닝으로 횡단보도만 드러나게 함) 이후 convex hull 사용.

이후 검출 결과를 String형태의 메시지를 토픽으로 쏴줌