Salma MayorquinTerry Rodriguez
Published © Apache-2.0

LitterBug - Autonomous Trash Rover

Self-driving rover using computer vision and deep learning to identify and clean up small pieces of trash.

IntermediateWork in progress3 days10,459

Things used in this project

Hardware components

Donkey Car Kit
Made by DIYRobocars, check out the link for full list of components
×1
Adafruit Ultimate GPS Breakout
Adafruit Ultimate GPS Breakout
×1
PIR Motion Sensor (generic)
PIR Motion Sensor (generic)
×1
Infrared Pi Camera
×1
Adafruit Mini USB microphone
×1
Analog Accelerometer: ADXL335
Adafruit Analog Accelerometer: ADXL335
×1
adafruit powerboost 500c
×1

Software apps and online services

TensorFlow
TensorFlow

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)
Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Custom parts and enclosures

Bumper with hinge

Bumper with hinges for LitterBug scooop

Right arm

Right arm for LitterBug scoop

Custom roll cage

Rod

Rod to hold arms taught and move scoop

Scoop

Scoop for LitterBug scoop

LitterBug V3 roll cage

final version for roll cage

Custom stilts

lowered stilts for desert monster rc car

Litterbug V2 roll cage

two separate u-shaped roll bars for 1/10 scale LitterBug

Left arm

Left arm for LitterBug scoop

Schematics

LitterBug Wiring V2

1/10 scale Litterbug wiring of sensors and servo

LitterBug Sensors

Code

controller.py

Python
Controller file for donkeycar repo. Replace <path-to-donkeycar-repo>/donkeycar/parts/controller.py file with this file.
import array
import time
import struct


from donkeycar.parts.web_controller.web import LocalWebController

