Raspberry Pi : DNN : 目標物体に到達する : プログラム

プログラム

作成: 2021-05-24
更新: 2021-05-26

#!/usr/bin/env python ##### モデル tiny YOLO の設定 ################# from core.utils import load_class_names, load_image, draw_boxes, draw_boxes_frame from core.yolo_tiny import YOLOv3_tiny from core.yolo import YOLOv3 # object のクラス class_names, n_classes = load_class_names() iou_threshold = 0.1 confidence_threshold = 0.25 model = YOLOv3_tiny(n_classes=n_classes, iou_threshold=iou_threshold, confidence_threshold=confidence_threshold) import tensorflow as tf inputs = tf.placeholder(tf.float32, [1, *model.input_size, 3]) detections = model(inputs) saver = tf.train.Saver(tf.global_variables(scope=model.scope)) ##### ターゲット ################# target_class = 'bottle' # 配列 class_names における target object の番号 target_n を求める for n in range(len(class_names)): if class_names[n] == target_class: target_n = n break ##### カメラ取込画像 ########################## # ヨコ, タテ CAMERA_WIDTH = 640 CAMERA_HEIGHT = 480 # 中心 ImgCenter_x = CAMERA_WIDTH/2 ###### GPIO #################################### import RPi.GPIO as GPIO # Set the GPIO pins as numbering GPIO.setmode(GPIO.BOARD) ###### Sensor #################################### TrigPin = 22 EchoPin = 18 # Set the TrigPin's mode is output GPIO.setup(TrigPin,GPIO.OUT) GPIO.output(TrigPin, GPIO.LOW) # Set the EchoPin's mode is input, and ON→ HIGH GPIO.setup(EchoPin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) ###### Distance from obstacle where GoPiGo should stop : 20cm distance_to_stop = 30 ###### 距離計測 #################################### import time def dist_read(): # 10us pulse to TrigPin time.sleep(0.3) GPIO.output(TrigPin, GPIO.HIGH) time.sleep(0.00001) GPIO.output(TrigPin, GPIO.LOW) # 超音波発信 # EchoPin が LOW から HIGH に変わる時刻 signaloff while GPIO.input(EchoPin) == GPIO.LOW: signaloff = time.time() # EchoPin が HIGH から LOW に変わる時刻 signalon while GPIO.input(EchoPin) == GPIO.HIGH: signalon = time.time() # EchoPin が HIGH だった時間 timepassed = signalon - signaloff distance = timepassed * 17000 return round(distance) ##### GoPiGo motor ########################## from easygopigo3 import EasyGoPiGo3 egpg = EasyGoPiGo3() import time ## 回転 #################### def rotation( t ): egpg.set_speed(50) print('rotation t：' + str(t)) if t > 0: egpg.right() time.sleep(t) else: egpg.left() time.sleep(-t) egpg.stop() ## カメラ映像画面の dx に対する GoPiGo 回転時間 t # 車輪モータの set_speed 値が 50 のとき def pixel2t( dx ): abs_dx = abs( dx ) if abs_dx < 60: return dx * 0.6 / 50 elif abs_dx < 110: return dx * 0.9 / 100 elif abs_dx < 160: return dx * 1.2 / 150 elif abs_dx < 210: return dx * 2.2 / 200 elif abs_dx < 310: return dx * 2.4 / 300 else: return 2.4 * dx / abs_dx ## 前進 ############### # go forward 1sec def go_fwd(): egpg.set_speed(100) egpg.forward() time.sleep(1) egpg.stop() ##### exit プロセス ################# import cv2 import sys def bye(): input('キー入力で, プログラム終了...') egpg.stop() # Release resource GPIO.cleanup() egpg.reset_all() #カメラキャプチャを停止 cap.release() #ストリーミングウインドを閉じる cv2.destroyAllWindows() #プログラムを終了 sys.exit() ##################################################### ##### Program starts from here ########################## ##################################################### with tf.Session() as sess: # モデル tiny YOLO の読み込み saver.restore(sess, './weights/model-tiny.ckpt') # カメラ映像の読み込み (カメラ番号は 0) cap = cv2.VideoCapture(0) # 前回ループのときの target_center_x 値を留める変数 (初期値 ImgCenter_x) target_center_x_last = ImgCenter_x # target が 20回続けて検出されないときは，終了する miss = 0 action = '' while True: ######## YOLO ################################## # カメラ映像 ret, frame = cap.read() # ヨコ,タテ frame_size = (frame.shape[1], frame.shape[0]) # object 検出 resized_frame = cv2.resize(frame, dsize=tuple((x) for x in model.input_size[::-1]), interpolation=cv2.INTER_NEAREST) result = sess.run(detections, feed_dict={inputs: [resized_frame]}) # 検出枠表示 draw_boxes_frame(frame, frame_size, result, class_names, model.input_size) cv2.imshow('frame', frame) ######## target を検出 ################################## if action == '': miss += 1 if miss > 20: print('target lost !\n') bye() else: miss = 0 action = '' # target 検出枠 boxes_dict = result[0] boxes = boxes_dict[target_n] resize_factor = (frame_size[0] / model.input_size[1], frame_size[1] / model.input_size[0]) # target が検出されたら if( len(boxes) != 0): print('ターゲット検出') for box in boxes: coordinates = box[:4] coordinates = [int(coordinates[i] * resize_factor[i % 2]) for i in range(4)] # target の中心 target_center_x = int((coordinates[0]+coordinates[2])/2) # 画像中心からの object 中心のズレ dx = target_center_x - ImgCenter_x if abs(dx) < 20: print("中心ズレ：" + str(dx) + '\n') action = 'still' else: if abs( target_center_x - target_center_x_last ) > 20: target_center_x_last = target_center_x print('中心ズレ：' + str(dx)) # abs(dx) > 20 ならば，GoPiGo の回転へ if abs(dx) > 20: action = 'turn' # 残りの box は取り上げない break # ＜for box in boxes＞　ここまで # ＜if( len(boxes) != 0)＞　ここまで else: print('ターゲット検出せず\n') #print('action : ' + action + '\n') ######## target の方向にターン ################################## if action == 'turn': print('向き変更開始') rotation( pixel2t( dx ) ) print('向き変更終了\n') ######## target との距離を測る ################################## if action != '': dist = dist_read() print('distance : ' + str(dist) + 'cm') if dist > 450: print('target lost !\n') bye() if dist < distance_to_stop: print('Arrived !\n') bye() ######## target に向かって前進 ################################## print('move forward') go_fwd() print('stop\n') ######## プログラムを途中終了する ################################## if cv2.waitKey(10) & 0xFF == ord('q'): bye()

