Autonomes Fahren

from lidarread import read_lidar_data
import ThunderBorg3 as ThunderBorg
import time
import numpy as np
import cv2 as cv
import os.path
from datetime import datetime
#from time import *
from time import sleep


distance = 0
frame_class = []
move_class = []

TEST = True 
MOVTEST = False


def set_up_Camera():
    
    global cap, out
    cap = cv.VideoCapture(0)
    if not cap.isOpened():
        print("cannot open camera !!")
        exit()
    fourcc = cv.VideoWriter_fourcc(*'XVID')
    out = cv.VideoWriter('lidarself4.avi', fourcc, 20.0, (640, 480))
        
def set_up_Thunder():
    global TB, seq
    # Setup the ThunderBorg
    TB = ThunderBorg.ThunderBorg()
    #TB.i2cAddress = 0x15                  # Uncomment and change the value if you have changed the board address
    TB.Init()
    if not TB.foundChip:
        boards = ThunderBorg.ScanForThunderBorg()
        if len(boards) == 0:
            print ('No ThunderBorg found, check you are attached :)')
        else:
            pass
        sys.exit()
    TB.SetCommsFailsafe(False)             # Disable the communications failsafe


    voltageIn = 12.0                        # Total battery voltage to the ThunderBorg
    voltageOut= 12.0 * 0.60               # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power

    # Setup the power limits
    if voltageOut > voltageIn:
        maxPower = 1.0
    else:
        maxPower = voltageOut / float(voltageIn)
        maxPower_start = maxPower

    # Set sequence,pairs of motor1 and2 drive levels
    power = 0.4
    seq = [
            [+power,    +power],
            [+power*2, -power*2],
            [+power*1.5,    0.0],
            [-power,    0.0]
            ]
def get_frame():
    cap = cv.VideoCapture(0)
    if not cap.isOpened():
        print("cannot open camera !!")
        exit()
    # Capture frame-by-frame
    ret, frame = cap.read()
    frame = cv.flip(frame,-1)
    # Display the resulting frame
    #cv.imshow('frame',frame)
    #cv.waitKey(0)
    #time.sleep(2)
    cv.destroyAllWindows()
    return frame

def add_title_to_frame(image,text):
    font = cv.FONT_HERSHEY_DUPLEX
    color = (0, 0, 255) # red
    fontsize = .8
    position = (20, 20)
    print(text)
    img_txt = cv.putText(image, text, position, font,
                         fontsize, color=color, thickness=1)
        
    return img_txt

def show_image(image, text):
    image = add_title_to_frame(image,text)
    cv.imshow('image',image)
    cv.waitKey(1000)
    cv.destroyAllWindows()


def generate_filename(label_id):
            class_dir = label_id
            timestamp = datetime.now()
            filename = "PI_CAM_" + timestamp.strftime(
                "%Y%m%d_%H%M%S%f") + '.png'
            
            print('filename in generate_filename:', filename )
            
            return os.path.join('captures', str(class_dir), filename)
        
def save_frame(filename, frame):
    """
    Saves an image to a specified location.

    Args:
      filename (str): The path where you'd like to save the image.
      frame: The bitmap image to save.
    """
    os.makedirs(os.path.dirname(filename), exist_ok=True)
    cv.imwrite(filename, frame)
    
    
    
def classification(distance):    
    if distance < 10:
        move_class = "stop"
        frame_class = move_class
    elif distance < 50:
        move_class = "spin"
        frame_class = move_class
    else:
        move_class = "forward"
        frame_class = move_class
    return frame_class, move_class 
           

def get_sensor_data():
    a = read_lidar_data()
    if TEST: print(a[0])
    return a[0]

def stop():
    TB.MotorsOff()
    exit()

def forward():
    if TEST: print('FORWARD')
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[0][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[0][1])
    time.sleep(.50)
    TB.MotorsOff()

def spin():
    if TEST: print('SPIN')    
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[1][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[1][1])
    time.sleep(.2)
    TB.MotorsOff()
    time.sleep(1)

def curve():
    if TEST: print('CURVE')    
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[2][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[2][1])
    

def move_monster(action):
    if TEST: print(action)
    if action == "stop": stop()
    elif action == "forward": forward()
    elif action == "spin": spin()
    elif action == "curve": curve()

