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:
pa
할일