PC から ssh -X 接続
$ source ./venv/bin/activate
(venv) $ cd ~/Tensorflow-YOLOv3
(venv) $ vi reach.py
|
<![CDATA[#!/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
## 前進・後退 ###############
# go forward
def go_fwd(dist):
egpg.set_speed(100)
egpg.drive_cm(dist, True)
# go backward 2sec
def go_bk():
egpg.set_speed(100)
egpg.backward()
time.sleep(2)
egpg.stop()
## 回転 ####################
def rotation(t):
egpg.set_speed(50)
if t > 0:
print('turn left')
egpg.left()
time.sleep(t)
else:
print('turn right')
egpg.right()
time.sleep(-t)
egpg.stop()
# 左45°回転
Rot45 = 3.8
# 左120°回転
Rot120 = 10.5
## カメラ映像画面の 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
## サーボ ###############
import gopigo3
gpg = gopigo3.GoPiGo3()
def pan(t):
gpg.set_servo(gpg.SERVO_1, t )
# カメラを正面に向ける
Pan_Center = 1500
# pan ＋45°
Pan45 = 400
# 初期位置
pan(Pan_Center)
##### exit プロセス #################
import cv2
import sys
def bye():
input('キー入力で, プログラム終了...')
egpg.stop()
pan(Pan_Center)
# Release resource
GPIO.cleanup()
egpg.reset_all()
gpg.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)
###################################################
######## SEARCH ##################################
detected = False
for i in range(3):
# カメラを右45°に向ける
pan_val = Pan_Center - Pan45
pan(pan_val)
print('searching target')
for j in range(3):
if j==0:
print('right')
elif j==1:
print('front')
else:
print('left')
for k in range(20):
# カメラ映像を読む
ret, frame = cap.read()
# 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]})
# その中に target 検出枠があるか？
boxes_dict = result[0]
boxes = boxes_dict[target_n]
# target 検出枠があったら
if( len(boxes) != 0):
print('target detected !\n')
camera_pan_last = j
detected = True
break
if detected == True:
break
else:
# カメラ pan
if j < 2:
pan_val += Pan45
pan(pan_val)
if detected == True:
break
else:
# GoPiGo 回転
print('target is not detected \nGoPiGo turns left 120 degree')
rotation(Rot120)
print('done\n')
if detected == False:
print('in the end, target is not detected !\n')
bye()
###################################################
######## target を視界に据える #########################
if camera_pan_last != 1:
if camera_pan_last == 0:
print('GoPiGo turns right 45 degree')
rotation(-Rot45)
print('done\n')
else:
print('GoPiGo turns left 45 degree')
rotation(Rot45)
# 視界から外さないため，少し後退
go_bk()
print('\n')
# カメラを正面に向ける
pan(Pan_Center)
###################################################
######## APPROACH ##################################
# カメラ画像の更新を，target の x 座業の変化で判定する
# あり得ない数値を初期値にセット
target_center_x_last = 1000
# ＜画像が更新されていない＞をカウント
image_old = 0
while True:
### target を検出する (試行 20回) ###
detected = False
for k in range(20):
# カメラ映像を読む
ret, frame = cap.read()
if ret == True:
# カメラ画像のヨコ,タテ
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]})
boxes_dict = result[0]
# その中に target 検出枠があったら
boxes = boxes_dict[target_n]
if( len(boxes) != 0):
detected = True
break
# target が検出されなかったら，終了
if detected == False:
print('target lost !\n')
bye()
### target の位置 ###
resize_factor = (frame_size[0] / model.input_size[1], frame_size[1] / model.input_size[0])
for box in boxes:
coordinates = box[:4]
coordinates = [int(coordinates[i] * resize_factor[i % 2]) for i in range(4)]
# target の中心
target_center_x = round( (coordinates[0]+coordinates[2])/2 )
# 画像中心からの object 中心のズレ
dx = target_center_x - ImgCenter_x
# 残りの box は無視
break
# カメラ画像が更新されていない場合
if abs(target_center_x - target_center_x_last) < 2:
if image_old == 0:
print('image is not refreshed')
image_old += 1
#つぎの時間つぶしは必要
time.sleep(20)
# 5分以上更新されてなければ，プログラム終了
if image_old > 15:
bye()
# カメラ画像が更新されていれば
else:
if image_old > 0:
image_old = 0
print('image is refreshed\n')
# つぎの表示は，動作チェック用
#print('target_center_x_last : ' + str(target_center_x_last) + ' -> ' + str(target_center_x))
target_center_x_last = target_center_x
# object検出枠表示
draw_boxes_frame(frame, frame_size, result, class_names, model.input_size)
cv2.imshow('frame', frame)
### target の位置に対し，action を決める ###
if abs(dx) > 20:
#### target の方向にターン ####
print('direction adjusting : ' + str(dx))
rotation( pixel2t( -dx ) )
print('done\n')
else:
#### target に向かって前進 ####
print('target is in front')
print("apart from center：" + str(dx) + '\n')
# target との距離を測る
dist = dist_read()
print('distance : ' + str(dist) + 'cm')
if dist > 450:
print('target lost !\n')
bye()
elif dist < distance_to_stop:
print('Arrived !\n')
bye()
else:
fdist = round(dist/3)
print('move forward : ' + str(fdist) + 'cm')
go_fwd( fdist )
print('done\n')
######## プログラムを途中終了する ############
if cv2.waitKey(10) & 0xFF == ord('q'):
bye()]]>
|
$ chmod +x reach.py
$ ./reach.py
以下は，ターゲット到達に成功したときのログ：
<![CDATA[searching target
right
front
left
target is not detected
GoPiGo turns left 120 degree
turn left
done
right
front
left
target detected !
GoPiGo turns left 45 degree
turn left
done
direction adjusting : -142.0
turn left
done
image is not refreshed
image is refreshed
direction adjusting : 36.0
turn right
done
image is not refreshed
image is refreshed
target is in front
apart from center：10.0
distance : 68cm
move forward
done
image is not refreshed
image is refreshed
target is in front
apart from center：20.0
distance : 48cm
move forward
done
image is not refreshed
image is refreshed
direction adjusting : 80.0
turn right
done
image is not refreshed
image is refreshed
target is in front
apart from center：6.0
distance : 34cm
move forward
done
target is in front
apart from center：-2.0
distance : 24cm
Arrived !
キー入力で, プログラム終了...]]>
|
