<jupyter_start><jupyter_text>Object Following - Live Demo Requires installation of Torch2TRT module as described in the Jetbot repositoryIn this notebook we'll show how you can follow an object with JetBot!  We'll use a pre-trained neural networkthat was trained on the [COCO dataset](http://cocodataset.org) to detect 90 different common objects.  These include* Person (index 1)* Cup (index 47)* bottle (index 44)and many others (you can check [this file](https://github.com/tensorflow/models/blob/master/research/object_detection/data/mscoco_complete_label_map.pbtxt) for a full list of class indices).  The model is sourced from the [TensorFlow object detection API](https://github.com/tensorflow/models/tree/master/research/object_detection),which provides utilities for training object detectors for custom tasks also!  Once the model is trained, we optimize it using NVIDIA TensorRT on the Jetson Nano.This makes the network very fast, capable of real-time execution on Jetson Nano!  We won't run through all of the training and optimization steps in this notebook though.Anyways, let's get started.  First, we'll want to import the ``ObjectDetector`` class which takes our pre-trained SSD engine.  Compute detections on single camera image<jupyter_code>from jetbot import ObjectDetector

model = ObjectDetector('ssd_mobilenet_v2_coco.engine')<jupyter_output><empty_output><jupyter_text>Internally, the ``ObjectDetector`` class uses the TensorRT Python API to execute the engine that we provide.  It also takes care of preprocessing the input to the neural network, aswell as parsing the detected objects.  Right now it will only work for engines created using the ``jetbot.ssd_tensorrt`` package. That package has the utilities for convertingthe model from the TensorFlow object detection API to an optimized TensorRT engine.Next, let's initialize our camera.  Our detector takes 300x300 pixel input, so we'll set this when creating the camera.> Internally, the Camera class uses GStreamer to take advantage of Jetson Nano's Image Signal Processor (ISP).  This is super fast and offloads> a lot of the resizing computation from the CPU.<jupyter_code>import ipywidgets.widgets as widgets
from jetbot import Camera, bgr8_to_jpeg

camera = Camera.instance(width=224, height=224)
image = widgets.Image(format='jpeg', width=224, height=224)<jupyter_output><empty_output><jupyter_text>Now, let's execute our network using some camera input.  By default the ``ObjectDetector`` class expects ``bgr8`` format that the camera produces.  However,you could override the default pre-processing function if your input is in a different format.<jupyter_code>detections = model(camera.value)

print(detections)<jupyter_output><empty_output><jupyter_text>If there are any COCO objects in the camera's field of view, they should now be stored in the ``detections`` variable.  Display detections in text areaWe'll use the code below to print out the detected objects.<jupyter_code>from IPython.display import display

detections_widget = widgets.Textarea()
detections_widget.value = str(detections)<jupyter_output><empty_output><jupyter_text>You should see the label, confidence, and bounding box of each object detected in each image.  There's only one image (our camera) in this example. To print just the first object detected in the first image, we could call the following> This may throw an error if no objects are detected<jupyter_code>image_number = 0
object_number = 0

print(detections[image_number][object_number])
#If error then no object has been detected, run detections=model cell again<jupyter_output><empty_output><jupyter_text>Control robot to follow central objectNow we want our robot to follow an object of the specified class.  To do this we'll do the following1.  Detect objects matching the specified class2.  Select object closest to center of camera's field of vision, this is the 'target' object3.  Steer robot towards target object, otherwise wander4.  If we're blocked by an obstacle, turn leftWe'll also create some widgets that we'll use to control the target object label, the robot speed, anda "turn gain", that will control how fast the robot turns based off the distance between the target objectand the center of the robot's field of view. First, let's load our collision detection model.  The pre-trained model is stored in this directory as a convenience, but if you followedthe collision avoidance example you may want to use that model if it's better tuned for your robot's environment.<jupyter_code>import torch
device = torch.device('cuda')
import torchvision
import torchvision.transforms as transforms
import torch.nn.functional as F
import cv2
import numpy as np
#NEW-----------------------------------
from torch2trt import TRTModule
import PIL.Image


#change to TRT model 
model_trt = TRTModule()
model_trt.load_state_dict(torch.load('../collision_avoidance/best_model_trt.pth'))

#old model ------------------
"""
collision_model = torchvision.models.alexnet(pretrained=False)
collision_model.classifier[6] = torch.nn.Linear(collision_model.classifier[6].in_features, 2)
collision_model.load_state_dict(torch.load('../collision_avoidance/best_model.pth'))
device = torch.device('cuda')
collision_model = collision_model.to(device)
mean = 255.0 * np.array([0.485, 0.456, 0.406])
stdev = 255.0 * np.array([0.229, 0.224, 0.225])
normalize = torchvision.transforms.Normalize(mean, stdev)

def preprocess(camera_value):
    global device, normalize
    x = camera_value
    x = cv2.resize(x, (224, 224))
    x = cv2.cvtColor(x, cv2.COLOR_BGR2RGB)
    x = x.transpose((2, 0, 1))
    x = torch.from_numpy(x).float()
    x = normalize(x)
    x = x.to(device)
    x = x[None, ...]
    return x
"""
#old model ------------------end

mean = torch.Tensor([0.485, 0.456, 0.406]).cuda().half()
std = torch.Tensor([0.229, 0.224, 0.225]).cuda().half()

normalize = torchvision.transforms.Normalize(mean, std)

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>Great, now let's initialize our robot so we can control the motors.<jupyter_code>from jetbot import Robot
import math

robot = Robot()<jupyter_output><empty_output><jupyter_text>Finally, let's display all the control widgets and connect the network execution function to the camera updates.<jupyter_code>def steering(x, y): 
    #script from stackexchange of user <NAME> 
    #https://electronics.stackexchange.com/questions/19669/algorithm-for-mixing-2-axis-analog-input-to-control-a-differential-motor-drive
    # convert to polar
    r = math.hypot(x, y)
    t = math.atan2(y, x)

    # rotate by 45 degrees
    t += math.pi / -4.0

    # back to cartesian
    left = r * math.cos(t)
    right = r * math.sin(t)

    # rescale the new coords
    left = left * math.sqrt(2)
    right = right * math.sqrt(2)

    # clamp to -1/+1 useing the speed widget slider Max 1 but movement at 0.2
    scalefactor= speed_widget.value
    left = max(scalefactor*-1.0, min(left, scalefactor))
    right = max(scalefactor*-1.0, min(right, scalefactor))
    
    #gamma correction for response sensitivity of joystick or center_x value while turning : TB
    gamma=turn_gain_widget.value  #using slider for joystick 1-4, for object recognition 2-40  
    if left <0.0 :
        left= -1.0* (((abs(left)/scalefactor)**(1.0/gamma))*scalefactor)
    else:
        left= ((abs(left)/scalefactor)**(1.0/gamma))*scalefactor
       
    if right <0.0:
        right= -1.0*(((abs(right)/scalefactor)**(1.0/gamma))*scalefactor)
    else:
        right= ((abs(right)/scalefactor)**(1.0/gamma))*scalefactor
            
    return left, right
#cell T1
from jetbot import bgr8_to_jpeg
import traitlets
import IPython
import time
#import random

global i, ii, oldcenter,w,countnoobject

blocked_widget = widgets.FloatSlider(min=0.0, max=1.0, value=0.0, description='blocked')
image_widget = widgets.Image(format='jpeg', width=224, height=224)
label_widget = widgets.IntText(value=1, description='tracked label')

#chose an item: water bottle label 44: click on the very right of the box to activate label number
collisionchoice_widget=widgets.IntText(value= 1, description='ColA.N=0Y=1)')
#0 no collison avoidance: will stop, can be used to test object targeting without collison avoidance interference
#1 will include search mode with collison avoidance, need to click 

speed_widget = widgets.FloatSlider(value=0.3, min=0.05, max=1.0, step=0.001, description='speed')
#TB higher speed requires smaller turn_gain values: 2.5 for speed 0.22, around 2 for speed 0.4
turn_gain_widget = widgets.FloatSlider(value=2.50, min=0.50, step=0.001, max=140.0, description='turn sensitivity')
#TB value different for different forward speed, but very small differences
motoradjustment_widget = widgets.FloatSlider(value=0.04, min=0.00, max=0.2, step=0.0001, description='motoradjustment')

#TB object_stop_threshold-----------------------------------------------------------------------
stop_threshold = widgets.FloatSlider(min=-0.2, max=1.2, value=0.9, step=0.01, description='object_stop_threshold')
stopduration_slider= widgets.IntSlider(min=1, max=1000, step=1, value=40, description='Manu. time stop') #anti-collision stop time ~ frames*0.1s if 10 Frames per seconds
block_threshold= widgets.FloatSlider(min=0, max=1.2, step=0.01, value=0.90, description='Manu. bloc. threshold') #anti-collision block probability

#widgets for visualization of some values, from data_collection TB
x=0.0
i=0.0
ii=00.0
w=0.0
count_stops=0
countnoobjects=0.0
oldcenter=0.0
go_on=1
x=0
y=0
button_layout = widgets.Layout(width='240px', height='64px') #TB

centerY = widgets.FloatText(layout=button_layout, value=x, description='centerY') #TB
centerX = widgets.FloatText(layout=button_layout, value=x, description='centerX') #TB
value1 = widgets.FloatText(layout=button_layout, value=x, description='x') #TB
value2 = widgets.FloatText(layout=button_layout, value=x, description='stop bottom') #TB
lostImages = widgets.FloatText(layout=button_layout, value=x, description='lost Object') #TB
countframes= widgets.FloatText(layout=button_layout, value=x, description='Nr. No-Object (Search>500)') #TB
motorleft = widgets.FloatText(layout=button_layout, value=x, description="left motor") #TB
motorright = widgets.FloatText(layout=button_layout, value=x, description="right motor") #TB

d2 = IPython.display.display("", display_id=2)

display(widgets.VBox([
    widgets.HBox([image_widget, blocked_widget,block_threshold]),
    widgets.HBox([widgets.Label(value="Track Label Number e.g. 44 for bottle....................................Activate Collision Avoidance 0: No, 1: Yes")]),
    widgets.HBox([label_widget, collisionchoice_widget]),
    speed_widget,
    turn_gain_widget,
    motoradjustment_widget,
    widgets.HBox([stop_threshold,stopduration_slider])
]))


#Display widgets from cell T1
display(widgets.HBox([centerY, centerX, value1, value2]))
display(widgets.HBox([motorleft, motorright, lostImages, countframes]))

width = int(image_widget.width)
height = int(image_widget.height)

def detection_center(detection):
    """Computes the center x, y coordinates of the object"""
    bbox = detection['bbox']
    center_x = (bbox[0] + bbox[2]) / 2.0 - 0.5
    center_y = (bbox[1] + bbox[3]) / 2.0 - 0.5
    botton=bbox[3]
    centerY.value=center_y
    centerX.value=center_x
    return (center_x, center_y, botton)
    
def norm(vec):
    """Computes the length of the 2D vector"""
    return np.sqrt(vec[0]**2 + vec[1]**2)

def closest_detection(detections):
    """Finds the detection closest to the image center"""
    closest_detection = None
    for det in detections:
        center = detection_center(det)
        if closest_detection is None:
            closest_detection = det
        elif norm(detection_center(det)) < norm(detection_center(closest_detection)):
            closest_detection = det
    return closest_detection

def collisionavoidance(image):
    # execute collision model to determine if blocked   
   
    #TRT model 
    collision_output = model_trt(preprocess(image))
    #new
    # we apply the `softmax` function to normalize the output vector so it sums to 1 (which makes it a probability distribution)
    collision_output= F.softmax(collision_output, dim=1)
    prob_blocked = float(collision_output.flatten()[0])
    blocked_widget.value = prob_blocked
    
    # turn left if blocked
    if prob_blocked > block_threshold.value:
        robot.left(0.3) 
        motorright.value=0.3
        motorleft.value=0
    else:
        y=speed_widget.value
        leftnew, rightnew= steering(0,y) #exchange left right in case for wrong-side steering
        motorright.value= round(rightnew, 2) #exchange left right in case for wrong-side steering
        motorleft.value= round(leftnew+motoradjustment_widget.value, 2)
        #motoradjustment value important to keep bot driving straight, small offset-values like 0.05
        robot.right_motor.value=motorright.value
        robot.left_motor.value=motorleft.value
        
    return  
def execute(change):
    global i,ii,oldcenter,w, countnoobjects,stop_threshold,stopduration_slider, count_stops, go_on,x,y
    
    t1 = time.time()
    stop_time=stopduration_slider.value
    image = change['new']
            
    # compute all detected objects
    detections = model(image)
    
    # draw all detections on image
    for det in detections[0]:
        bbox = det['bbox']
        cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (255, 0, 0), 2)
    
    # select detections that match selected class label
    matching_detections = [d for d in detections[0] if d['label'] == int(label_widget.value)]
    
    #get detection closest to center of field of view and draw it
    #here start to check for previous postive detections? TB to avoid object skipping runave 4
    det = closest_detection(matching_detections)
    if det is not None:
        bbox = det['bbox']
        cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (0, 255, 0), 5)
      
    # otherwise go stop or go forward if no target detected
    #TB decide Object is lost or just not detected ii: needs to be adjusted
    if det is None:
        if i>=1.0 and 1.0<=ii<=8.0:
            w+=1.0 #if only few images without objects, just stop and wait for object appears
            lostImages.value=w
            if ii>1:                
                motorright.value=0.0
                motorleft.value= 0.0
                robot.left_motor.value=0.0
                robot.right_motor.value=0.0
            countnoobjects=0.0
        else:    
            i=0.0
            ii=0.0
            w=0.0
            countnoobjects+=1.0
            lostImages.value=w
            countframes.value=countnoobjects
            #activation of search mode and using collision_avoidance trained base
            if int(collisionchoice_widget.value)==1:
                if countnoobjects>100: #number of frames/time to wait before going into search mode Object detection about 14 FPS
                    collisionavoidance(image)
            else:
                robot.forward(float(0)) 
                
        ii+=float(1.0)
                    
    # otherwsie steer towards target
    else:
        # move robot forward and steer proportional target's x-distance from center
        if w>=1.0 :
            i+=1.0 #to count the object event per image
            ii=1.0
        else:
            i+=1.0
            
        center = detection_center(det)
        
        #------add stop before reaching object
        stop_before_object=center[2]
        value2.value=stop_before_object #show box center value, adjusted
        if go_on==1:      
            if stop_before_object>=stop_threshold.value:
                y=0.0 #set speed zero
                x=0.0
                count_stops +=1
                go_on=2
            else:
                go_on=1
                count_stops=0
                y=speed_widget.value
                x=center[0]/4.0 #TB reduce the values for x coordinates to make steering less sensitive
        else:
            count_stops += 1
            if count_stops<stop_time:
                y=0 #speed
                x=0
            else:
                go_on=1
                count_stops=0
                y=speed_widget.value
      
    
        #------
        value1.value=x #show box botton value, 
        leftnew, rightnew= steering(x,y) #exchange left right in case for wrong-side steering
        motorright.value= round(rightnew, 2) #exchange left right in case for wrong-side steering
        motorleft.value= round(leftnew+motoradjustment_widget.value, 2)
        #motoradjustment value important to keep bot driving straight, small offset-values like 0.05
        robot.right_motor.value=motorright.value
        robot.left_motor.value=motorleft.value     
          
    
    # update image widget   
    image_widget.value = bgr8_to_jpeg(image)
    #timer
    t2 = time.time()
    s = f"""{int(1/(t2-t1))} FPSS"""
    d2.update(IPython.display.HTML(s) )
    