class Joystick():
    """
    An interface to a physical joystick available at /dev/input
    """
    access_url = None #required to be consistent with web controller

    def __init__(self, dev_fn='/dev/input/js0'):
        self.axis_states = {}
        self.button_states = {}
        self.axis_map = []
        self.button_map = []
        self.jsdev = None
        self.dev_fn = dev_fn

        # These constants were borrowed from linux/input.h
        self.axis_names = {
            0x00 : 'x',
            0x01 : 'y',
            0x02 : 'z',
            0x03 : 'rx',
            0x04 : 'ry',
            0x05 : 'rz',
            0x06 : 'trottle',
            0x07 : 'rudder',
            0x08 : 'wheel',
            0x09 : 'gas',
            0x0a : 'brake',
            0x10 : 'hat0x',
            0x11 : 'hat0y',
            0x12 : 'hat1x',
            0x13 : 'hat1y',
            0x14 : 'hat2x',
            0x15 : 'hat2y',
            0x16 : 'hat3x',
            0x17 : 'hat3y',
            0x18 : 'pressure',
            0x19 : 'distance',
            0x1a : 'tilt_x',
            0x1b : 'tilt_y',
            0x1c : 'tool_width',
            0x20 : 'volume',
            0x28 : 'misc',
        }

        self.button_names = {
            0x120 : 'trigger',
            0x121 : 'thumb',
            0x122 : 'thumb2',
            0x123 : 'top',
            0x124 : 'top2',
            0x125 : 'pinkie',
            0x126 : 'base',
            0x127 : 'base2',
            0x128 : 'base3',
            0x129 : 'base4',
            0x12a : 'base5',
            0x12b : 'base6',

            #PS3 sixaxis specific
            0x12c : "triangle",
            0x12d : "circle",
            0x12e : "cross",
            0x12f : 'square',

            0x130 : 'a',
            0x131 : 'b',
            0x132 : 'c',
            0x133 : 'x',
            0x134 : 'y',
            0x135 : 'z',
            0x136 : 'tl',
            0x137 : 'tr',
            0x138 : 'tl2',
            0x139 : 'tr2',
            0x13a : 'select',
            0x13b : 'start',
            0x13c : 'mode',
            0x13d : 'thumbl',
            0x13e : 'thumbr',

            0x220 : 'dpad_up',
            0x221 : 'dpad_down',
            0x222 : 'dpad_left',
            0x223 : 'dpad_right',

            # XBox 360 controller uses these codes.
            0x2c0 : 'dpad_left',
            0x2c1 : 'dpad_right',
            0x2c2 : 'dpad_up',
            0x2c3 : 'dpad_down',
        }


    def init(self):
        from fcntl import ioctl
        """
        call once to setup connection to dev/input/js0 and map buttons
        """
        # Open the joystick device.
        print('Opening %s...' % self.dev_fn)
        self.jsdev = open(self.dev_fn, 'rb')

        # Get the device name.
        buf = array.array('B', [0] * 64)
        ioctl(self.jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
        self.js_name = buf.tobytes().decode('utf-8')
        print('Device name: %s' % self.js_name)

        # Get number of axes and buttons.
        buf = array.array('B', [0])
        ioctl(self.jsdev, 0x80016a11, buf) # JSIOCGAXES
        self.num_axes = buf[0]

        buf = array.array('B', [0])
        ioctl(self.jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
        self.num_buttons = buf[0]

        # Get the axis map.
        buf = array.array('B', [0] * 0x40)
        ioctl(self.jsdev, 0x80406a32, buf) # JSIOCGAXMAP

        for axis in buf[:self.num_axes]:
            axis_name = self.axis_names.get(axis, 'unknown(0x%02x)' % axis)
            self.axis_map.append(axis_name)
            self.axis_states[axis_name] = 0.0

        # Get the button map.
        buf = array.array('H', [0] * 200)
        ioctl(self.jsdev, 0x80406a34, buf) # JSIOCGBTNMAP

        for btn in buf[:self.num_buttons]:
            btn_name = self.button_names.get(btn, 'unknown(0x%03x)' % btn)
            self.button_map.append(btn_name)
            self.button_states[btn_name] = 0

        return True


    def show_map(self):
        """
        list the buttons and axis found on this joystick
        """
        print ('%d axes found: %s' % (self.num_axes, ', '.join(self.axis_map)))
        print ('%d buttons found: %s' % (self.num_buttons, ', '.join(self.button_map)))


    def poll(self):
        """
        query the state of the joystick, returns button which was pressed, if any,
        and axis which was moved, if any. button_state will be None, 1, or 0 if no changes,
        pressed, or released. axis_val will be a float from -1 to +1. button and axis will
        be the string label determined by the axis map in init.
        """
        button = None
        button_state = None
        axis = None
        axis_val = None

        # Main event loop
        evbuf = self.jsdev.read(8)

        if evbuf:
            tval, value, typev, number = struct.unpack('IhBB', evbuf)

            if typev & 0x80:
                #ignore initialization event
                return button, button_state, axis, axis_val

            if typev & 0x01:
                button = self.button_map[number]
                if button:
                    self.button_states[button] = value
                    button_state = value

            if typev & 0x02:
                axis = self.axis_map[number]
                if axis:
                    fvalue = value / 32767.0
                    self.axis_states[axis] = fvalue
                    axis_val = fvalue

        return button, button_state, axis, axis_val


class JoystickController(object):
    """
    Joystick client using access to local physical input
    """

    def __init__(self, poll_delay=0.0,
                 max_throttle=1.0,
                 dumping_axis='ry',
                 steering_axis='x',
                 throttle_axis='rz',
                 dumping_scale=1.0,
                 steering_scale=1.0,
                 throttle_scale=-1.0,
                 dev_fn='/dev/input/js0',
                 auto_record_on_throttle=True):

        self.dumping = 0.0
        self.angle = 0.0
        self.throttle = 0.0
        self.mode = 'user'
        self.poll_delay = poll_delay
        self.running = True
        self.max_throttle = max_throttle
        self.dumping_axis = dumping_axis
        self.steering_axis = steering_axis
        self.throttle_axis = throttle_axis
        self.dumping_scale = dumping_scale
        self.steering_scale = steering_scale
        self.throttle_scale = throttle_scale
        self.recording = False
        self.constant_throttle = False
        self.auto_record_on_throttle = auto_record_on_throttle
        self.dev_fn = dev_fn
        self.js = None

        #We expect that the framework for parts will start a new
        #thread for our update fn. We used to do that and it caused
        #two threads to be polling for js events.

    def on_throttle_changes(self):
        """
        turn on recording when non zero throttle in the user mode.
        """
        if self.auto_record_on_throttle:
            self.recording = (self.throttle != 0.0 and self.mode == 'user')

    def init_js(self):
        """
        attempt to init joystick
        """
        try:
            self.js = Joystick(self.dev_fn)
            self.js.init()
        except FileNotFoundError:
            print(self.dev_fn, "not found.")
            self.js = None
        return self.js is not None


    def update(self):
        """
        poll a joystick for input events

        button map name => PS3 button => function
        * top2 = PS3 dpad up => increase throttle scale
        * base = PS3 dpad down => decrease throttle scale
        * base2 = PS3 dpad left => increase steering scale
        * pinkie = PS3 dpad right => decrease steering scale
        * trigger = PS3 select => switch modes
        * top = PS3 start => toggle constant throttle
        * base5 = PS3 left trigger 1
        * base3 = PS3 left trigger 2
        * base6 = PS3 right trigger 1
        * base4 = PS3 right trigger 2
        * thumb2 = PS3 right thumb
        * thumb = PS3 left thumb
        * circle = PS3 circrle => toggle recording
        * triangle = PS3 triangle => increase max throttle
        * cross = PS3 cross => decrease max throttle
        """

        #wait for joystick to be online
        while self.running and not self.init_js():
            time.sleep(5)

        while self.running:
            button, button_state, axis, axis_val = self.js.poll()

            if axis == self.dumping_axis:
                self.dumping = self.dumping_scale * axis_val
                print("dumping", self.dumping)

            if axis == self.steering_axis:
                self.angle = self.steering_scale * axis_val
                print("angle", self.angle)

            if axis == self.throttle_axis:
                #this value is often reversed, with positive value when pulling down
                self.throttle = (self.throttle_scale * axis_val * self.max_throttle)
                print("throttle", self.throttle)
                self.on_throttle_changes()

            if button == 'trigger' and button_state == 1:
                """
                switch modes from:
                user: human controlled steer and throttle
                local_angle: ai steering, human throttle
                local: ai steering, ai throttle
                """
                if self.mode == 'user':
                    self.mode = 'local_angle'
                elif self.mode == 'local_angle':
                    self.mode = 'local'
                else:
                    self.mode = 'user'
                print('new mode:', self.mode)

            if button == 'b' and button_state == 1:
                """
                toggle recording on/off
                """
                if self.auto_record_on_throttle:
                    print('auto record on throttle is enabled.')
                elif self.recording:
                    #self.recording = False
                    self.recording = True
                    print('self recording false')
                else:
                    print('self recording true')
                    #self.recording = True
                    self.recording = False

                print('recording:', self.recording)

            if button == 'x' and button_state == 1:
                """
                increase max throttle setting
                """
                print("I am triangle!")
                self.max_throttle = round(min(1.0, self.max_throttle + 0.01), 2)
                if self.constant_throttle:
                    self.throttle = self.max_throttle
                    self.on_throttle_changes()

                print('max_throttle:', self.max_throttle)

            if button == 'a' and button_state == 1:
                """
                decrease max throttle setting
                """
                print("I am cross!")
                self.max_throttle = round(max(0.0, self.max_throttle - 0.01), 2)
                if self.constant_throttle:
                    self.throttle = self.max_throttle
                    self.on_throttle_changes()

                print('max_throttle:', self.max_throttle)

            if button == 'dpad_up' and button_state == 1:
                """
                increase throttle scale
                """
                print("I am base!")
                self.throttle_scale = round(min(0.0, self.throttle_scale + 0.05), 2)
                print('throttle_scale:', self.throttle_scale)

            if button == 'dpad_down' and button_state == 1:
                """
                decrease throttle scale
                """
                print("I am top2!")
                self.throttle_scale = round(max(-1.0, self.throttle_scale - 0.05), 2)
                print('throttle_scale:', self.throttle_scale)

            if button == 'dpad_left' and button_state == 1:
                """
                increase steering scale
                """
                print("I am base2!")
                self.steering_scale = round(min(1.0, self.steering_scale + 0.05), 2)
                print('steering_scale:', self.steering_scale)

            if button == 'dpad_right' and button_state == 1:
                """
                decrease steering scale
                """
                print("I am pinkie!")
                self.steering_scale = round(max(0.0, self.steering_scale - 0.05), 2)
                print('steering_scale:', self.steering_scale)

            if button == 'start' and button_state == 1:
                """
                toggle constant throttle
                """
                if self.constant_throttle:
                    self.constant_throttle = False
                    self.throttle = 0
                    self.on_throttle_changes()
                else:
                    self.constant_throttle = True
                    self.throttle = self.max_throttle
                    self.on_throttle_changes()
                print('constant_throttle:', self.constant_throttle)

            time.sleep(self.poll_delay)

    def run_threaded(self, img_arr=None):
        self.img_arr = img_arr
        return self.dumping, self.angle, self.throttle, self.mode, self.recording

    def run(self, img_arr=None):
        raise Exception("We expect for this part to be run with the threaded=True argument.")
        return False

    def shutdown(self):
        self.running = False
        time.sleep(0.5)

manage.py modified

Python
Modified manage.py file for mycar directory on donkeycar that includes scoop mechanism. Replace <path-to-mycar>/mycar/manage.py file with this file.
#!/usr/bin/env python3
"""
Scripts to drive a donkey 2 car and train a model for it.

Usage:
    manage.py (drive) [--model=<model>] [--js] [--chaos]
    manage.py (train) [--tub=<tub1,tub2,..tubn>]  (--model=<model>) [--base_model=<base_model>] [--no_cache]

Options:
    -h --help        Show this screen.
    --tub TUBPATHS   List of paths to tubs. Comma separated. Use quotes to use wildcards. ie "~/tubs/*"
    --js             Use physical joystick.
    --chaos          Add periodic random steering when manually driving
"""
import os
from docopt import docopt

import donkeycar as dk

#import parts
from donkeycar.parts.camera import PiCamera
from donkeycar.parts.transform import Lambda
from donkeycar.parts.keras import KerasCategorical
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
from donkeycar.parts.datastore import TubGroup, TubWriter
from donkeycar.parts.controller import LocalWebController, JoystickController
from donkeycar.parts.clock import Timestamp


def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
    """
    Construct a working robotic vehicle from many parts.
    Each part runs as a job in the Vehicle loop, calling either
    it's run or run_threaded method depending on the constructor flag `threaded`.
    All parts are updated one after another at the framerate given in
    cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
    Parts may have named outputs and inputs. The framework handles passing named outputs
    to parts requesting the same named input.
    """

    V = dk.vehicle.Vehicle()

    clock = Timestamp()
    V.add(clock, outputs='timestamp')

    cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
    V.add(cam, outputs=['cam/image_array'], threaded=True)

    if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
        ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
                                 steering_scale=cfg.JOYSTICK_STEERING_SCALE,
                                 dumping_scale=cfg.JOYSTICK_DUMPING_SCALE,
                                 auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
   #     V.add(ctr,
   #             inputs=['cam/image_array'],
   #             outputs=['user/dumping', 'user/angle', 'user/throttle', 'user/mode', 'recording'],
   #             threaded=True)
    else:
        # This web controller will create a web server that is capable
        # of managing steering, throttle, and modes, and more.
        ctr = LocalWebController(use_chaos=use_chaos)
    #    V.add(ctr,
    #            inputs=['cam/image_array'],
    #            outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
    #            threaded=True)
    V.add(ctr,
            inputs=['cam/image_array'],
            outputs=['user/dumping', 'user/angle', 'user/throttle', 'user/mode', 'recording'],
            threaded=True)

    # See if we should even run the pilot module.
    # This is only needed because the part run_condition only accepts boolean
    def pilot_condition(mode):
        if mode == 'user':
            return False
        else:
            return True

    pilot_condition_part = Lambda(pilot_condition)
    V.add(pilot_condition_part, inputs=['user/mode'],
                                outputs=['run_pilot'])

    # Run the pilot if the mode is not user.
    kl = KerasCategorical()
    if model_path:
        kl.load(model_path)

    V.add(kl, inputs=['cam/image_array'],
              outputs=['pilot/angle', 'pilot/throttle'],
              run_condition='run_pilot')

    # Choose what inputs should change the car.
    def drive_mode(mode,
                   user_dumping, user_angle, user_throttle,
                   pilot_angle, pilot_throttle):
        if mode == 'user':
            return user_dumping, user_angle, user_throttle

        elif mode == 'local_angle':
            return pilot_angle, user_throttle

        else:
            return pilot_angle, pilot_throttle

    drive_mode_part = Lambda(drive_mode)
    V.add(drive_mode_part,
          inputs=['user/mode', 'user/dumping', 'user/angle', 'user/throttle',
                   'pilot/angle', 'pilot/throttle'],
          outputs=['dumping', 'angle', 'throttle'])

    #Dumping control
    dumping_controller = PCA9685(cfg.DUMPING_CHANNEL)
    dumping = PWMSteering(controller=dumping_controller,
                           left_pulse=cfg.DUMPING_UP_PWM,
                           right_pulse=cfg.DUMPING_DOWN_PWM)

    steering_controller = PCA9685(cfg.STEERING_CHANNEL)
    steering = PWMSteering(controller=steering_controller,
                           left_pulse=cfg.STEERING_LEFT_PWM,
                           right_pulse=cfg.STEERING_RIGHT_PWM)

    throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
    throttle = PWMThrottle(controller=throttle_controller,
                           max_pulse=cfg.THROTTLE_FORWARD_PWM,
                           zero_pulse=cfg.THROTTLE_STOPPED_PWM,
                           min_pulse=cfg.THROTTLE_REVERSE_PWM)

    #if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
    #    V.add(dumping, inputs=['dumping'])
    V.add(dumping, inputs=['dumping'])
    V.add(steering, inputs=['angle'])
    V.add(throttle, inputs=['throttle'])

    # add tub to save data
    #if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
    #    inputs = ['cam/image_array', 'user/dumping', 'user/angle', 'user/throttle', 'user/mode', 'timestamp']
    #    types = ['image_array', 'float', 'float', 'float', 'str', 'str']
    #else:
    #    inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp']
    #    types = ['image_array', 'float', 'float', 'str', 'str']
    inputs = ['cam/image_array', 'user/dumping', 'user/angle', 'user/throttle', 'user/mode', 'timestamp']
    types = ['image_array', 'float', 'float', 'float', 'str', 'str']


    #multiple tubs
    #th = TubHandler(path=cfg.DATA_PATH)
    #tub = th.new_tub_writer(inputs=inputs, types=types)

    # single tub
    tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
    V.add(tub, inputs=inputs, run_condition='recording')

    # run the vehicle
    V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
            max_loop_count=cfg.MAX_LOOPS)




def train(cfg, tub_names, new_model_path, base_model_path=None ):
    """
    use the specified data in tub_names to train an artifical neural network
    saves the output trained model as model_name
    """
    X_keys = ['cam/image_array']
    y_keys = ['user/angle', 'user/throttle']
    def train_record_transform(record):
        """ convert categorical steering to linear and apply image augmentations """
        record['user/angle'] = dk.util.data.linear_bin(record['user/angle'])
        # TODO add augmentation that doesn't use opencv
        return record

    def val_record_transform(record):
        """ convert categorical steering to linear """
        record['user/angle'] = dk.util.data.linear_bin(record['user/angle'])
        return record

    new_model_path = os.path.expanduser(new_model_path)

    kl = KerasCategorical()
    if base_model_path is not None:
        base_model_path = os.path.expanduser(base_model_path)
        kl.load(base_model_path)

    print('tub_names', tub_names)
    if not tub_names:
        tub_names = os.path.join(cfg.DATA_PATH, '*')
    tubgroup = TubGroup(tub_names)
    train_gen, val_gen = tubgroup.get_train_val_gen(X_keys, y_keys,
                                                    train_record_transform=train_record_transform,
                                                    val_record_transform=val_record_transform,
                                                    batch_size=cfg.BATCH_SIZE,
                                                    train_frac=cfg.TRAIN_TEST_SPLIT)

    total_records = len(tubgroup.df)
    total_train = int(total_records * cfg.TRAIN_TEST_SPLIT)
    total_val = total_records - total_train
    print('train: %d, validation: %d' % (total_train, total_val))
    steps_per_epoch = total_train // cfg.BATCH_SIZE
    print('steps_per_epoch', steps_per_epoch)

    kl.train(train_gen,
             val_gen,
             saved_model_path=new_model_path,
             steps=steps_per_epoch,
             train_split=cfg.TRAIN_TEST_SPLIT)


if __name__ == '__main__':
    args = docopt(__doc__)
    cfg = dk.load_config()

    if args['drive']:
        drive(cfg, model_path = args['--model'], use_joystick=args['--js'], use_chaos=args['--chaos'])

    elif args['train']:
        tub = args['--tub']
        new_model_path = args['--model']
        base_model_path = args['--base_model']
        cache = not args['--no_cache']
        train(cfg, tub, new_model_path, base_model_path)

config.py modified

Python
Modified config.py file for mycar directory on donkeycar. Replace <path-to-mycar>/mycar/config.py with this file
"""
CAR CONFIG

This file is read by your car application's manage.py script to change the car
performance.

EXMAPLE
-----------
import dk
cfg = dk.load_config(config_path='~/mycar/config.py')
print(cfg.CAMERA_RESOLUTION)

"""


import os

#PATHS
CAR_PATH = PACKAGE_PATH = os.path.dirname(os.path.realpath(__file__))
DATA_PATH = os.path.join(CAR_PATH, 'data')
MODELS_PATH = os.path.join(CAR_PATH, 'models')

#VEHICLE
DRIVE_LOOP_HZ = 20
MAX_LOOPS = 100000

#CAMERA
CAMERA_RESOLUTION = (120, 160) #(height, width)
CAMERA_FRAMERATE = DRIVE_LOOP_HZ

#STEERING
STEERING_CHANNEL = 1
#STEERING_LEFT_PWM = 420
#STEERING_LEFT_PWM = 630
STEERING_LEFT_PWM = 590
#STEERING_RIGHT_PWM = 360
#STEERING_RIGHT_PWM = 230
STEERING_RIGHT_PWM = 190

#DUMPING
DUMPING_CHANNEL = 2
#DUMPING_UP_PWM = 100
#DUMPING_DOWN_PWM = 550
DUMPING_UP_PWM = 550
DUMPING_DOWN_PWM = 100

#THROTTLE
THROTTLE_CHANNEL = 0
#THROTTLE_FORWARD_PWM = 500
THROTTLE_FORWARD_PWM = 450
#THROTTLE_FORWARD_PWM = 420
THROTTLE_STOPPED_PWM = 370
#THROTTLE_REVERSE_PWM = 300
THROTTLE_REVERSE_PWM = 340

#TRAINING
BATCH_SIZE = 128
TRAIN_TEST_SPLIT = 0.8


#JOYSTICK
USE_JOYSTICK_AS_DEFAULT = False
#JOYSTICK_MAX_THROTTLE = 0.25
#JOYSTICK_MAX_THROTTLE = 1.2
JOYSTICK_MAX_THROTTLE = 1.0
JOYSTICK_STEERING_SCALE = 1.0
JOYSTICK_DUMPING_SCALE = 1.0
AUTO_RECORD_ON_THROTTLE = True 


TUB_PATH = os.path.join(CAR_PATH, 'tub') # if using a single tub

#ROPE.DONKEYCAR.COM
ROPE_TOKEN="GET A TOKEN AT ROPE.DONKEYCAR.COM"

gyro_accel_bmp.py

Python
Python script collecting meta data from optional sensors and logging to sqlite3 table. Includes functions for adafruit gps breakout and 10DOF breakout sensor (LSM303DLHC, L3GD20, BMP180 sensors in one board!). Start with crontab job @reboot for continuous data collection
# Distributed with a free-will license.
# Use it any way you want, profit or free, provided it fits in the licenses of its associated works.
# L3GD20
# This code is designed to work with the L3GD20_I2CS I2C Mini Module available from ControlEverything.com.
# https://www.controleverything.com/products

import smbus2 as smbus
import sqlite3
from datetime import datetime
import Adafruit_BMP.BMP085 as BMP085
import time

# Get I2C bus
bus = smbus.SMBus(1)

# Start BMP 
bmp = BMP085.BMP085()

# Connect to table
conn = sqlite3.connect("/home/pi/sensors", timeout=30000)
c = conn.cursor()


def get_gyro():
    t = str(datetime.now())
    bus.write_byte_data(0x40, 0x20, 0x0F)
    bus.write_byte_data(0x40, 0x23, 0x30)

    time.sleep(0.5)

    data0 = bus.read_byte_data(0x40, 0x28)
    data1 = bus.read_byte_data(0x40, 0x29)

    # Convert the data
    xGyro = data1 * 256 + data0
    if xGyro > 32767 :
        xGyro -= 65536

    data0 = bus.read_byte_data(0x40, 0x2A)
    data1 = bus.read_byte_data(0x40, 0x2B)

    # Convert the data
    yGyro = data1 * 256 + data0
    if yGyro > 32767 :
        yGyro -= 65536

    data0 = bus.read_byte_data(0x40, 0x2C)
    data1 = bus.read_byte_data(0x40, 0x2D)

    # Convert the data
    zGyro = data1 * 256 + data0
    if zGyro > 32767 :
        zGyro -= 65536
    #print("Rotation in X-Axis : %d" %xGyro)
    #print("Rotation in Y-Axis : %d" %yGyro)
    #print("Rotation in Z-Axis : %d" %zGyro)
    c.execute('''INSERT INTO gyro(date, x, y, z)
                              VALUES(?,?,?,?)''', (t,xGyro, yGyro, zGyro))
    conn.commit()
    #return xGyro, yGyro, zGyro
    return


def get_accel():
    t = str(datetime.now())
    bus.write_byte_data(0x68, 0x20, 0x27)
    bus.write_byte_data(0x68, 0x23, 0x00)

    time.sleep(0.5)

    data0 = bus.read_byte_data(0x68, 0x28)
    data1 = bus.read_byte_data(0x68, 0x29)

    # Convert the data
    xAccl = data1 * 256 + data0
    if xAccl > 32767 :
        xAccl -= 65536

    data0 = bus.read_byte_data(0x68, 0x2A)
    data1 = bus.read_byte_data(0x68, 0x2B)

    # Convert the data
    yAccl = data1 * 256 + data0
    if yAccl > 32767 :
        yAccl -= 65536

    data0 = bus.read_byte_data(0x68, 0x2C)
    data1 = bus.read_byte_data(0x68, 0x2D)

    # Convert the data
    zAccl = data1 * 256 + data0
    if zAccl > 32767 :
        zAccl -= 65536
    #print("Acceleration in X-Axis : %d" % xAccl)
    #print("Acceleration in Y-Axis : %d" % yAccl)
    #print("Acceleration in Z-Axis : %d" % zAccl)
    c.execute('''INSERT INTO accel(date, x, y, z)
                              VALUES(?,?,?,?)''', (t,xAccl, yAccl, zAccl))
    conn.commit()
    #return xAccl, yAccl, zAccl 
    return

def get_bmp():
    t = str(datetime.now())
    temp = bmp.read_temperature()
    pressure = bmp.read_pressure()
    alt = bmp.read_altitude()
    c.execute('''INSERT INTO bmp(date, temp, pressure, alt)
                              VALUES(?,?,?,?)''', (t,temp, pressure, alt))
    conn.commit()
    return

if __name__ == "__main__":
    get_gyro()
    get_accel()
    get_bmp()
    c.close()

motion.py

Python
Motion detection script to take images when motion is detected. Start script at night time with cron tab as a security feature while rover is not driving.
import RPi.GPIO as GPIO
from picamera import PiCamera
from datetime import datetime
import time

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)