set_up_Thunder()

for i in range(75):
    distance = get_sensor_data()
    print('distance:', distance)     
    frame_class, move_class = classification(distance) 
    print('frame_class',frame_class)
    print('move_class',move_class)
    image = get_frame()
    text = 'frame class ' + str(frame_class)
    show_image(image, text)    
    
    filename = generate_filename(frame_class)
    save_frame(filename, image)
    print('Saved: %s' % filename)


    if MOVTEST: move_monster(move_class)
    time.sleep(1)
    
stop()
cap.release()

# Copyright 2021 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Trains a new image classification model using local images.

This script is designed for a three-step workflow with two other scripts:

1. Capture some images to train your model:

    python3 collect_images.py -l my-labels.txt

2. Train the model with those images, using this script (by default, this script
looks for images in the "captures" directory, which is where the
collect_images.py script saves them by default):

    python3 train_images.py -l my-labels.txt

3. And then run the model (my-model.tflite is the default name given to the
retrained model but you can change it by passing the `--out_model` flag to
the train_models.py script):

    python3 classify_video.py -m my-model.tflite

NOTE: The retrained model is specifically built to run with acceleration on the
Coral Edge TPU, so the model cannot run if your system does not have the
Coral USB Accelerator or another Edge TPU attached.

For information about the script options, run:

    python3 train_images.py --help

For more instructions, see g.co/aiy/maker
"""

import argparse
import os

from PIL import Image

from pycoral.adapters import classify
from pycoral.adapters import common
from pycoral.learn.imprinting.engine import ImprintingEngine
from pycoral.utils.edgetpu import make_interpreter
from pycoral.utils.dataset import read_label_file
import models


def read_image(path, shape):
    with Image.open(path) as img:
        return img.convert('RGB').resize(shape, Image.NEAREST)


def train(capture_dir, labels, model, out_model):
    engine = ImprintingEngine(model, keep_classes=False)
    extractor = make_interpreter(engine.serialize_extractor_model(),
                                 device=':0')
    extractor.allocate_tensors()

    for class_id in sorted(labels):
        class_name = labels[class_id]
        print('\nClass: %s (id=%d)' % (class_name, class_id))
        class_capture_dir = os.path.join(capture_dir, class_name)
        for img in os.listdir(class_capture_dir):
            imgpath = os.path.join(class_capture_dir, img)
            common.set_input(extractor,
                             read_image(imgpath, common.input_size(extractor)))
            extractor.invoke()
            embedding = classify.get_scores(extractor)
            print('  %s => %s' % (imgpath, embedding))
            engine.train(embedding, class_id)

    with open(out_model, 'wb') as f:
        f.write(engine.serialize_model())
    print('\nTrained model was saved to %s' % out_model)


def main():
    parser = argparse.ArgumentParser(
        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument('--labels', '-l', type=str, required=True,
                        help='Labels file from your training dataset')
    parser.add_argument('--capture_dir', '-d', type=str,
                        default='captures',
                        help='Captures directory with your training images')
    parser.add_argument('--model', '-m', type=str,
                        default=models.CLASSIFICATION_IMPRINTING_MODEL,
                        help='Base model that will be retrained')
    parser.add_argument('--out_model', '-om', type=str,
                        default='my-model.tflite',
                        help='Output filename for the retrained model')
    args = parser.parse_args()

    labels = read_label_file(args.labels)
    train(args.capture_dir, labels, args.model, args.out_model)


if __name__ == '__main__':
    main()

# Copyright 2021 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Performs continuous image classification with the camera video.

To classify things using a default MobileNet model, simply run the script:

    python3 classify_video.py

Or classify using your own model and labels file:

    python3 classify_video.py -m my_model.tflite

For information about the script options, run:

    python3 classify_video.py --help

For more instructions, see g.co/aiy/maker
"""

import argparse
from pycoral.utils.dataset import read_label_file
from aiymakerkit import vision
from aiymakerkit.utils import read_labels_from_metadata
import models