execute({'new': camera.value})<jupyter_output><empty_output><jupyter_text>Call the block below to connect the execute function to each camera frame update.<jupyter_code>camera.unobserve_all()
camera.observe(execute, names='value')<jupyter_output><empty_output><jupyter_text>Awesome!  If the robot is not blocked you should see boxes drawn around the detected objects in blue.  The target object (which the robot follows) will be displayed in green.The robot should steer towards the target when it is detected.  If it is blocked by an object it will simply turn left.You can call the code block below to manually disconnect the processing from the camera and stop the robot.<jupyter_code>import time

camera.unobserve_all()
time.sleep(1.0)
robot.stop()
camera_link.unlink()  # TB 12072020 don't stream to browser (will still run camera)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.10480948720764248, 'fraction_numerical': 0.023553310640166903, 'mean_word_length': 3.5164889660302503, 'pattern_counts': {'":': 0, '<': 48, '<?xml version=': 0, '>': 52, 'https://': 3, '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': '286561', 'license': 'MIT', 'n_tokens_mistral': 5777, 'n_tokens_neox': 5404, 'n_words': 1718, 'path': '.ipynb_checkpoints/TRT_object_following_tweak_object_stop-checkpoint.ipynb', 'repo_name': 'tomMEM/Jetbot-Project', 'size': '18221'}