camera = PiCamera()
pir_sensor = 4

GPIO.setup(pir_sensor, GPIO.IN, GPIO.PUD_DOWN)

current_state = 0

try:
    while True:
        time.sleep(0.1)
        current_state = GPIO.input(pir_sensor)
        if current_state == 1:
          print("GPIO pin %s is %s" % (pir_sensor, current_state)) # motion detected
          camera.capture('/home/pi/pictures/'+ str(datetime.now())+ '.jpg')
          time.sleep(2) # wait 1 seconds for PIR to reset. 
        else:
            pass
except KeyboardInterrupt:
    GPIO.cleanup()

trashpilot V1

Protobuf
Preliminary autopilot model for identifying and driving towards trash.
No preview (download only).

litterbug V2

Protobuf
Simple autopilot model for navigation (no trash pickup). You can use this model for transfer learning when training your own off road donkeycar!
No preview (download only).

keras.py

Python
Old keras.py file for donkeycar repo. Replace <path-to-donkeycar-repo>/donkeycar/parts/keras.py file with this file.
""""
keras.py
functions to run and train autopilots using keras
"""

from tensorflow.python.keras.layers import Input
from tensorflow.python.keras.models import Model, load_model
from tensorflow.python.keras.layers import Convolution2D
from tensorflow.python.keras.layers import Dropout, Flatten, Dense, Cropping2D, Lambda
from tensorflow.python.keras.callbacks import ModelCheckpoint, EarlyStopping

