<jupyter_start><jupyter_text>Road following + stop when obstruct on the path on JetbotIn this notebook we will be combining both road following and obstruct avoidance, so that the jetbot could follwoing the lane and stop when it detected obstruct on its path.  To begin, let's execute the code below to transfer the device from CPU memory to the GPU device.<jupyter_code>import torch
device = torch.device('cuda')<jupyter_output><empty_output><jupyter_text>**Road Following**   - Upload the "*best_steering_model_xy.pth*" model file from road following notebook. The sample data (420 pictures) for this model were collect by myself, and neural network training by myself. **Collision Avoidance**             - Upload the "*best_model.pth*" model file from the jetbot wiki in github.  Load the TRT optimized models by executing the cell below<jupyter_code>import torch
from torch2trt import TRTModule

model_trt = TRTModule()
model_trt.load_state_dict(torch.load('best_steering_model_xy_trt.pth')) # sample collected and neural network training by myself

model_trt_collision = TRTModule()
model_trt_collision.load_state_dict(torch.load('best_model_trt.pth')) # sample collected and neural network training by myself<jupyter_output><empty_output><jupyter_text>Creating the Pre-Processing Function We have now loaded our models, but there's a slight issue. The format that we trained our models on doesn't exactly match the format of the camera. To do that, we need to do some preprocessing. This involves the following steps:1. Convert from HWC layout to CHW layout2. Normalize using same parameters as we did during training (our camera provides values in [0, 255] range and training loaded images in [0, 1] range so we need to scale by 255.03. Transfer the data from CPU memory to GPU memory4. Add a batch dimension<jupyter_code>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):
    image = PIL.Image.fromarray(image)
    image = transforms.functional.to_tensor(image).to(device).half()
    image.sub_(mean[:, None, None]).div_(std[:, None, None])
    return image[None, ...]<jupyter_output><empty_output><jupyter_text>Defined our pre-processing function which can convert images from the camera format to the neural network input format.Display our camera.<jupyter_code>from IPython.display import display
import ipywidgets
import traitlets
from jetbot import Camera, bgr8_to_jpeg

camera = Camera.instance(width=224, height=224, fps=10)
image_widget = ipywidgets.Image()

traitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=bgr8_to_jpeg)<jupyter_output><empty_output><jupyter_text>We'll also create our robot instance which we'll need to drive the motors.<jupyter_code>from jetbot import Robot

robot = Robot()<jupyter_output><empty_output><jupyter_text>Now, we will define some sliders to control the JetBot> Do reseach and experiment to find out the optimal seting base on my own envirement and my hardware specification, should start everything from 0.011. Speed Control slider: To start your JetBot increase ``speed_control_slider`` 2. Steering Gain slider: If you see your JetBot is woblling, you need to reduce ``steering_gain_slider`` till it is smooth3. Steering Bias slider: If you see your JetBot is biased towards extreme right or extreme left side of the track, you should control this slider till JetBot start following line or track in the center.  This accounts for motor biases as well as camera offsets4. Blocked slider: Display the probability in which there is an obstacle in the front of the Jetbot using the collision avoidance model5. Time for stop slider: To manually set the time for which the jetbot should remain stopped after an object has been removed6. Blocked threshold slider: To manually set the blocked threshold to stop the Jetbot after an object has been detected<jupyter_code># Road Following sliders
speed_control_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, description='speed control')
steering_gain_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, value=0.04, 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_control_slider, steering_gain_slider, steering_dgain_slider, steering_bias_slider)

# Collision Avoidance sliders
blocked_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, orientation='horizontal', description='blocked')
stopduration_slider= ipywidgets.IntSlider(min=1, max=1000, step=1, value=10, description='time for stop') 
blocked_threshold= ipywidgets.FloatSlider(min=0, max=1.0, step=0.01, value=0.8, description='blocked threshold')

display(image_widget)

display(ipywidgets.HBox([blocked_slider, blocked_threshold, stopduration_slider]))<jupyter_output><empty_output><jupyter_text>Place the jetbot on the track, the jetbot will start to move on the follwing stepsCreate a function that will get called whenever the camera's value changes. This function will do the following steps1. Pre-process the camera image2. Execute the neural network models for Road following and Collision Avoidance3. Check an if statements which would allow the Jetbot to perform road following and stop whenever an obstacle has been detected 4. Compute the approximate steering value5. Control the motors using proportional / derivative control (PD)<jupyter_code>import math

angle = 0.0
angle_last = 0.0
count_stops = 0
go_on = 1
stop_time = 10 # The number of frames to remain stopped
x = 0.0
y = 0.0
speed_value = speed_control_slider.value

def execute(change):
    global angle, angle_last, blocked_slider, robot, count_stops, stop_time, go_on, x, y, blocked_threshold
    global speed_value, steer_gain, steer_dgain, steer_bias
                
    steer_gain = steering_gain_slider.value
    steer_dgain = steering_dgain_slider.value
    steer_bias = steering_bias_slider.value
       
    image_preproc = preprocess(change['new']).to(device)
     
    #Collision Avoidance model:
    
    prob_blocked = float(F.softmax(model_trt_collision(image_preproc), dim=1).flatten()[0])
    
    blocked_slider.value = prob_blocked    
    stop_time=stopduration_slider.value
    
    if go_on == 1:    
        if prob_blocked > blocked_threshold.value: # threshold should be above 0.5
            count_stops += 1
            go_on = 2
        else:
            #start of road following detection
            go_on = 1
            count_stops = 0
            xy = model_trt(image_preproc).detach().float().cpu().numpy().flatten()        
            x = xy[0]            
            y = (0.5 - xy[1]) / 2.0
            speed_value = speed_control_slider.value
    else:
        count_stops += 1
        if count_stops < stop_time:
            x = 0.0 #set x steering to zero
            y = 0.0 #set y steering to zero
            speed_value = 0 # set speed to zero (can set to turn as well)
        else:
            go_on = 1
            count_stops = 0
            
    
    angle = math.atan2(x, y)        
    pid = angle * steer_gain + (angle - angle_last) * steer_dgain
    steer_val = pid + steer_bias 
    angle_last = angle
    robot.left_motor.value = max(min(speed_value + steer_val, 1.0), 0.0)
    robot.right_motor.value = max(min(speed_value - steer_val, 1.0), 0.0) 

execute({'new': camera.value})<jupyter_output><empty_output><jupyter_text>Attach neural network execution function to the camera for processing.We accomplish that with the observe function.<jupyter_code>camera.observe(execute, names='value')<jupyter_output><empty_output><jupyter_text>If you want to stop this behavior, you can unattach this callback by executing the code below.<jupyter_code>import time

camera.unobserve(execute, names='value')

time.sleep(0.1)  # add a small sleep to make sure frames have finished processing

robot.stop()<jupyter_output><empty_output><jupyter_text>Turn off the camera  properly for reuse<jupyter_code>camera.stop()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.08130277442702051, 'fraction_numerical': 0.020386007237635707, 'mean_word_length': 4.472607260726073, 'pattern_counts': {'":': 0, '<': 42, '<?xml version=': 0, '>': 43, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '10', 'dir': 'jupyter-structured-clean-dedup', 'id': '84523', 'license': 'MIT', 'n_tokens_mistral': 2585, 'n_tokens_neox': 2450, 'n_words': 855, 'path': 'JetbotFeatures/Line_Follow_Stop_Go/Line_follow_stop_go.ipynb', 'repo_name': 'strongbrain001/2021SummerDGMD-S-17-finalProject', 'size': '8290'}