ターゲット検出中心ズレ：166.0 向き変更開始 rotation t：1.8260000000000003 向き変更終了 distance : 100cm move forward stop ターゲット検出ターゲット検出ターゲット検出ターゲット検出ターゲット検出ターゲット検出中心ズレ：-60.0 向き変更開始 rotation t：-0.54 向き変更終了 distance : 92cm move forward stop ターゲット検出ターゲット検出ターゲット検出ターゲット検出ターゲット検出ターゲット検出中心ズレ：9.0 distance : 87cm move forward stop ターゲット検出中心ズレ：10.0 distance : 81cm move forward stop ターゲット検出中心ズレ：10.0 distance : 78cm move forward stop ターゲット検出中心ズレ：10.0 distance : 74cm move forward stop ターゲット検出中心ズレ：10.0 distance : 68cm move forward stop ターゲット検出中心ズレ：10.0 distance : 65cm move forward stop ターゲット検出中心ズレ：31.0 向き変更開始 rotation t：0.37199999999999994 向き変更終了 distance : 57cm move forward stop ターゲット検出ターゲット検出ターゲット検出ターゲット検出中心ズレ：65.0 向き変更開始 rotation t：0.585 向き変更終了 distance : 52cm move forward stop ターゲット検出中心ズレ：93.0 向き変更開始 rotation t：0.8370000000000001 向き変更終了 distance : 47cm move forward stop ターゲット検出中心ズレ：69.0 向き変更開始 rotation t：0.621 向き変更終了 distance : 42cm move forward stop ターゲット検出ターゲット検出ターゲット検出ターゲット検出中心ズレ：34.0 向き変更開始 rotation t：0.408 向き変更終了 distance : 83cm move forward stop ターゲット検出中心ズレ：-62.0 向き変更開始 rotation t：-0.558 向き変更終了 distance : 78cm move forward stop ターゲット検出中心ズレ：-136.0 向き変更開始 rotation t：-1.0879999999999999 向き変更終了 distance : 25cm Arrived ! キー入力で, プログラム終了... $