from donkeycar import util


class KerasPilot:

    def load(self, model_path):
        self.model = load_model(model_path)

    def shutdown(self):
        pass

    def train(self, train_gen, val_gen,
              saved_model_path, epochs=100, steps=100, train_split=0.8,
              verbose=1, min_delta=.0005, patience=5, use_early_stop=True):
        """
        train_gen: generator that yields an array of images an array of
        """

        # checkpoint to save model after each epoch
        save_best = ModelCheckpoint(saved_model_path,
                                    monitor='val_loss',
                                    verbose=verbose,
                                    save_best_only=True,
                                    mode='min')

        # stop training if the validation error stops improving.
        early_stop = EarlyStopping(monitor='val_loss',
                                   min_delta=min_delta,
                                   patience=patience,
                                   verbose=verbose,
                                   mode='auto')

        callbacks_list = [save_best]

        if use_early_stop:
            callbacks_list.append(early_stop)

        hist = self.model.fit_generator(
            train_gen,
            steps_per_epoch=steps,
            epochs=epochs,
            verbose=1,
            validation_data=val_gen,
            callbacks=callbacks_list,
            validation_steps=steps * (1.0 - train_split) / train_split)
        return hist


class KerasCategorical(KerasPilot):
    def __init__(self, model=None, *args, **kwargs):
        super(KerasCategorical, self).__init__(*args, **kwargs)
        if model:
            self.model = model
        else:
            self.model = default_categorical()

    def run(self, img_arr):
        img_arr = img_arr.reshape((1,) + img_arr.shape)
        angle_binned, throttle = self.model.predict(img_arr)
        angle_unbinned = util.data.linear_unbin(angle_binned[0])
        return angle_unbinned, throttle[0][0]


