publisher 코드

import rospy   #ros노드 작성을 파이썬으로 작성하기 위한 라이브러리
import pyrealsense2 as rs # 리얼센스 모듈 임폴트함
import numpy as np
import cv2
from std_msgs.msg import String #노드간의 데이터를 주고 받을때 사용하는 데이터의 형태 를 사용하기 위해 불러옴

def talker():
    pub_p = rospy.Publisher('vision', String, queue_size=1)
    rospy.init_node('vision', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    pipeline = rs.pipeline()  # 이미지 가져옴
    config = rs.config()  # 설정 파일 생성
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  # 크기 , 포맷, 프레임 설정
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    profile = pipeline.start(config)  # 설정을 적용하여 이미지 취득 시작, 프로파일 얻음

    depth_sensor = profile.get_device().first_depth_sensor()  # 깊이 센서를 얻음
    depth_scale = depth_sensor.get_depth_scale()  # 깊이 센서의 깊이 스케일 얻음
    print("Depth Scale is: ", depth_scale)

    align_to = rs.stream.color  # depth 이미지를 맞추기 위한 이미지, 컬러 이미지
    align = rs.align(align_to)  # depth 이미지와 맞추기 위해 align 생성

    YOLO_net = cv2.dnn.readNet("")

    classes = []
    with open("", 'r') as f:
        classes = [line.strip() for line in f.readlines()]
    layer_names = YOLO_net.getLayerNames()
    output_layers = [layer_names[i[0] - 1] for i in YOLO_net.getUnconnectedOutLayers()]
    
    try:
        while not rospy.is_shutdown():
            frames = pipeline.wait_for_frames()  # color와 depth의 프레임셋을 기다림
            # frames.get_depth_frame() 은 640x360 depth 이미지이다.

            aligned_frames = align.process(frames)  # 모든(depth 포함) 프레임을 컬러 프레임에 맞추어 반환

            aligned_depth_frame = aligned_frames.get_depth_frame()  # aligned depth 프레임은 640x480 의 depth 이미지이다
            color_frame = aligned_frames.get_color_frame()  # 컬러 프레임을 얻음

            if not aligned_depth_frame or not color_frame:  # 프레임이 없으면, 건너 뜀
                continue

            depth_image = np.asanyarray(aligned_depth_frame.get_data())  # depth이미지를 배열로,
            color_image = np.asanyarray(color_frame.get_data())  # color 이미지를 배열로

            # 이미지 렌더링
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
            # applyColorMap(src, 필터) 필터를 적용함 , COLORMAP_JET=  연속적인 색상, blue -> red
            # convertScaleAbs: 인자적용 후 절대값, 8비트 반환
            images = np.hstack((color_image, depth_colormap))  # 두 이미지를 수평으로 연결
            # cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)   #이미지 윈도우 정의

            # 웹캠 프레임
            frame = color_image
            h, w, c = frame.shape

            blob = cv2.dnn.blobFromImage(frame, 0.00392, (640, 480), (0, 0, 0), True, crop=False)
            YOLO_net.setInput(blob)
            outs = YOLO_net.forward(output_layers)

            class_ids = []
            confidences = []
            boxes = []

            for out in outs:
                for detection in out:
                    scores = detection[5:]
                    class_id = np.argmax(scores)
                    confidence = scores[class_id]

                    # 검출 신뢰도
                    if confidence > 0.9:
                        # Object detected
                        center_x = int(detection[0] * w)
                        center_y = int(detection[1] * h)
                        dw = int(detection[2] * w)
                        dh = int(detection[3] * h)

                        x = int(center_x - dw / 2)
                        y = int(center_y - dh / 2)
                        boxes.append([x, y, dw, dh])
                        confidences.append(float(confidence))
                        class_ids.append(class_id)

            indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.45, 0.4)

            for i in range(len(boxes)):
                if i in indexes:
                    x, y, w, h = boxes[i]
                    label = str(classes[class_ids[i]])
                    score = confidences[i]
                    depth_obj = depth_image[x:x + w, y:y + h]
                    depth = np.median(depth_obj)
                    labelplusdepth = label + ',' + str(depth) + 'mm'

                    cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 5)
                    cv2.putText(frame, labelplusdepth, (x, y - 20), cv2.FONT_ITALIC, 0.5, (255, 255, 255), 1)
                    if label == 'ripe_Cherry tomato':
                        print('x=', x + w / 2, 'y=', y + h / 2, 'depth=', depth, 'mm')
                        if depth < 800:
                            vision_str = str('T')
                            rospy.loginfo(vision_str)
                            pub_p.publish(vision_str)
                            rate.sleep()

            cv2.imshow("YOLOv2", frame)

            key = cv2.waitKey(1)  # 키 입력
            if key & 0xFF == ord('q') or key == 27:  # 나가기
                cv2.destroyAllWindows()  # 윈도우 제거
                break

    finally:
        pipeline.stop()  # 리얼센스 데이터 스트리밍 중지

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass