衝突回避プログラムを単純な道路標識に改造しラインフォローと組み合わせ。
道路標識は左折と右折の2つを用意しました。
JETBOT付属のJetcamだとCNNがnum_classes=3で上手く動かず手間取りましたが、
結局GITHUBのNVIDIAからJetcamをインストールして解決。
動画はこちらです。
https://www.youtube.com/watch?v=-lz1_Bln0bA
https://github.com/NVIDIA-AI-IOT/jetcam
jetcamのsetupに従うとフォルダが追加されました。
initialize the PyTorch model（変更なし）
live_demo.ipynb
import torch
import torchvision
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from PIL import Image
import numpy as np
Define network（CNNモデルはステアリング；miniNet2、道路標識；miniNet3）
live_demo.ipynb
class miniNet2(nn.Module):
def __init__(self, num_classes=2):
super(miniNet2, self).__init__()
# 特徴抽出用の部品
self.conv1 = nn.Conv2d(3, 6, 3)
self.pool1 = nn.MaxPool2d(2, 2)
self.conv2 = nn.Conv2d(6, 16, 3)
self.pool2 = nn.MaxPool2d(2, 2)
self.conv3 = nn.Conv2d(16, 32, 3)
self.pool3 = nn.MaxPool2d(2, 2)
# ニューラルネットワーク用の部品
self.fc1 = nn.Linear(4160, 1024)
#self.fc1 = nn.Linear(61440, 1024)
#self.fc1 = nn.Linear(32 * 26 * 26, 1024)
self.fc2 = nn.Linear(1024, 512)
self.fc3 = nn.Linear(512, num_classes)
def forward(self, x):
x = F.relu(self.conv1(x))
x = self.pool1(x)
x = F.relu(self.conv2(x))
x = self.pool2(x)
#print(x.shape)
x = F.relu(self.conv3(x))
x = self.pool3(x)
# 行列をベクトルに変換
#x = x.view(-1, 32 * 26 * 26)
#x = x.view(-1, 61440)
x = x.view(-1, 4160)
x = F.relu(self.fc1(x))
x = F.relu(self.fc2(x))
x = self.fc3(x)
return x
class miniNet3(nn.Module):
def __init__(self, num_classes=3):
super(miniNet3, self).__init__()
# 特徴抽出用の部品
self.conv1 = nn.Conv2d(3, 6, 3)
self.pool1 = nn.MaxPool2d(2, 2)
self.conv2 = nn.Conv2d(6, 16, 3)
self.pool2 = nn.MaxPool2d(2, 2)
self.conv3 = nn.Conv2d(16, 32, 3)
self.pool3 = nn.MaxPool2d(2, 2)
# ニューラルネットワーク用の部品
self.fc1 = nn.Linear(4160, 1024)
#self.fc1 = nn.Linear(61440, 1024)
#self.fc1 = nn.Linear(32 * 26 * 26, 1024)
self.fc2 = nn.Linear(1024, 512)
self.fc3 = nn.Linear(512, num_classes)
def forward(self, x):
x = F.relu(self.conv1(x))
x = self.pool1(x)
x = F.relu(self.conv2(x))
x = self.pool2(x)
#print(x.shape)
x = F.relu(self.conv3(x))
x = self.pool3(x)
# 行列をベクトルに変換
#x = x.view(-1, 32 * 26 * 26)
#x = x.view(-1, 61440)
x = x.view(-1, 4160)
x = F.relu(self.fc1(x))
x = F.relu(self.fc2(x))
x = self.fc3(x)
return x
Load model（変更あり、モデルを2つ読み込みます）
live_demo.ipynb
Steer_net = miniNet2()
Steer_net.load_state_dict(torch.load('best_steering_model_xy.pth'))
RL3_net = miniNet3()
RL3_net.load_state_dict(torch.load('model_RL3a.pth'))
transfer to the GPU device（変更あり、モデルが2つです）
live_demo.ipynb
device = torch.device('cuda')
Steer_net.to(device)
Steer_net = Steer_net.eval().half()
RL3_net.to(device)
RL3_net = RL3_net.eval().half()
some preprocessing（修正なし）
live_demo.ipynb
import torchvision.transforms as transforms
import torch.nn.functional as F
import cv2
import PIL.Image
import numpy as np
mean = torch.Tensor([0.485, 0.456, 0.406]).cuda().half()
std = torch.Tensor([0.229, 0.224, 0.225]).cuda().half()
def preprocess(image):
image2 = np.copy(image) # trimmed
image2 = image2[168:224, 0:224] # trimmed
#image2 = cv2.rectangle(image2, (0, 0), (224, 168), (0, 0, 0), thickness=-1)
image = PIL.Image.fromarray(image2)
image = transforms.functional.to_tensor(image).to(device).half()
image.sub_(mean[:, None, None]).div_(std[:, None, None])
return image[None, ...]
CSICameraとbgr8_to_jpegのimport元を変えています。
live_demo.ipynb
from IPython.display import display
import ipywidgets
import traitlets
# from jetbot import Camera, bgr8_to_jpeg ###
from jetcam.csi_camera import CSICamera ###
from jetcam.utils import bgr8_to_jpeg ###
camera = CSICamera(width=224, height=224) ###
camera.running = True ###
# camera = Camera()
image_widget = ipywidgets.Image()
def display_xy2(camera_image): # trimmed
image1 = np.copy(camera_image) # trimmed
image1 = image1[168:224, 0:224] #trimmed
#image1 = cv2.rectangle(image1, (0, 0), (224, 112), (0, 0, 0), thickness=-1)
jpeg_image1 = bgr8_to_jpeg(image1)
return jpeg_image1
# traitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=bgr8_to_jpeg)
camera_link = traitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=display_xy2) # trimmed
display(image_widget)
create our robot instance（修正なし）
live_demo.ipynb
from jetbot import Robot
robot = Robot()
define sliders to control JetBot（修正なし）
live_demo.ipynb
speed_gain_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, value=0.16, description='speed gain')
steering_gain_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, value=0.05, description='steering gain')
steering_dgain_slider = ipywidgets.FloatSlider(min=0.0, max=0.5, step=0.001, value=0.0, description='steering kd')
steering_bias_slider = ipywidgets.FloatSlider(min=-0.3, max=0.3, step=0.01, value=0.0, description='steering bias')
display(speed_gain_slider, steering_gain_slider, steering_dgain_slider, steering_bias_slider)
display some sliders（道路標識の読み取り部分を追加してます）
live_demo.ipynb
x_slider = ipywidgets.FloatSlider(min=-1.0, max=1.0, description='x')
y_slider = ipywidgets.FloatSlider(min=0, max=1.0, orientation='vertical', description='y')
index0_slider = ipywidgets.FloatSlider(min=0, max=4.0, orientation='vertical', description='index0')
index1_slider = ipywidgets.FloatSlider(min=0, max=4.0, orientation='vertical', description='index1')
steering_slider = ipywidgets.FloatSlider(min=-1.0, max=1.0, description='steering')
speed_slider = ipywidgets.FloatSlider(min=0, max=1.0, orientation='vertical', description='speed')
display(ipywidgets.HBox([y_slider, speed_slider, index0_slider,index1_slider]))
display(x_slider, steering_slider)
create a function(camera's value changes)（変更あり）
live_demo.ipynb
import time
angle = 0.0
angle_last = 0.0
def execute(change):
global angle, angle_last
image = change['new']
#image2 = np.copy(image) # trimmed
#image2 = cv2.rectangle(image2, (0, 0), (224, 168), (0, 0, 0), thickness=-1) # trimmed
#image2 = image2[168:224, 0:224] #trimmed
#xy = model(preprocess(image)).detach().float().cpu().numpy().flatten() # trimmed
image1 = preprocess(image)
#xy = Steer_net(preprocess(image)).detach().float().cpu().numpy().flatten()
xy = Steer_net(image1).detach().float().cpu().numpy().flatten()
x = xy[0]
y = (0.5 - xy[1]) / 2.0
x_slider.value = x
y_slider.value = y
speed_slider.value = speed_gain_slider.value
angle = np.arctan2(x, y)
pid = angle * steering_gain_slider.value + (angle - angle_last) * steering_dgain_slider.value
angle_last = angle
steering_slider.value = pid + steering_bias_slider.value
#global blocked_slider, robot
y1 = RL3_net(image1)
# we apply the `softmax` function to normalize the output vector so it sums to 1 (which makes it a probability distribution)
#y1 = F.softmax(y1, dim=1)
output0 = F.softmax(y1, dim=0).detach().cpu().numpy().flatten()
output = F.softmax(y1, dim=1).detach().cpu().numpy().flatten()
category_index = float(output.argmax())
index0_slider.value = float(output0.argmax())
index1_slider.value = category_index
if category_index == 2:
robot.right(0.2)
elif category_index == 1:
robot.left(0.2)
else:
robot.left_motor.value = max(min(speed_slider.value + steering_slider.value, 1.0), 0.025)
robot.right_motor.value = max(min(speed_slider.value - steering_slider.value, 1.0), 0.025)
time.sleep(0.001)
execute({'new': camera.value})
camera for processing（変更なし）
live_demo.ipynb
camera.observe(execute, names='value')
ビデオのストリーミングを中止しプログラム負荷を減らす。（追加）
live_demo.ipynb
camera_link.unlink() # don't stream to browser (will still run camera)
道路標識を優先し、標識が見えない時は道路に沿って走ります。