class KerasLinear(KerasPilot):
    def __init__(self, model=None, num_outputs=None, *args, **kwargs):
        super(KerasLinear, self).__init__(*args, **kwargs)
        if model:
            self.model = model
        elif num_outputs is not None:
            self.model = default_n_linear(num_outputs)
        else:
            self.model = default_linear()

    def run(self, img_arr):
        img_arr = img_arr.reshape((1,) + img_arr.shape)
        outputs = self.model.predict(img_arr)
        # print(len(outputs), outputs)
        steering = outputs[0]
        throttle = outputs[1]
        return steering[0][0], throttle[0][0]


def default_categorical():
    img_in = Input(shape=(120, 160, 3),
                   name='img_in')  # First layer, input layer, Shape comes from camera.py resolution, RGB
    x = img_in
    x = Convolution2D(24, (5, 5), strides=(2, 2), activation='relu')(
        x)  # 24 features, 5 pixel x 5 pixel kernel (convolution, feauture) window, 2wx2h stride, relu activation
    x = Convolution2D(32, (5, 5), strides=(2, 2), activation='relu')(
        x)  # 32 features, 5px5p kernel window, 2wx2h stride, relu activatiion
    x = Convolution2D(64, (5, 5), strides=(2, 2), activation='relu')(
        x)  # 64 features, 5px5p kernal window, 2wx2h stride, relu
    x = Convolution2D(64, (3, 3), strides=(2, 2), activation='relu')(
        x)  # 64 features, 3px3p kernal window, 2wx2h stride, relu
    x = Convolution2D(64, (3, 3), strides=(1, 1), activation='relu')(
        x)  # 64 features, 3px3p kernal window, 1wx1h stride, relu

    # Possibly add MaxPooling (will make it less sensitive to position in image).  Camera angle fixed, so may not to be needed

    x = Flatten(name='flattened')(x)  # Flatten to 1D (Fully connected)
    x = Dense(100, activation='relu')(x)  # Classify the data into 100 features, make all negatives 0
    x = Dropout(.1)(x)  # Randomly drop out (turn off) 10% of the neurons (Prevent overfitting)
    x = Dense(50, activation='relu')(x)  # Classify the data into 50 features, make all negatives 0
    x = Dropout(.1)(x)  # Randomly drop out 10% of the neurons (Prevent overfitting)
    # categorical output of the angle
    angle_out = Dense(15, activation='softmax', name='angle_out')(
        x)  # Connect every input with every output and output 15 hidden units. Use Softmax to give percentage. 15 categories and find best one based off percentage 0.0-1.0

    # continous output of throttle
    throttle_out = Dense(1, activation='relu', name='throttle_out')(x)  # Reduce to 1 number, Positive number only

    model = Model(inputs=[img_in], outputs=[angle_out, throttle_out])
    model.compile(optimizer='adam',
                  loss={'angle_out': 'categorical_crossentropy',
                        'throttle_out': 'mean_absolute_error'},
                  loss_weights={'angle_out': 0.9, 'throttle_out': .01})

    return model