def main():
    parser = argparse.ArgumentParser(
        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument('-m', '--model', default=models.CLASSIFICATION_MODEL,
                        help='File path of .tflite file. Default is vision.CLASSIFICATION_MODEL')
    parser.add_argument('-l', '--labels', default=None,
                        help='File path of labels file. If not specified, ' \
                        'we get the labels from the model metadata.')
    args = parser.parse_args()

    classifier = vision.Classifier(args.model)
    if args.labels is not None:
        labels = read_label_file(args.labels)
    else:
        labels = read_labels_from_metadata(args.model)

    for frame in vision.get_frames(mirror=True):
        classes = classifier.get_classes(frame, top_k=1, threshold=0.3)
        if classes:
            score = classes[0].score
            label = labels.get(classes[0].id)
            print('label::',label)
            vision.draw_label(frame, f'{label}: {round(score, 4)}')


if __name__ == '__main__':
    main()

# Copyright 2021 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Performs continuous image classification with the camera video.

To classify things using a default MobileNet model, simply run the script:

    python3 classify_video.py

Or classify using your own model and labels file:

    python3 classify_video.py -m my_model.tflite

For information about the script options, run:

    python3 classify_video.py --help

For more instructions, see g.co/aiy/maker
"""

import argparse
from pycoral.utils.dataset import read_label_file
from aiymakerkit import vision
from aiymakerkit.utils import read_labels_from_metadata
import models
import ThunderBorg3 as ThunderBorg
import cv2 as cv
import time
from time import sleep



def main():
    MOVTEST = False
    TEST = True
    
    set_up_Thunder()
    
    parser = argparse.ArgumentParser(
        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument('-m', '--model', default=models.CLASSIFICATION_MODEL,
                        help='File path of .tflite file. Default is vision.CLASSIFICATION_MODEL')
    parser.add_argument('-l', '--labels', default=None,
                        help='File path of labels file. If not specified, ' \
                        'we get the labels from the model metadata.')
    args = parser.parse_args()

    classifier = vision.Classifier(args.model)
    if args.labels is not None:
        labels = read_label_file(args.labels)
    else:
        labels = read_labels_from_metadata(args.model)

    for frame in vision.get_frames(mirror=True):
        classes = classifier.get_classes(frame, top_k=1, threshold=0.8)
        if classes:
            score = classes[0].score
            label = labels.get(classes[0].id)
            vision.draw_label(frame, f'{label}: {round(score, 4)}')
            if MOVTEST: move_monster(label)
        key = cv.waitKey(1) & 0xFF
        if key == ord("q"):
            stop()
            cv.destroyAllWindows()        
            break

            
def set_up_Thunder():
    global TB, seq
    # Setup the ThunderBorg
    TB = ThunderBorg.ThunderBorg()
    #TB.i2cAddress = 0x15                  # Uncomment and change the value if you have changed the board address
    TB.Init()
    if not TB.foundChip:
        boards = ThunderBorg.ScanForThunderBorg()
        if len(boards) == 0:
            print ('No ThunderBorg found, check you are attached :)')
        else:
            pass
        sys.exit()
    TB.SetCommsFailsafe(False)             # Disable the communications failsafe


    voltageIn = 12.0                        # Total battery voltage to the ThunderBorg
    voltageOut= 12.0 * 0.60               # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power

    # Setup the power limits
    if voltageOut > voltageIn:
        maxPower = 1.0
    else:
        maxPower = voltageOut / float(voltageIn)
        maxPower_start = maxPower

    # Set sequence,pairs of motor1 and2 drive levels
    power = 0.4
    seq = [
            [+power,    +power],
            [+power*2, -power*2],
            [+power*1.5,    0.0],
            [-power,    0.0]
            ]

def stop():
    TB.MotorsOff()
    exit()

def forward():
    #if TEST: print('FORWARD')
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[0][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[0][1])
    #time.sleep(.50)
    #TB.MotorsOff()

def spin():
    #if TEST: print('SPIN')    
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[1][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[1][1])
    time.sleep(.2)
    TB.MotorsOff()
    time.sleep(.5)
    return

def curve():
    #if TEST: print('CURVE')    
    TB.SetCommsFailsafe(False)
    TB.SetMotor1(seq[2][0])
    TB.SetCommsFailsafe(False)
    TB.SetMotor2(seq[2][1])
    

def move_monster(action):
    #if TEST: print(action)
    if action == "stop": stop()
    elif action == "forward": forward()
    elif action == "spin": spin()
    elif action == "curve": curve()




if __name__ == '__main__':
    
    main()