Jetbot、ラインフォローと単純な道路標識

衝突回避プログラムを単純な道路標識に改造しラインフォローと組み合わせ。
道路標識は左折と右折の2つを用意しました。
JETBOT付属のJetcamだとCNNがnum_classes=3で上手く動かず手間取りましたが、
結局GITHUBのNVIDIAからJetcamをインストールして解決。
動画はこちらです。
https://www.youtube.com/watch?v=-lz1_Bln0bA
![you.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/617683/ad229c8a-4a55-d17b-ba20-a7db1030f4b2.png)

https://github.com/NVIDIA-AI-IOT/jetcam
jetcamのsetupに従うとフォルダが追加されました。
![Jetcam.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/617683/5d178e3d-30c6-655f-4b79-8d977545d129.png)
initialize the PyTorch model（変更なし）

```ruby: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）

```ruby: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つ読み込みます）

```ruby: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つです）

```ruby: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（修正なし）

```ruby: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元を変えています。

```ruby: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（修正なし）

```ruby:live_demo.ipynb
from jetbot import Robot
robot = Robot()
```

define sliders to control JetBot（修正なし）

```ruby: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（道路標識の読み取り部分を追加してます）

```ruby: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)（変更あり）

```ruby: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（変更なし）

```ruby:live_demo.ipynb
camera.observe(execute, names='value')
```

ビデオのストリーミングを中止しプログラム負荷を減らす。（追加）

```ruby:live_demo.ipynb
camera_link.unlink()  # don't stream to browser (will still run camera)
```
道路標識を優先し、標識が見えない時は道路に沿って走ります。
#よかったら、LGTMお願いします!