from tensorflow.python.keras import backend as K


def linear_unbin_layer(tnsr):
    bin = K.constant((2 / 14), dtype='float32')
    norm = K.constant(1, dtype='float32')

    b = K.cast(K.argmax(tnsr), dtype='float32')
    a = b - norm
    # print('linear_unbin_layer out: {}'.format(a))
    return a


def default_catlin():
    """
    Categorial Steering output before linear conversion.
    :return:
    """
    img_in = Input(shape=(120, 160, 3),
                   name='img_in')  # First layer, input layer, Shape comes from camera.py resolution, RGB
    x = img_in
    x = Convolution2D(24, (5, 5), strides=(2, 2), activation='relu')(
        x)  # 24 features, 5 pixel x 5 pixel kernel (convolution, feauture) window, 2wx2h stride, relu activation
    x = Convolution2D(32, (5, 5), strides=(2, 2), activation='relu')(
        x)
    x = Convolution2D(64, (5, 5), strides=(2, 2), activation='relu')(
        x)  # 64 features, 5px5p kernal window, 2wx2h stride, relu
    x = Convolution2D(64, (3, 3), strides=(2, 2), activation='relu')(
        x)  # 64 features, 3px3p kernal window, 2wx2h stride, relu
    x = Convolution2D(64, (3, 3), strides=(1, 1), activation='relu')(
        x)  # 64 features, 3px3p kernal window, 1wx1h stride, relu

    # Possibly add MaxPooling (will make it less sensitive to position in image).  Camera angle fixed, so may not to be needed

    x = Flatten(name='flattened')(x)  # Flatten to 1D (Fully connected)
    x = Dense(100, activation='relu')(x)  # Classify the data into 100 features, make all negatives 0
    x = Dropout(.1)(x)  # Randomly drop out (turn off) 10% of the neurons (Prevent overfitting)
    x = Dense(50, activation='relu')(x)  # Classify the data into 50 features, make all negatives 0
    x = Dropout(.1)(x)  # Randomly drop out 10% of the neurons (Prevent overfitting)
    # categorical output of the angle
    angle_cat_out = Dense(15, activation='softmax', name='angle_cat_out')(x)
    angle_out = Dense(1, activation='sigmoid', name='angle_out')(angle_cat_out)
    # angle_out = Lambda(linear_unbin_layer, output_shape=(1,1, ), name='angle_out')(angle_cat_out)

    # continuous output of throttle
    throttle_out = Dense(1, activation='relu', name='throttle_out')(x)  # Reduce to 1 number, Positive number only

    model = Model(inputs=[img_in], outputs=[angle_out, throttle_out])
    model.compile(optimizer='adam',
                  loss={'angle_out': 'mean_squared_error',
                        'throttle_out': 'mean_absolute_error'},
                  loss_weights={'angle_out': 0.9, 'throttle_out': .01})

    return model


def default_linear():
    img_in = Input(shape=(120, 160, 3), name='img_in')
    x = img_in
    x = Convolution2D(24, (5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(32, (5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(64, (5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(64, (3, 3), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(64, (3, 3), strides=(1, 1), activation='relu')(x)

    x = Flatten(name='flattened')(x)
    x = Dense(100, activation='linear')(x)
    x = Dropout(.1)(x)
    x = Dense(50, activation='linear')(x)
    x = Dropout(.1)(x)
    # categorical output of the angle
    angle_out = Dense(1, activation='linear', name='angle_out')(x)

    # continous output of throttle
    throttle_out = Dense(1, activation='linear', name='throttle_out')(x)

    model = Model(inputs=[img_in], outputs=[angle_out, throttle_out])

    model.compile(optimizer='adam',
                  loss={'angle_out': 'mean_squared_error',
                        'throttle_out': 'mean_squared_error'},
                  loss_weights={'angle_out': 0.5, 'throttle_out': .5})

    return model


def default_n_linear(num_outputs):
    img_in = Input(shape=(120, 160, 3), name='img_in')
    x = img_in
    x = Cropping2D(cropping=((60, 0), (0, 0)))(x)  # trim 60 pixels off top
    x = Lambda(lambda x: x / 127.5 - 1.)(x)  # normalize and re-center
    x = Convolution2D(24, (5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(32, (5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(64, (5, 5), strides=(1, 1), activation='relu')(x)
    x = Convolution2D(64, (3, 3), strides=(1, 1), activation='relu')(x)
    x = Convolution2D(64, (3, 3), strides=(1, 1), activation='relu')(x)

    x = Flatten(name='flattened')(x)
    x = Dense(100, activation='relu')(x)
    x = Dropout(.1)(x)
    x = Dense(50, activation='relu')(x)
    x = Dropout(.1)(x)

    outputs = []

    for i in range(num_outputs):
        outputs.append(Dense(1, activation='linear', name='n_outputs' + str(i))(x))

    model = Model(inputs=[img_in], outputs=outputs)

    model.compile(optimizer='adam',
                  loss='mse')
    return model

Credits

Salma Mayorquin

Salma Mayorquin

21 projects • 373 followers
Software engineer AI/ML & hardware tinkerer interested in embedded AI. Lets hack the change we want to see!
Terry Rodriguez

Terry Rodriguez

21 projects • 192 followers
hack the change you want to see

Comments