├── tests ├── __init__.py ├── test_datasets.py ├── test_camera.py └── test_actuators.py ├── docs ├── tf-requirements.txt ├── 03-train_pilots.md ├── 99-Contribute.md ├── 02-start_driving.md ├── Mechanical_Hardware_Instructions.md └── 01-build_a_car.md ├── .gitignore ├── hardware ├── Camera Holder.stl ├── Roll Cage 2nd Version.stl ├── Adjustable Camera Holder.stl └── Top And Bottom Plate-Will Car.dwg ├── demos ├── train_download.py ├── serve_no_pilot.py ├── train.py ├── serve_pilot.py ├── drive_sim.py ├── train_generator.py ├── drive_pi.py └── train_model_search.py ├── donkey ├── templates │ ├── static │ │ ├── style.css │ │ ├── main.js │ │ └── nipple.js │ └── monitor.html ├── __init__.py ├── vehicles.py ├── pilots.py ├── datasets.py ├── actuators.py ├── sensors.py ├── utils.py ├── models.py ├── sessions.py └── remotes.py ├── README.md └── setup.py /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/tf-requirements.txt: -------------------------------------------------------------------------------- 1 | keras 2 | tensorflow 3 | h5py 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | env/* 2 | data/* 3 | *.pyc 4 | .ipynb_checkpoints 5 | *.egg-info/ 6 | -------------------------------------------------------------------------------- /hardware/Camera Holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSSDC/donkey/HEAD/hardware/Camera Holder.stl -------------------------------------------------------------------------------- /hardware/Roll Cage 2nd Version.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSSDC/donkey/HEAD/hardware/Roll Cage 2nd Version.stl -------------------------------------------------------------------------------- /hardware/Adjustable Camera Holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSSDC/donkey/HEAD/hardware/Adjustable Camera Holder.stl -------------------------------------------------------------------------------- /docs/03-train_pilots.md: -------------------------------------------------------------------------------- 1 | # Train your car. 2 | See the `train.py` demo for an exmaple how to train a pilot from session data. -------------------------------------------------------------------------------- /hardware/Top And Bottom Plate-Will Car.dwg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSSDC/donkey/HEAD/hardware/Top And Bottom Plate-Will Car.dwg -------------------------------------------------------------------------------- /tests/test_datasets.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import donkey as dk 4 | 5 | 6 | class TestDatasets(unittest.TestCase): 7 | 8 | def test_moving_square(self): 9 | X, Y = dk.datasets.moving_square() 10 | 11 | 12 | -------------------------------------------------------------------------------- /demos/train_download.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Example how to download a pickled dataset from a url. 3 | ''' 4 | import donkey as dk 5 | 6 | 7 | print('Downloading file, this could take some time.) 8 | url = 'https://s3.amazonaws.com/donkey_resources/port.pkl' 9 | X, Y = dk.datasets.load_url(url) 10 | 11 | print('Loading Model.') 12 | model = dk.models.cnn3_full1() 13 | 14 | print('Fitting Model.') 15 | model.fit(X,Y, nb_epoch=10, validation_split=0.2) 16 | 17 | 18 | model.save('test_model') -------------------------------------------------------------------------------- /donkey/templates/static/style.css: -------------------------------------------------------------------------------- 1 | 2 | #joystick_container { 3 | position: fixed; 4 | bottom:0; 5 | right:0; 6 | width: 400px; 7 | height:40%; 8 | border:1pt solid black; 9 | background-color: rgba(255, 0, 0, 0.1); 10 | } 11 | 12 | #mpeg-image { 13 | width: 400px; 14 | } 15 | 16 | @media (max-width: 600px) { 17 | #mpeg-image { 18 | position: relative; 19 | width: 100%; 20 | } 21 | 22 | #joystick_container { 23 | width:100%; 24 | } 25 | } 26 | 27 | 28 | -------------------------------------------------------------------------------- /demos/serve_no_pilot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Script to start server to drive your car. 3 | """ 4 | 5 | import donkey as dk 6 | 7 | 8 | #setup how server will save files and which pilot to use 9 | pilot = dk.pilots.BasePilot() 10 | 11 | sessions_path = '~/donkey_data/sessions' 12 | sh = dk.sessions.SessionHandler(sessions_path=sessions_path) 13 | session = sh.new() 14 | 15 | #start server 16 | w = dk.remotes.RemoteServer(session, pilot, port=8886) 17 | w.start() 18 | 19 | #in a browser go to localhost:8887 to drive your car 20 | -------------------------------------------------------------------------------- /tests/test_camera.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import donkey as dk 4 | 5 | class TestBaseCamera(unittest.TestCase): 6 | 7 | def setUp(self): 8 | self.camera = dk.sensors.BaseCamera() 9 | 10 | 11 | def test_capture_arr(self): 12 | arr = self.camera.capture_arr() 13 | assert arr.shape[2] == 3 #3 RGB channels 14 | 15 | def test_capture_img(self): 16 | img = self.camera.capture_img() 17 | print(type(img)) 18 | 19 | 20 | 21 | if __name__ == '__main__': 22 | unittest.main() -------------------------------------------------------------------------------- /donkey/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | uname = os.uname() 4 | if uname[1] != 'raspberrypi': 5 | from . import (utils, 6 | models, 7 | datasets, 8 | remotes, 9 | sensors, 10 | actuators, 11 | vehicles, 12 | pilots, 13 | templates,) 14 | 15 | else: 16 | print('Detected running on rasberrypi. Only importing select modules.') 17 | from . import actuators, remotes, sensors, vehicles 18 | -------------------------------------------------------------------------------- /docs/99-Contribute.md: -------------------------------------------------------------------------------- 1 | 2 | # Contribute to Donkey 3 | This is an opensource project to help accelerate the developement of self driving autos. See below how you can contribute. 4 | 5 | 1. Submit a github issue when you find a problem with the code. 6 | 2. Submit a github issue that describes a feature that you want. This is where the conversation can start. 7 | 3. Fork the code, make your change and submit a pull request. 8 | 9 | 10 | ## Tests 11 | Make sure all tests pass before submitting your pull request. 12 | 13 | Run tests with: `python -m unittest` -------------------------------------------------------------------------------- /demos/train.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of how to train a keras model from simulated images or a 3 | previously recorded session. 4 | 5 | """ 6 | 7 | import donkey as dk 8 | 9 | #Train on session pictures 10 | sh = dk.sessions.SessionHandler(sessions_path='~/donkey_data/sessions/') 11 | s = sh.load('manual_white_line') 12 | X, Y = s.load_dataset() 13 | 14 | 15 | #Train on simulated pictures 16 | #X, Y = dk.datasets.moving_square(n_frames=2000, return_x=True, return_y=False) 17 | 18 | 19 | model = dk.models.cnn3_full1() 20 | 21 | 22 | 23 | model.fit(X,Y, nb_epoch=10, validation_split=0.2) 24 | 25 | 26 | model.save('test_model') -------------------------------------------------------------------------------- /docs/02-start_driving.md: -------------------------------------------------------------------------------- 1 | ## Drive your car. 2 | 3 | Now that you have built your car, you'll want to drive it. 4 | 5 | #### Start your server 6 | 1. Open a termial and find your ip address by typing `ifconfig` 7 | 2. Start a server by running the `serve_no_pilot.py` demo script. 8 | 3. Now you can load the control page at `localhost:8887` 9 | 10 | #### Start your car 11 | 12 | 1. Open another terminal 13 | 2. Use this code to find the ip address of your pi. Change the IP base ip address to match your router. 14 | 15 | ``` 16 | sudo nmap -sP 192.168.1.0/24 | awk '/^Nmap/{ip=$NF}/B8:27:EB/{print ip}' 17 | ``` 18 | 3. Connect to your pi by running `ssh pi@` 19 | 4. Run the `drive_pi.py` demo script 20 | -------------------------------------------------------------------------------- /demos/serve_pilot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Script to start server to drive your car. 3 | """ 4 | 5 | import donkey as dk 6 | 7 | import keras 8 | 9 | #Load a trained keras model and use it in the KerasAngle pilot 10 | #model_file = '/home/wroscoe/code/donkey/whiteline_model.hdf5' 11 | model_file = '/home/ubuntu/will_car/ac_all-cnn3_full1-valloss623.82' 12 | model = keras.models.load_model(model_file) 13 | pilot = dk.pilots.KerasAngle(model=model, throttle=20) 14 | 15 | #specify where sessions data should be saved 16 | sh = dk.sessions.SessionHandler(sessions_path='~/donkey_data/sessions') 17 | session = sh.new() 18 | 19 | #start server 20 | w = dk.remotes.RemoteServer(session, pilot, port=8886) 21 | w.start() 22 | 23 | #in a browser go to localhost:8887 to drive your car 24 | -------------------------------------------------------------------------------- /demos/drive_sim.py: -------------------------------------------------------------------------------- 1 | """ 2 | Run a fake car on the same machine as your server. Fake images are 3 | created for the camera. 4 | 5 | """ 6 | 7 | 8 | import os 9 | 10 | import donkey as dk 11 | 12 | 13 | #X, Y = dk.datasets.moving_square(n_frames=1000) 14 | 15 | sh = dk.sessions.SessionHandler('/home/wroscoe/donkey_data/sessions') 16 | s = sh.load('test') 17 | X, Y = s.load_dataset() 18 | 19 | 20 | camera_sim = dk.sensors.ImgArrayCamera(X) #For testing 21 | 22 | no_steering = dk.actuators.BaseSteeringActuator() 23 | no_throttle = dk.actuators.BaseThrottleActuator() 24 | 25 | remote_pilot = dk.remotes.RemoteClient('http://localhost:8887', vehicle_id='mycar') 26 | 27 | 28 | car = dk.vehicles.BaseVehicle(camera=camera_sim, 29 | steering_actuator=no_steering, 30 | throttle_actuator=no_throttle, 31 | pilot=remote_pilot) 32 | 33 | #start the drive loop 34 | car.start() 35 | -------------------------------------------------------------------------------- /tests/test_actuators.py: -------------------------------------------------------------------------------- 1 | 2 | import unittest 3 | from donkey.actuators import map_range 4 | 5 | class TestMapping(unittest.TestCase): 6 | def test_positive(self): 7 | min = map_range(-100, -100, 100, 0, 1000) 8 | half = map_range(0, -100, 100, 0, 1000) 9 | max = map_range(100, -100, 100, 0, 1000) 10 | assert min == 0 11 | assert half == 500 12 | assert max == 1000 13 | 14 | def test_negative(self): 15 | ranges = (0, 100, 0, 1000) 16 | min = map_range(0, *ranges) 17 | half = map_range(50, *ranges) 18 | max = map_range(100, *ranges) 19 | assert min == 0 20 | assert half == 500 21 | assert max == 1000 22 | 23 | 24 | def test_reverse(self): 25 | ranges = (100, 0, 0, 1000) 26 | min = map_range(0, *ranges) 27 | half = map_range(50, *ranges) 28 | max = map_range(100, *ranges) 29 | assert min == 1000 30 | assert half == 500 31 | assert max == 0 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Donkey: a self driving library and control platform for small scale DIY vehicles. 2 | 3 | Donkey is minimalist and modular self driving library written in Python. It is developed for hobbiests with a focus on allowing fast experimentation and easy community contributions. 4 | 5 | ####Use Donkey if you want to: 6 | * [Quickly build your own self driving RC car.](docs/01-build_a_car.md) (~$200 + 5hrs). 7 | * Use existing autopilots to drive your car. 8 | * Use community datasets to create, improve and test autopilots that other people can use. 9 | 10 | 11 | #### Library Features: 12 | * Data logging of image, steering angle, & throttle outputs. 13 | * Wifi car controls (a virtual joystic). 14 | * Community contributed driving data and autopilots. 15 | * Hardware CAD designs for optional upgrades. 16 | 17 | 18 | ### Drive your car 19 | Once you have built your car you can use it like this. 20 | 21 | 1. Start the default pilot server. `python demos/serve.py` 22 | 23 | 2. Start your car and connect it to the pilot server. `python demos/drive_pi.py --remote http://:8887` 24 | 25 | 3. Go to `:8887` on your phone or computer to start driving your car. 26 | 27 | Use the [demos](demos) to see how to record driving data, train autopilots and more. 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /demos/train_generator.py: -------------------------------------------------------------------------------- 1 | """ 2 | For large sessions that won't fit in memory you'll need to use a generator. 3 | 4 | This example shows how to use a generator and only save the model that has 5 | the lowest validation loss. 6 | """ 7 | 8 | import donkey as dk 9 | from keras.callbacks import ModelCheckpoint 10 | 11 | #Read in pictures and velocities and create a predictor 12 | sh = dk.sessions.SessionHandler(sessions_path='~/donkey_data/sessions/') 13 | s = sh.load('test') 14 | 15 | 16 | #split training and test data. 17 | all_imgs = s.img_paths() 18 | train_imgs, test_imgs = dk.utils.split_list(all_imgs, test_frac=.8, sequential=True) 19 | 20 | train_gen = s.load_generator(add_dim=True, img_paths=train_imgs) 21 | test_gen = s.load_generator(add_dim=True, img_paths=test_imgs) 22 | 23 | 24 | # Use a checkpoint to save best model after each epoch 25 | filepath="best_model.hdf5" 26 | checkpoint = ModelCheckpoint(filepath, monitor='val_loss', verbose=1, 27 | save_best_only=True, mode='min') 28 | callbacks_list = [checkpoint] 29 | 30 | #use 3 layer convolution netowrk with one fully connected 31 | model = dk.models.cnn3_full1() 32 | 33 | #fit model with generator 34 | model.fit_generator(train_gen, samples_per_epoch=70, nb_epoch=8, 35 | validation_data=test_gen, nb_val_samples=100, callbacks=callbacks_list) -------------------------------------------------------------------------------- /docs/Mechanical_Hardware_Instructions.md: -------------------------------------------------------------------------------- 1 | ##Mechanical Hardware Documentation 2 | The hardware for the Donkey is fairly straight forward, however following it is pretty important. It is to everybody's benefit that we end up with same/similar training data. Only two cars are supported right now: 3 | 4 | * Helion Invictus from Hobbytown 5 | * Trooper from Hobby king (both Pro and non Pro version) 6 | 7 | We will evaluate support for other cars on an ongoing basis. 8 | 9 | ###Top Plate 10 | The top plate is two parts laser cut from 3mm material like baltic birch plywood. the two top plates are then glued together. The current top plate is for the Helion Invictus, the Trooper will be added shortly. 11 | 12 | ###Roll Cage 13 | The roll cage is an optional part printed on a 3D Printer, it can be attached to the top plate using M3 screws . While optional the roll cage does help protect electronics, but possibly more importanly provides much needed handles for the car such that you dont touch wiring connections when moving the car. 14 | 15 | ###Camera Holder 16 | There are two camera holders one is short "camera holder.stl" and one is tall and has 3 camera position options "adjustable camera holder.stl. The short one is now depricated. Adjustable camera holder has 3 positions 0, -15 and -30 degrees. The -30 degree setting is recommended for all data collection, other settings are available for testing. Both camera holders are held down with M3 screws and optionally can use a square nut to for easier tightening. -------------------------------------------------------------------------------- /donkey/vehicles.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | class BaseVehicle: 4 | def __init__(self, 5 | drive_loop_delay = .2, 6 | camera=None, 7 | steering_actuator=None, 8 | throttle_actuator=None, 9 | pilot=None): 10 | 11 | self.drive_loop_delay = drive_loop_delay #how long to wait between loops 12 | 13 | #these need tobe updated when vehicle is defined 14 | self.camera = camera 15 | self.steering_actuator = steering_actuator 16 | self.throttle_actuator = throttle_actuator 17 | self.pilot = pilot 18 | 19 | 20 | def start(self): 21 | 22 | start_time = time.time() 23 | angle = 0 24 | throttle = 0 25 | 26 | #drive loop 27 | while True: 28 | now = time.time() 29 | milliseconds = int( (now - start_time) * 1000) 30 | 31 | #get image array image from camera 32 | img_arr = self.camera.capture_arr() 33 | 34 | angle, throttle = self.pilot.decide( img_arr, 35 | angle, 36 | throttle, 37 | milliseconds) 38 | 39 | self.steering_actuator.update(angle) 40 | pulse = self.throttle_actuator.update(throttle) 41 | print(pulse) 42 | 43 | 44 | #print current car state 45 | print('angle: %s throttle: %s' %(angle, throttle) ) 46 | time.sleep(self.drive_loop_delay) 47 | -------------------------------------------------------------------------------- /demos/drive_pi.py: -------------------------------------------------------------------------------- 1 | """ 2 | Script to start controlling your car remotely via on Raspberry Pi that 3 | constantly requests directions from a remote server. See serve_no_pilot.py 4 | to start a server on your laptop. 5 | 6 | Usage: 7 | drive.py [--remote=] 8 | 9 | 10 | Options: 11 | --remote= recording session name 12 | """ 13 | 14 | import os 15 | from docopt import docopt 16 | 17 | import donkey as dk 18 | 19 | 20 | 21 | # Get args. 22 | args = docopt(__doc__) 23 | 24 | 25 | if __name__ == '__main__': 26 | 27 | remote_url = args['--remote'] 28 | 29 | 30 | #Set up your PWM values for your steering and throttle actuator here. 31 | mythrottle = dk.actuators.PWMThrottleActuator(channel=0, 32 | min_pulse=280, 33 | max_pulse=490, 34 | zero_pulse=350) 35 | 36 | mysteering = dk.actuators.PWMSteeringActuator(channel=1, 37 | left_pulse=300, 38 | right_pulse=400) 39 | 40 | #asych img capture from picamera 41 | mycamera = dk.sensors.PiVideoStream() 42 | 43 | #Get all autopilot signals from remote host 44 | mypilot = dk.remotes.RemoteClient(remote_url, vehicle_id='mycar') 45 | 46 | #Create your car your car 47 | car = dk.vehicles.BaseVehicle(camera=mycamera, 48 | steering_actuator=mysteering, 49 | throttle_actuator=mythrottle, 50 | pilot=mypilot) 51 | 52 | 53 | #Start the drive loop 54 | car.start() 55 | -------------------------------------------------------------------------------- /donkey/pilots.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 3 | Methods to create, use, save and load pilots. Pilots 4 | contain the highlevel logic used to determine the angle 5 | and throttle of a vehicle. Pilots can include one or more 6 | models to help direct the vehicles motion. 7 | 8 | ''' 9 | import os 10 | import numpy as np 11 | 12 | import random 13 | 14 | from donkey import utils 15 | 16 | class BasePilot(): 17 | ''' 18 | Base class to define common functions. 19 | When creating a class, only override the funtions you'd like to replace. 20 | ''' 21 | 22 | def decide(self, img_arr): 23 | angle = 0 24 | speed = 0 25 | 26 | #Do prediction magic 27 | 28 | return angle, speed 29 | 30 | 31 | class SwervePilot(BasePilot): 32 | ''' 33 | Example predictor that should not be used. 34 | ''' 35 | def __init__(self): 36 | self.angle= random.randrange(-45, 46) 37 | self.throttle = 20 38 | 39 | 40 | def decide(self, img_arr): 41 | 42 | new_angle = self.angle + random.randrange(-4, 5) 43 | self.angle = min(max(-45, new_angle), 45) 44 | 45 | return self, angle, self.throttle 46 | 47 | 48 | class KerasAngle(): 49 | def __init__(self, model, throttle): 50 | self.model = model 51 | self.throttle = throttle 52 | self.last_angle = 0 53 | 54 | def decide(self, img_arr): 55 | img_arr = img_arr.reshape((1,) + img_arr.shape) 56 | angle = self.model.predict(img_arr) 57 | angle = angle[0][0] 58 | 59 | #add some smoothing 60 | a = .8 61 | angel = a * angle + (1-a) * self.last_angle 62 | self.last_angle = angle 63 | 64 | return angle, self.throttle 65 | 66 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup(name='donkey', 4 | version='0.01', 5 | description='A library for small scale DIY self driving cars', 6 | url='https//github.com/wroscoe/donkey', 7 | author='Will Roscoe', 8 | author_email='wroscoe@gmail.com', 9 | license='MIT', 10 | 11 | classifiers=[ 12 | # How mature is this project? Common values are 13 | # 3 - Alpha 14 | # 4 - Beta 15 | # 5 - Production/Stable 16 | 'Development Status :: 3 - Alpha', 17 | 18 | # Indicate who your project is intended for 19 | 'Intended Audience :: Developers', 20 | 'Topic :: Scientific/Engineering :: Artificial Intelligence', 21 | 22 | # Pick your license as you wish (should match "license" above) 23 | 'License :: OSI Approved :: MIT License', 24 | 25 | # Specify the Python versions you support here. In particular, ensure 26 | # that you indicate whether you support Python 2, Python 3 or both. 27 | 28 | 'Programming Language :: Python :: 3.3', 29 | 'Programming Language :: Python :: 3.4', 30 | 'Programming Language :: Python :: 3.5', 31 | 'Programming Language :: Python :: 3.6', 32 | ], 33 | keywords='selfdriving cars drive', 34 | 35 | install_requires=['numpy', 36 | 'pillow', 37 | 'docopt==0.6.2', 38 | 'tornado', 39 | 'requests' 40 | ], 41 | 42 | extras_require={'server': [ 43 | 'keras', 44 | 'h5py', 45 | 'envoy', 46 | 'scikit-image' 47 | ], 48 | 'pi': [ 49 | 'picamera', 50 | 'Adafruit_PCA9685', 51 | ] 52 | }, 53 | 54 | packages=find_packages(exclude=(['tests', 'docs', 'env'])), 55 | ) 56 | -------------------------------------------------------------------------------- /donkey/datasets.py: -------------------------------------------------------------------------------- 1 | from os.path import dirname, realpath 2 | import os 3 | import PIL 4 | import numpy as np 5 | import pickle 6 | 7 | import requests 8 | import io 9 | 10 | from .sessions import SessionHandler 11 | 12 | 13 | def save_dataset(X, Y, file_path): 14 | with open(file_path, 'wb') as f: 15 | pickel.dump((X, Y), f) 16 | 17 | 18 | def load_file(file_path): 19 | with open(file_path, 'rb') as f: 20 | X, Y = pickle.load(f) 21 | return X,Y 22 | 23 | 24 | def load_url(url): 25 | r = requests.get(url) 26 | f = io.BytesIO(r.content) 27 | X, Y = pickle.load(f) 28 | return X, Y 29 | 30 | 31 | def moving_square(n_frames=100, return_x=True, return_y=True): 32 | 33 | ''' 34 | Generate sequence of images of square bouncing around 35 | the image and labels of its coordinates. Can be used as a 36 | basic simple performance test of convolution networks. 37 | ''' 38 | 39 | row = 120 40 | col = 160 41 | movie = np.zeros((n_frames, row, col, 3), dtype=np.float) 42 | labels = np.zeros((n_frames, 2), dtype=np.float) 43 | 44 | #initial possition 45 | x = np.random.randint(20, col-20) 46 | y = np.random.randint(20, row-20) 47 | 48 | # Direction of motion 49 | directionx = -1 50 | directiony = 1 51 | 52 | # Size of the square 53 | w = 4 54 | 55 | for t in range(n_frames): 56 | #move 57 | x += directionx 58 | y += directiony 59 | 60 | #make square bounce off walls 61 | if y < 5 or y > row-5: 62 | directiony *= -1 63 | if x < 5 or x > col-5: 64 | directionx *= -1 65 | 66 | #draw square and record labels 67 | movie[t, y - w: y + w, x - w: x + w, 1] += 1 68 | labels[t] = np.array([x, y]) 69 | 70 | #convert array to dtype that PIL.Image accepts 71 | #and scale it to 0-256 72 | movie = np.uint8(movie*255.999) 73 | 74 | #only return requested labels 75 | if return_x and return_y: 76 | return movie, labels 77 | elif return_x and not return_y: 78 | return movie, labels[:,0] 79 | else: 80 | return movie, labels[:,1] 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /donkey/templates/monitor.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Donkey Monitor 6 | 7 | 8 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 |
23 |

Donkey Control

24 | Use this website to drive your vehicle. Data will only be recorded when you are actively steering. 25 | 26 |
27 | 28 |
29 |
30 |

Image Stream (MJPEG)

31 | 32 |
33 |
34 | 35 |
36 | 37 |
38 |
39 | 40 |
41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 |
Value
62 |
63 | 64 |
65 |
66 | 67 | 68 |
69 | 70 |
71 | Click/touch to use joystic. 72 |
73 | 74 |
75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /demos/train_model_search.py: -------------------------------------------------------------------------------- 1 | """ 2 | A script designed to run train multiple models on multiple datasets. This 3 | can be helpful if you use a paid GPU instance from amazon and want to use 4 | the resource efficiently. 5 | 6 | Features: 7 | * Saves best model each epoch. 8 | * 9 | """ 10 | 11 | import os 12 | 13 | from operator import itemgetter 14 | 15 | import donkey as dk 16 | from keras.callbacks import ModelCheckpoint 17 | 18 | #sh = dk.sessions.SessionHandler('/home/wroscoe/donkey_data/sessions/') 19 | #s = sh.load('test') 20 | 21 | #datasets we want to test 22 | 23 | #datasets = {'square': dk.datasets.moving_square(1000, return_y=False)} 24 | datasets = {'square': dk.datasets.load_file('/home/wroscoe/code/notebooks/whiteline_dataset.pkl')} 25 | 26 | 27 | #the models we want to test 28 | models = { 29 | #'cnn3_full1_rnn1': dk.models.cnn3_full1_rnn1(), 30 | 'cnn3_full1': dk.models.cnn3_full1(), 31 | #'norm_cnn3_full1': dk.models.norm_cnn3_full1(), 32 | 'norm_cnn3_full1': dk.models.norm_cnn3_full1(), 33 | #'cnn1_full1': dk.models.cnn1_full1(), 34 | } 35 | 36 | 37 | results = [] 38 | 39 | for ds_name, ds in datasets.items(): 40 | print('Using dataset: %s' %ds_name) 41 | 42 | for m_name, m in models.items(): 43 | print('Using model: %s' %m_name) 44 | 45 | filepath="best-weights.hdf5" 46 | checkpoint = ModelCheckpoint(filepath, monitor='val_loss', verbose=1, 47 | save_best_only=True, mode='min') 48 | callbacks_list = [checkpoint] 49 | 50 | X, Y = ds 51 | 52 | hist = m.fit(X, Y, batch_size=32, nb_epoch=10, 53 | validation_split=.2, callbacks=callbacks_list) 54 | 55 | print(hist.history) 56 | result = {'model_name': m_name, 'dataset_name': ds_name, 57 | 'min_val_loss': min(hist.history['val_loss']) 58 | } 59 | results.append(result) 60 | 61 | 62 | val_loss = '{0:.2f}'.format(min(hist.history['val_loss'])) 63 | new_filepath = ds_name + '-' + m_name + '-valloss' + str(val_loss) 64 | os.rename(filepath, new_filepath) 65 | 66 | print(results) 67 | 68 | #try using this ami: vict0rsch-1.0 69 | 70 | #git clone https://github.com/wroscoe/donkey.git 71 | #git fetch origin wr/amazon_test 72 | #git checkout wr/amazon_test 73 | 74 | #virtualenv -p python3 env 75 | #source env/bin/activate 76 | 77 | #pip install -e . 78 | #pip install keras tensorflow-gpu scikit-image 79 | #python demos/amazon_test.py 80 | 81 | #install cudu: https://developer.nvidia.com/cuda-downloads 82 | -------------------------------------------------------------------------------- /docs/01-build_a_car.md: -------------------------------------------------------------------------------- 1 | # Get Started 2 | This doc will walk you through how to setup your donkey. 3 | 4 | 5 | ## Bill of Materials. 6 | #### Required 7 | * Raspberry Pi 3 B ($35) 8 | * Raspberry Pi Camera ($15) 9 | * Micro USB power adapter. ($9) 10 | * USB Battery Pack ($15) 11 | * USB Keyboard 12 | * USB Mouse 13 | * Monitor 14 | * HDMI cable (to connect monitor to PI) ($7) 15 | * Micro SD Card (campatible with Raspberry Pi) ($8) 16 | * Servo Sheild ($15) 17 | * RC CAR (($120-300)) 18 | 19 | #### Optional 20 | * Xbox 360 Controller 21 | * Xbox USB Adapter 22 | 23 | 24 | 25 | ## Setup 26 | These instructions are based on [Geoff Boeing's post on setting up scientific python on a Raspberry Pi](http://geoffboeing.com/2016/03/scientific-python-raspberry-pi/). 27 | 28 | #### Boot your Raspberry Pi 29 | These instructions assume you're using Ubuntu operationg system. If you don't have Ubuntu, try using the NOOB method. 30 | 31 | 1. Download recent Disk image. I use Jessie. 32 | 2. Extract disk image from zip file. 33 | 3. Open Ubuntu's "Startup Disk Creator" application. 34 | 4. Insert micro usb disk via a usb adapter. This disk should show up in the Startup Disk Creator app. 35 | 5. Select your RPi .img file and click create disk. 36 | 6. Once the img has been created, take the SD card from your computer and put it in your RPi. 37 | 7. Connect your Monitor with your HDMI cable, your mouse and keyboard and then finally power up the RPi by plugging in the Micro USB power adaptor. 38 | 39 | 40 | #### Install Basic Libraries 41 | Since the RPi is not as powerful as a laptop, it can take a long time to install python packages (ie. numpy & PIL) using pip. Luckly Adafruit has precompiled these libraries into packages that can be installed via `apt-get`. 42 | 43 | 1. Open a terminal (Ctrl-Alt-t) and upgrade your system packages. 44 | 45 | ``` 46 | sudo apt-get update 47 | sudo apt-get upgrade 48 | ``` 49 | 2. Save These initial conditions. 50 | 51 | ``` 52 | dpkg -l > ~/Desktop/packages.list 53 | pip freeze > ~/Desktop/pip-freeze-initial.list 54 | ``` 55 | 56 | 3. Install the necessary libraries 57 | 58 | ``` 59 | sudo apt-get install xsel xclip libxml2-dev libxslt-dev libzmq-dev libspatialindex-dev virtualenv 60 | ``` 61 | 4. Pandas & Jupyter Requirements 62 | ``` 63 | sudo apt-get install python3-lxml python3-h5py python3-numexpr python3-dateutil python3-tz python3-bs4 python3-xlrd python3-tables python3-sqlalchemy python3-xlsxwriter python3-httplib2 python3-zmq 64 | ``` 65 | 5. Scientific Python 66 | ``` 67 | sudo apt-get install python3-numpy python3-matplotlib python3-scipy python3-pandas 68 | ``` 69 | 70 | #### Install your Camera 71 | Follow the instructions [here](https://www.raspberrypi.org/learning/getting-started-with-picamera/worksheet/). 72 | 73 | 74 | 1. Connect your camera to the RPi. 75 | 2. Enable your camera in the Menu > Preferences > Raspberry Pi Config 76 | 3. Restart your Pi. `sudo reboot` 77 | 78 | 79 | #### Connect your servo sheild. 80 | 81 | 1. Assemble and test your servo shield with the instructions given by Adafruit. 82 | 83 | 84 | #### Install Donkey 85 | 86 | 87 | clone repo & create virtual env 88 | 89 | ``` 90 | mkdir code 91 | cd code 92 | git clone http://github.com/wroscoe/donkey.git 93 | 94 | mkdir car 95 | cd car 96 | virtualenv --system-site-packages -p python3 env 97 | source env/bin/activate 98 | pip install -e ../donkey[pi] 99 | ``` 100 | 101 | 102 | -------------------------------------------------------------------------------- /donkey/actuators.py: -------------------------------------------------------------------------------- 1 | # Simple car movement using the PCA9685 PWM servo/LED controller library. 2 | # 3 | # Attribution: hacked from sample code from Tony DiCola 4 | 5 | import time 6 | import sys 7 | 8 | # Import the PCA9685 module. 9 | 10 | 11 | 12 | # Uncomment to enable debug output. 13 | #import logging 14 | #logging.basicConfig(level=logging.DEBUG) 15 | 16 | 17 | def map_range(x, X_min, X_max, Y_min, Y_max): 18 | X_range = X_max - X_min 19 | Y_range = Y_max - Y_min 20 | XY_ratio = X_range/Y_range 21 | 22 | y = ((x-X_min) / XY_ratio + Y_min) // 1 23 | 24 | return int(y) 25 | 26 | 27 | class BaseSteeringActuator(): 28 | ''' Placeholder until real logic is implemented ''' 29 | 30 | def update(self, angle): 31 | print('BaseSteeringActuator.update: angle=%s' %angle) 32 | 33 | class BaseThrottleActuator(): 34 | ''' Placeholder until real logic is implemented ''' 35 | 36 | def update(self, throttle): 37 | print('BaseThrottleActuator.update: throttle=%s' %throttle) 38 | 39 | 40 | class Adafruit_PCA9685_Actuator(): 41 | 42 | def __init__(self, channel, frequency=60): 43 | import Adafruit_PCA9685 44 | # Initialise the PCA9685 using the default address (0x40). 45 | self.pwm = Adafruit_PCA9685.PCA9685() 46 | 47 | # Set frequency to 60hz, good for servos. 48 | self.pwm.set_pwm_freq(frequency) 49 | self.channel = channel 50 | 51 | 52 | class PWMSteeringActuator(Adafruit_PCA9685_Actuator): 53 | 54 | #max angle wheels can turn 55 | LEFT_ANGLE = -45 56 | RIGHT_ANGLE = 45 57 | 58 | def __init__(self, channel=1, 59 | frequency=60, 60 | left_pulse=290, 61 | right_pulse=490): 62 | 63 | super().__init__(channel, frequency) 64 | self.left_pulse = left_pulse 65 | self.right_pulse = right_pulse 66 | 67 | def update(self, angle): 68 | #map absolute angle to angle that vehicle can implement. 69 | pulse = map_range(angle, 70 | self.LEFT_ANGLE, self.RIGHT_ANGLE, 71 | self.left_pulse, self.right_pulse) 72 | 73 | self.pwm.set_pwm(self.channel, 0, pulse) 74 | 75 | 76 | 77 | class PWMThrottleActuator(Adafruit_PCA9685_Actuator): 78 | 79 | MIN_THROTTLE = -100 80 | MAX_THROTTLE = 100 81 | 82 | def __init__(self, channel=0, 83 | frequency=60, 84 | max_pulse=300, 85 | min_pulse=490, 86 | zero_pulse=350): 87 | 88 | super().__init__(channel, frequency) 89 | self.max_pulse = max_pulse 90 | self.min_pulse = min_pulse 91 | self.zero_pulse = zero_pulse 92 | self.calibrate() 93 | 94 | 95 | def calibrate(self): 96 | #Calibrate ESC (TODO: THIS DOES NOT WORK YET) 97 | print('center: %s' % self.zero_pulse) 98 | self.pwm.set_pwm(self.channel, 0, self.zero_pulse) #Set Max Throttle 99 | time.sleep(1) 100 | 101 | 102 | def update(self, throttle): 103 | print('throttle update: %s' %throttle) 104 | if throttle > 0: 105 | pulse = map_range(throttle, 106 | 0, self.MAX_THROTTLE, 107 | self.zero_pulse, self.max_pulse) 108 | else: 109 | pulse = map_range(throttle, 110 | self.MIN_THROTTLE, 0, 111 | self.min_pulse, self.zero_pulse) 112 | 113 | print('pulse: %s' % pulse) 114 | sys.stdout.flush() 115 | self.pwm.set_pwm(self.channel, 0, pulse) 116 | return '123' 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /donkey/sensors.py: -------------------------------------------------------------------------------- 1 | 2 | import io 3 | import os 4 | import time 5 | from threading import Thread 6 | from itertools import cycle 7 | 8 | import numpy as np 9 | 10 | from PIL import Image 11 | 12 | from donkey import utils 13 | 14 | 15 | class BaseCamera: 16 | 17 | def __init__(self, resolution=(160, 120)): 18 | self.resolution = resolution 19 | self.frame = np.zeros(shape=(self.resolution[1], self.resolution[0], 3)) 20 | 21 | def start(self): 22 | # start the thread to read frames from the video stream 23 | t = Thread(target=self.update, args=()) 24 | t.daemon = True 25 | t.start() 26 | time.sleep(1) 27 | return self 28 | 29 | def update(self): 30 | while True: 31 | pass 32 | 33 | def read(self): 34 | return self.frame 35 | 36 | def capture_arr(self): 37 | return self.read() 38 | 39 | def capture_img(self): 40 | arr = self.capture_arr() 41 | print(type(arr)) 42 | img = Image.fromarray(arr, 'RGB') 43 | return img 44 | 45 | def capture_binary(self): 46 | img = self.capture_img() 47 | return utils.img_to_binary(img) 48 | 49 | 50 | 51 | 52 | class PiVideoStream(BaseCamera): 53 | def __init__(self, resolution=(160, 120), framerate=12): 54 | from picamera.array import PiRGBArray 55 | from picamera import PiCamera 56 | 57 | # initialize the camera and stream 58 | self.camera = PiCamera() 59 | self.camera.resolution = resolution 60 | self.camera.framerate = framerate 61 | self.rawCapture = PiRGBArray(self.camera, size=resolution) 62 | self.stream = self.camera.capture_continuous(self.rawCapture, 63 | format="bgr", use_video_port=True) 64 | 65 | # initialize the frame and the variable used to indicate 66 | # if the thread should be stopped 67 | self.frame = None 68 | self.stopped = False 69 | 70 | print('PiVideoStream loaded.. .warming camera') 71 | 72 | time.sleep(2) 73 | self.start() 74 | 75 | 76 | 77 | def update(self): 78 | # keep looping infinitely until the thread is stopped 79 | for f in self.stream: 80 | # grab the frame from the stream and clear the stream in 81 | # preparation for the next frame 82 | self.frame = f.array 83 | self.rawCapture.truncate(0) 84 | 85 | # if the thread indicator variable is set, stop the thread 86 | # and resource camera resources 87 | if self.stopped: 88 | self.stream.close() 89 | self.rawCapture.close() 90 | self.camera.close() 91 | return 92 | 93 | 94 | def stop(self): 95 | # indicate that the thread should be stopped 96 | self.stopped = True 97 | 98 | 99 | class ImgArrayCamera(BaseCamera): 100 | 101 | def __init__(self, X): 102 | self.X = X 103 | self.frame = X[0] 104 | self.start() 105 | 106 | 107 | def generator(self): 108 | while True: 109 | for i in self.X: 110 | yield i 111 | 112 | def update(self): 113 | # keep looping infinitely until the thread is stopped 114 | for x in self.generator(): 115 | # grab the frame from the stream and clear the stream in 116 | # preparation for the next frame 117 | self.frame = x 118 | time.sleep(.2) 119 | 120 | 121 | 122 | 123 | class FakeCamera(BaseCamera): 124 | ''' 125 | Class that acts like a PiCamera but reads files from a dir. 126 | Used for testing on non-Pi devices. 127 | ''' 128 | def __init__(self, img_paths, **kwargs): 129 | print('loading FakeCamera') 130 | 131 | self.file_list = img_paths 132 | self.file_list.sort() 133 | self.file_cycle = cycle(self.file_list) #create infinite iterator 134 | self.counter = 0 135 | 136 | # if the thread should be stopped 137 | self.frame = None 138 | self.start() 139 | 140 | 141 | def update(self): 142 | # keep looping infinitely until the thread is stopped 143 | for f in self.file_cycle: 144 | # grab the frame from the stream and clear the stream in 145 | # preparation for the next frame 146 | self.frame = np.array(Image.open(f)) 147 | self.counter += 1 148 | time.sleep(.2) 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /donkey/templates/static/main.js: -------------------------------------------------------------------------------- 1 | 2 | $( document ).ready(function() { 3 | console.log( "document ready!" ); 4 | velocity.bind() 5 | 6 | var joystick_options = { 7 | zone: document.getElementById('joystick_container'), // active zone 8 | color: '#668AED', 9 | size: 350, 10 | }; 11 | 12 | var manager = nipplejs.create(joystick_options); 13 | 14 | bindNipple(manager); 15 | 16 | }); 17 | 18 | 19 | //Defaults 20 | var postLoopRunning=false; 21 | var sendURL = "control/" 22 | 23 | 24 | function sendControl(angle, throttle, drive_mode, recording) { 25 | //Send post request to server. 26 | data = JSON.stringify({ 'angle': angle, 27 | 'throttle':throttle, 28 | 'drive_mode':drive_mode, 29 | 'recording':recording}) 30 | $.post(sendURL, data) 31 | } 32 | 33 | 34 | // Send control updates to the server every .1 seconds. 35 | function postLoop () { 36 | setTimeout(function () { 37 | sendControl($('#angleInput').val(), 38 | $('#throttleInput').val(), 39 | 'user', 40 | 'true') 41 | 42 | if (postLoopRunning) { 43 | postLoop(); 44 | } else { 45 | // Send zero angle, throttle and stop recording 46 | sendControl(0, 0, 'user', 'false') 47 | } 48 | }, 100) 49 | } 50 | 51 | 52 | function bindNipple(manager) { 53 | manager.on('start end', function(evt, data) { 54 | $('#angleInput').val(0); 55 | $('#throttleInput').val(0); 56 | 57 | if (!postLoopRunning) { 58 | postLoopRunning=true; 59 | postLoop(); 60 | } else { 61 | postLoopRunning=false; 62 | } 63 | 64 | }).on('move', function(evt, data) { 65 | angle = data['angle']['radian'] 66 | distance = data['distance'] 67 | 68 | $('#angleInput').val(Math.round(distance * Math.cos(angle)/2)); 69 | $('#throttleInput').val(Math.round(distance * Math.sin(angle)/2)); 70 | }); 71 | } 72 | 73 | 74 | $(document).keydown(function(e) { 75 | if(e.which == 73) { 76 | // 'i' throttle up 77 | velocity.throttleUp() 78 | } 79 | 80 | if(e.which == 75) { 81 | // 'k' slow down 82 | velocity.throttleDown() 83 | } 84 | 85 | if(e.which == 74) { 86 | // 'j' turn left 87 | velocity.angleLeft() 88 | } 89 | 90 | if(e.which == 76) { 91 | // 'l' turn right 92 | velocity.angleRight() 93 | } 94 | 95 | if(e.which == 65) { 96 | // 'a' turn on auto mode 97 | velocity.updateDriveMode('auto') 98 | } 99 | if(e.which == 68) { 100 | // 'a' turn on auto mode 101 | velocity.updateDriveMode('user') 102 | } 103 | if(e.which == 83) { 104 | // 'a' turn on auto mode 105 | velocity.updateDriveMode('auto_angle') 106 | } 107 | }); 108 | 109 | 110 | var velocity = (function() { 111 | //functions to change velocity of vehicle 112 | 113 | var angle = 0 114 | var throttle = 0 115 | var driveMode = 'user' 116 | var angleEl = "#angleInput" 117 | var throttleEl = "#throttleInput" 118 | var sendURL = "control/" 119 | 120 | var bind = function(data){ 121 | //Bind a function to capture the coordinates of the click. 122 | $(angleEl).change(function(e) { 123 | sendVelocity() 124 | }); 125 | $(throttleEl).change(function(e) { 126 | sendVelocity() 127 | }); 128 | }; 129 | 130 | var sendVelocity = function() { 131 | //Send angle and throttle values 132 | data = JSON.stringify({ 'angle': angle, 'throttle':throttle, 'drive_mode':driveMode, 'recording':'false'}) 133 | $.post(sendURL, data) 134 | }; 135 | 136 | var throttleUp = function(){ 137 | //Bind a function to capture the coordinates of the click. 138 | throttle = Math.min(throttle + 5, 400); 139 | $(throttleEl).val(throttle) 140 | sendVelocity() 141 | }; 142 | 143 | var throttleDown = function(){ 144 | //Bind a function to capture the coordinates of the click. 145 | throttle = Math.max(throttle - 10, -200); 146 | $(throttleEl).val(throttle); 147 | sendVelocity() 148 | }; 149 | 150 | var angleLeft = function(){ 151 | //Bind a function to capture the coordinates of the click. 152 | angle = Math.max(angle - 10, -90) 153 | $(angleEl).val(angle); 154 | sendVelocity() 155 | }; 156 | 157 | var angleRight = function(){ 158 | //Bind a function to capture the coordinates of the click. 159 | angle = Math.min(angle + 10, 90) 160 | $(angleEl).val(angle); 161 | sendVelocity() 162 | }; 163 | 164 | var updateDriveMode = function(mode){ 165 | //Bind a function to capture the coordinates of the click. 166 | driveMode = mode; 167 | $('#driveMode').val(mode); 168 | sendVelocity() 169 | }; 170 | 171 | return { 172 | bind: bind, 173 | throttleUp: throttleUp, 174 | throttleDown: throttleDown, 175 | angleLeft: angleLeft, 176 | angleRight: angleRight, 177 | sendVelocity: sendVelocity, 178 | updateDriveMode: updateDriveMode 179 | }; 180 | })(); 181 | -------------------------------------------------------------------------------- /donkey/utils.py: -------------------------------------------------------------------------------- 1 | import random 2 | import pickle 3 | import math 4 | from io import BytesIO 5 | import os 6 | import glob 7 | import socket 8 | 9 | from PIL import Image 10 | import numpy as np 11 | 12 | import envoy 13 | 14 | 15 | 16 | ''' 17 | IMAGES 18 | ''' 19 | 20 | def scale(im, size=128): 21 | ''' 22 | accepts: PIL image, size of square sides 23 | returns: PIL image scaled so sides lenght = size 24 | ''' 25 | 26 | size = (size,size) 27 | im.thumbnail(size, Image.ANTIALIAS) 28 | return im 29 | 30 | 31 | def img_to_binary(img): 32 | ''' 33 | accepts: PIL image 34 | returns: binary stream (used to save to database) 35 | ''' 36 | 37 | f = BytesIO() 38 | img.save(f, format='jpeg') 39 | return f.getvalue() 40 | 41 | 42 | def arr_to_binary(arr): 43 | ''' 44 | accepts: numpy array with shape (Hight, Width, Channels) 45 | returns: binary stream (used to save to database) 46 | ''' 47 | img = arr_to_img(arr) 48 | return img_to_binary(img) 49 | 50 | 51 | def arr_to_img(arr): 52 | ''' 53 | accepts: numpy array with shape (Hight, Width, Channels) 54 | returns: binary stream (used to save to database) 55 | ''' 56 | arr = np.uint8(arr) 57 | img = Image.fromarray(arr) 58 | return img 59 | 60 | def img_to_arr(img): 61 | ''' 62 | accepts: numpy array with shape (Hight, Width, Channels) 63 | returns: binary stream (used to save to database) 64 | ''' 65 | return np.array(img) 66 | 67 | 68 | def binary_to_img(binary): 69 | ''' 70 | accepts: binary file object from BytesIO 71 | returns: PIL image 72 | ''' 73 | img = BytesIO(binary) 74 | return Image.open(img) 75 | 76 | 77 | def create_video(img_dir_path, output_video_path): 78 | 79 | # Setup path to the images with telemetry. 80 | full_path = os.path.join(img_dir_path, 'frame_*.png') 81 | 82 | # Run ffmpeg. 83 | command = ("""ffmpeg 84 | -framerate 30/1 85 | -pattern_type glob -i '%s' 86 | -c:v libx264 87 | -r 15 88 | -pix_fmt yuv420p 89 | -y 90 | %s""" % (full_path, output_video_path)) 91 | response = envoy.run(command) 92 | 93 | 94 | 95 | 96 | ''' 97 | DATASETS 98 | ''' 99 | 100 | def split_data(X, Y, test_frac=.8): 101 | count = len(X) 102 | assert len(X) == len(Y) 103 | 104 | cutoff = int((count * test_frac) // 1) 105 | 106 | X_train = X[:cutoff] 107 | Y_train = Y[:cutoff] 108 | 109 | X_test = X[cutoff:] 110 | Y_test = Y[cutoff:] 111 | 112 | return X_train, Y_train, X_test, Y_test 113 | 114 | 115 | def split_list(L, sequential=False, test_frac=.8): 116 | 117 | count = len(L) 118 | cutoff = int((count * test_frac) // 1) 119 | 120 | if sequential == False: 121 | random.shuffle(L) 122 | 123 | L_train = L[:cutoff] 124 | L_test = L[cutoff:] 125 | 126 | return L_train, L_test 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | ''' 138 | file utilities 139 | ''' 140 | 141 | 142 | def most_recent_file(dir_path, ext=''): 143 | ''' 144 | return the most recent file given a directory path and extension 145 | ''' 146 | query = dir_path + '/*' + ext 147 | newest = min(glob.iglob(query), key=os.path.getctime) 148 | return newest 149 | 150 | 151 | def make_dir(path): 152 | real_path = os.path.expanduser(path) 153 | if not os.path.exists(real_path): 154 | os.makedirs(real_path) 155 | return real_path 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | ''' 167 | BINNING 168 | functions to help converte between floating point numbers and categories. 169 | ''' 170 | 171 | def log_bin(a, has_negative=True): 172 | """ 173 | Returns bin number (between 0 and 14) of a number 174 | num (between -100 and 100). 175 | 176 | If has_negative == True: bin range = 0-14 177 | If has_negative == False: bin range = 0-7 178 | 179 | """ 180 | b = int(round(math.copysign(math.log(abs(a) + 1, 2.0), a))) 181 | if has_negative: 182 | b = b + 7 183 | return b 184 | 185 | 186 | def log_unbin(b, has_negative=True): 187 | if has_negative: 188 | b = b - 7 189 | 190 | a = math.copysign(2 ** abs(b), b) - 1 191 | return a 192 | 193 | 194 | def bin_telemetry(angle, throttle): 195 | #convert angle between -90 (left) and 90 (right) into a 15 bin array. 196 | a_arr = np.zeros(15, dtype='float') 197 | a_arr[log_bin(angle)] = 1 198 | 199 | #convert throttle between 0 (stopped) and 100 (full throttle) into a 5 bin array. 200 | t_arr = np.zeros(7, dtype='float') 201 | t_arr[log_bin(throttle, has_negative=False)] = 1 202 | 203 | y = np.concatenate([a_arr, t_arr]) 204 | 205 | #return array containing both angle and throttle bins. 206 | #y.shape = (15+6) 207 | return y 208 | 209 | 210 | 211 | def unbin_telemetry(y): 212 | #convert binned telemetry array to angle and throttle 213 | a_arr = y[:15] 214 | t_arr = y[15:] 215 | 216 | angle = log_unbin(np.argmax(a_arr)) #not 90 so 0 angle is possible 217 | print(np.argmax(a_arr)) 218 | throttle = log_unbin(np.argmax(t_arr), has_negative=False) 219 | 220 | return angle, throttle 221 | 222 | 223 | 224 | 225 | 226 | ''' 227 | NETWORKING 228 | ''' 229 | 230 | def my_ip(): 231 | s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 232 | s.connect(('192.0.0.8', 1027)) 233 | return s.getsockname()[0] -------------------------------------------------------------------------------- /donkey/models.py: -------------------------------------------------------------------------------- 1 | """ 2 | Keras model constructors. 3 | 4 | All models accept 120x160x3 images and output a 5 | single floating point number (ie steering angle) 6 | 7 | """ 8 | 9 | from keras.layers import Input, LSTM, Dense, merge 10 | from keras.models import Model 11 | from keras.models import Sequential 12 | from keras.layers import Convolution2D, MaxPooling2D, SimpleRNN, Reshape, BatchNormalization 13 | from keras.layers import Activation, Dropout, Flatten, Dense 14 | 15 | def cnn3_full1(): 16 | 17 | img_in = Input(shape=(120, 160, 3), name='img_in') 18 | angle_in = Input(shape=(1,), name='angle_in') 19 | 20 | x = Convolution2D(8, 3, 3)(img_in) 21 | x = Activation('relu')(x) 22 | x = MaxPooling2D(pool_size=(2, 2))(x) 23 | 24 | x = Convolution2D(16, 3, 3)(x) 25 | x = Activation('relu')(x) 26 | x = MaxPooling2D(pool_size=(2, 2))(x) 27 | 28 | x = Convolution2D(32, 3, 3)(x) 29 | x = Activation('relu')(x) 30 | x = MaxPooling2D(pool_size=(2, 2))(x) 31 | 32 | merged = Flatten()(x) 33 | 34 | x = Dense(256)(merged) 35 | x = Activation('linear')(x) 36 | x = Dropout(.2)(x) 37 | 38 | angle_out = Dense(1, name='angle_out')(x) 39 | 40 | model = Model(input=[img_in], output=[angle_out]) 41 | model.compile(optimizer='adam', loss='mean_squared_error') 42 | 43 | return model 44 | 45 | 46 | def cnn3_full1_rnn1(): 47 | 48 | img_in = Input(shape=(120, 160, 3), name='img_in') 49 | angle_in = Input(shape=(1,), name='angle_in') 50 | 51 | x = Convolution2D(8, 3, 3)(img_in) 52 | x = Activation('relu')(x) 53 | x = MaxPooling2D(pool_size=(2, 2))(x) 54 | 55 | x = Convolution2D(16, 3, 3)(x) 56 | x = Activation('relu')(x) 57 | x = MaxPooling2D(pool_size=(2, 2))(x) 58 | 59 | x = Convolution2D(32, 3, 3)(x) 60 | x = Activation('relu')(x) 61 | x = MaxPooling2D(pool_size=(2, 2))(x) 62 | 63 | merged = Flatten()(x) 64 | 65 | x = Dense(256)(merged) 66 | x = Activation('relu')(x) 67 | x = Dropout(.2)(x) 68 | 69 | x = Reshape((1, 256))(x) 70 | x = SimpleRNN(256, activation='linear')(x) 71 | 72 | throttle_out = Dense(1, name='throttle_out')(x) 73 | angle_out = Dense(1, name='angle_out')(x) 74 | 75 | model = Model(input=[img_in], output=[angle_out]) 76 | model.compile(optimizer='adam', loss='mean_squared_error') 77 | return model 78 | 79 | 80 | def cnn1_full1(): 81 | 82 | img_in = Input(shape=(120, 160, 3), name='img_in') 83 | angle_in = Input(shape=(1,), name='angle_in') 84 | 85 | x = Convolution2D(1, 3, 3)(img_in) 86 | x = Activation('relu')(x) 87 | x = MaxPooling2D(pool_size=(2, 2))(x) 88 | 89 | merged = Flatten()(x) 90 | 91 | x = Dense(32)(merged) 92 | x = Activation('linear')(x) 93 | x = Dropout(.05)(x) 94 | 95 | angle_out = Dense(1, name='angle_out')(x) 96 | 97 | model = Model(input=[img_in], output=[angle_out]) 98 | model.compile(optimizer='adam', loss='mean_squared_error') 99 | return model 100 | 101 | 102 | def norm_cnn3_full1(): 103 | 104 | img_in = Input(shape=(120, 160, 3), name='img_in') 105 | angle_in = Input(shape=(1,), name='angle_in') 106 | 107 | x = BatchNormalization()(img_in) 108 | x = Convolution2D(8, 3, 3)(x) 109 | x = Activation('relu')(x) 110 | x = MaxPooling2D(pool_size=(2, 2))(x) 111 | 112 | x = Convolution2D(16, 3, 3)(x) 113 | x = Activation('relu')(x) 114 | x = MaxPooling2D(pool_size=(2, 2))(x) 115 | 116 | x = Convolution2D(32, 3, 3)(x) 117 | x = Activation('relu')(x) 118 | x = MaxPooling2D(pool_size=(2, 2))(x) 119 | 120 | merged = Flatten()(x) 121 | 122 | x = Dense(256)(merged) 123 | x = Activation('linear')(x) 124 | x = Dropout(.2)(x) 125 | 126 | angle_out = Dense(1, name='angle_out')(x) 127 | 128 | model = Model(input=[img_in], output=[angle_out]) 129 | model.compile(optimizer='adam', loss='mean_squared_error') 130 | return model 131 | 132 | 133 | def vision_2D(dropout_frac=.2): 134 | ''' 135 | Network with 4 convolutions, 2 residual shortcuts to predict angle. 136 | ''' 137 | img_in = Input(shape=(120, 160, 3), name='img_in') 138 | 139 | net = Convolution2D(64, 6, 6, subsample=(4,4), name='conv0')(img_in) 140 | net = Dropout(dropout_frac)(net) 141 | 142 | net = Convolution2D(64, 3, 3, subsample=(2,2), name='conv1')(net) 143 | net = Dropout(dropout_frac)(net) 144 | 145 | #Create residual to shortcut 146 | aux1 = Flatten(name='aux1_flat')(net) 147 | aux1 = Dense(64, name='aux1_dense')(aux1) 148 | 149 | net = Convolution2D(128, 3, 3, subsample=(2,2), border_mode='same', name='conv2')(net) 150 | net = Dropout(dropout_frac)(net) 151 | 152 | net = Convolution2D(128, 3, 3, subsample=(2,2), border_mode='same', name='conv3')(net) 153 | net = Dropout(dropout_frac)(net) 154 | 155 | aux2 = Flatten(name='aux2_flat')(net) 156 | aux2 = Dense(64, name='aux2_dense')(aux2) 157 | 158 | net = Flatten(name='net_flat')(net) 159 | net = Dense(512, activation='relu', name='net_dense1')(net) 160 | net = Dropout(dropout_frac)(net) 161 | net = Dense(256, activation='relu', name='net_dense2')(net) 162 | net = Dropout(dropout_frac)(net) 163 | net = Dense(128, activation='relu', name='net_dense3')(net) 164 | net = Dropout(dropout_frac)(net) 165 | net = Dense(64, activation='linear', name='net_dense4')(net) 166 | 167 | net = merge([net, aux1, aux2], mode='sum') #combine residual layers 168 | angle_out = Dense(1, name='angle_out')(net) 169 | model = Model(input=[img_in], output=[angle_out]) 170 | model.compile(optimizer='adam', loss='mean_squared_error') 171 | return model -------------------------------------------------------------------------------- /donkey/sessions.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import numpy as np 4 | from PIL import Image 5 | from skimage import exposure 6 | 7 | import pickle 8 | 9 | import donkey as dk 10 | 11 | class Session(): 12 | ''' 13 | Class to store images and vehicle data to the local file system and later retrieve them 14 | as arrays or generators. 15 | ''' 16 | 17 | def __init__(self, path): 18 | print('Loading Session') 19 | self.session_dir = path 20 | 21 | self.frame_count = 0 22 | 23 | def put(self, img, angle=None, throttle=None, milliseconds=None): 24 | 25 | ''' 26 | Save image with encoded angle, throttle and time data in the filename 27 | ''' 28 | 29 | self.frame_count += 1 30 | 31 | file_name = str("%s/" % self.session_dir + 32 | "frame_" + str(self.frame_count).zfill(5) + 33 | "_ttl_" + str(throttle) + 34 | "_agl_" + str(angle) + 35 | "_mil_" + str(milliseconds) + 36 | '.jpg') 37 | 38 | img.save(file_name, 'jpeg') 39 | 40 | 41 | def get(self, file_path): 42 | ''' 43 | Retrieve an image and the data saved with it. 44 | ''' 45 | 46 | with Image.open(file_path) as img: 47 | img_arr = np.array(img) 48 | 49 | 50 | f = file_path.split('/')[-1] 51 | f = f.split('.')[0] 52 | f = f.split('_') 53 | 54 | throttle = int(f[3]) 55 | angle = int(f[5]) 56 | milliseconds = int(f[7]) 57 | 58 | data = {'throttle':throttle, 'angle':angle, 'milliseconds': milliseconds} 59 | 60 | return img_arr, data 61 | 62 | 63 | def img_paths(self): 64 | """ 65 | Returns a list of file paths for the images in the session. 66 | """ 67 | files = os.listdir(self.session_dir) 68 | files = [f for f in files if f[-3:] =='jpg'] 69 | files.sort() 70 | file_paths = [os.path.join(self.session_dir, f) for f in files] 71 | return file_paths 72 | 73 | 74 | def img_count(self): 75 | return len(self.img_paths()) 76 | 77 | 78 | def load_dataset(self, img_paths=None): 79 | ''' 80 | Returns image arrays and data arrays. 81 | 82 | X - array of n samples of immage arrays representing. 83 | Y - array with the shape (samples, 1) containing the 84 | angle lable for each image 85 | 86 | Where n is the number of recorded images. 87 | ''' 88 | 89 | gen = self.load_generator(img_paths=img_paths) 90 | 91 | X = [] #images 92 | Y = [] #velocity (angle, speed) 93 | for _ in range(self.img_count()): 94 | x, y = next(gen) 95 | X.append(x) 96 | Y.append(y) 97 | 98 | X = np.array(X) #image array [[image1],[image2]...] 99 | Y = np.array(Y) #array [[angle1, speed1],[angle2, speed2] ...] 100 | 101 | return X, Y 102 | 103 | 104 | def load_generator(self, img_paths=None, add_dim=False): 105 | ''' 106 | Rerturn a generator that will loops through image arrays and data labels. 107 | 108 | add_dim: add dimension to returned arrays as needed by keras 109 | ''' 110 | if img_paths == None: 111 | img_paths = self.img_paths() 112 | 113 | while True: 114 | for f in img_paths: 115 | 116 | img_arr, data = self.get(f) 117 | 118 | 119 | #return only angle for now 120 | data_arr = np.array(data['angle']) 121 | 122 | if add_dim == True: 123 | data_arr = data_arr.reshape((1,) + data_arr.shape) 124 | img_arr = img_arr.reshape((1,) + img_arr.shape) 125 | 126 | yield img_arr, data_arr 127 | 128 | 129 | 130 | 131 | def variant_generator(self, img_paths, variant_funcs): 132 | 133 | def orient(arr, flip=False): 134 | if flip == False: 135 | return arr 136 | else: 137 | return np.fliplr(arr) 138 | 139 | print('images before variants %s' % len(img_paths)) 140 | 141 | while True: 142 | for flip in [True, False]: 143 | for v in variant_funcs: 144 | for img_path in img_paths: 145 | img = Image.open(img_path) 146 | img = np.array(img) 147 | img = v['func'](img, **v['args']) 148 | img = orient(img, flip=flip) 149 | angle, speed = self.parse_file_name(img_path) 150 | if flip == True: 151 | angle = -angle #reverse stering angle 152 | y = np.array([angle, speed]) 153 | x = np.expand_dims(x, axis=0) 154 | y = y.reshape(1, 2) 155 | yield x, y 156 | 157 | 158 | 159 | 160 | 161 | 162 | class SessionHandler(): 163 | ''' 164 | Convienience class to create and load sessions. 165 | ''' 166 | 167 | 168 | def __init__(self, sessions_path): 169 | 170 | self.sessions_path = os.path.expanduser(sessions_path) 171 | 172 | 173 | def new(self, name=None): 174 | ''' 175 | Create a new session 176 | ''' 177 | 178 | path = self.make_session_dir(self.sessions_path, session_name=name) 179 | session = Session(path) 180 | return session 181 | 182 | 183 | def load(self, name): 184 | ''' 185 | Load a session given it's name. 186 | ''' 187 | path = os.path.join(self.sessions_path, name) 188 | session = Session(path) 189 | return session 190 | 191 | 192 | def last(self): 193 | ''' 194 | Return the last created session. 195 | ''' 196 | dirs = [ name for name in os.listdir(self.sessions_path) if os.path.isdir(os.path.join(self.sessions_path, name)) ] 197 | dirs.sort() 198 | path = os.join(self.sessions_path, dirs[-1]) 199 | session = Seession(path) 200 | return session 201 | 202 | 203 | def make_session_dir(self, base_path, session_name=None): 204 | ''' 205 | Make a new dir with a given name. If name doesn't exist 206 | use the current date/time. 207 | ''' 208 | 209 | base_path = os.path.expanduser(base_path) 210 | if session_name is None: 211 | session_dir_name = time.strftime('%Y_%m_%d__%I_%M_%S_%p') 212 | else: 213 | session_dir_name = session_name 214 | 215 | print('Creating a new session directory: %s' %session_dir_name) 216 | 217 | session_full_path = os.path.join(base_path, session_dir_name) 218 | if not os.path.exists(session_full_path): 219 | os.makedirs(session_full_path) 220 | return session_full_path 221 | 222 | 223 | def pickle_sessions(sessions_folder, session_names, file_path): 224 | 225 | ''' 226 | Combine, pickle and safe session data to a file. 227 | 228 | 'sessions_folder' where the session folders reside 229 | 'session_names' the names of the folders of the sessions to Combine 230 | 'file_path' name of the pickled file that will be saved 231 | ''' 232 | 233 | sh = dk.sessions.SessionHandler(sessions_folder) 234 | 235 | X = [] 236 | Y = [] 237 | 238 | for name in session_names: 239 | s = sh.load(name) 240 | x, y = s.load_dataset() 241 | X.append(x) 242 | Y.append(y) 243 | 244 | X = np.concatenate(X) 245 | Y = np.concatenate(Y) 246 | 247 | with open(file_path, 'wb') as f: 248 | pkl = pickle.dump((X,Y), f) 249 | -------------------------------------------------------------------------------- /donkey/remotes.py: -------------------------------------------------------------------------------- 1 | """ 2 | Classes used to communicate between vehicle and server. 3 | """ 4 | 5 | import time 6 | import json 7 | import io 8 | import os 9 | import copy 10 | 11 | import numpy as np 12 | 13 | import requests 14 | import tornado.ioloop 15 | import tornado.web 16 | 17 | from PIL import Image 18 | 19 | 20 | import donkey as dk 21 | 22 | 23 | class RemoteClient(): 24 | ''' 25 | Class used by vehicle to send driving data and recieve predictions. 26 | ''' 27 | 28 | def __init__(self, remote_url, vehicle_id='mycar'): 29 | 30 | self.record_url = remote_url + '/' + vehicle_id + '/drive/' 31 | 32 | 33 | def decide(self, img_arr, angle, throttle, milliseconds): 34 | ''' 35 | Accepts: image and control attributes and saves 36 | them to learn how to drive.''' 37 | 38 | #load features 39 | data = { 40 | 'angle': angle, 41 | 'throttle': throttle, 42 | 'milliseconds': milliseconds 43 | } 44 | 45 | 46 | r = requests.post(self.record_url, 47 | files={'img': dk.utils.arr_to_binary(img_arr), 48 | 'json': json.dumps(data)}) #hack to put json in file 49 | 50 | data = json.loads(r.text) 51 | angle = int(float(data['angle'])) 52 | throttle = int(float(data['throttle'])) 53 | print('remote client: %s' %r.text) 54 | 55 | return angle, throttle 56 | 57 | 58 | 59 | 60 | 61 | class RemoteServer(): 62 | ''' 63 | Class used to create server that accepts driving data, records it, 64 | runs a predictor and returns the predictions. 65 | ''' 66 | 67 | def __init__(self, session, pilot, port=8887): 68 | 69 | self.port = int(port) 70 | self.session = session 71 | self.pilot = pilot 72 | 73 | vehicle_data = {'user_angle': 0, 74 | 'user_throttle': 0, 75 | 'drive_mode':'user', 76 | 'milliseconds': 0, 77 | 'recording': False} 78 | 79 | 80 | self.vehicles = {'mycar':vehicle_data} 81 | 82 | this_dir = os.path.dirname(os.path.realpath(__file__)) 83 | self.static_file_path = os.path.join(this_dir, 'templates', 'static') 84 | print(self.static_file_path) 85 | 86 | 87 | def start(self): 88 | ''' 89 | Start the webserver. 90 | ''' 91 | 92 | #load features 93 | app = tornado.web.Application([ 94 | 95 | #temporary redirect until vehicles is not a singleton 96 | (r"/", tornado.web.RedirectHandler, 97 | dict(url="/mycar/")), 98 | 99 | (r"/?(?P[A-Za-z0-9-]+)?/", VehicleHandler), 100 | 101 | (r"/?(?P[A-Za-z0-9-]+)?/control/", 102 | ControllerHandler, 103 | dict(vehicles = self.vehicles) 104 | ), 105 | (r"/?(?P[A-Za-z0-9-]+)?/mjpeg/?(?P[^/]*)?", 106 | CameraMJPEGHandler, 107 | dict(vehicles = self.vehicles) 108 | ), 109 | (r"/?(?P[A-Za-z0-9-]+)?/drive/", 110 | DriveHandler, 111 | dict(pilot=self.pilot, session=self.session, vehicles=self.vehicles) 112 | ), 113 | 114 | (r"/static/(.*)", tornado.web.StaticFileHandler, {"path": self.static_file_path}), 115 | 116 | ], debug=True) 117 | 118 | app.listen(self.port) 119 | tornado.ioloop.IOLoop.instance().start() 120 | 121 | return True 122 | 123 | 124 | 125 | 126 | class VehicleHandler(tornado.web.RequestHandler): 127 | def get(self, vehicle_id): 128 | ''' 129 | Serves web page used to control the vehicle. 130 | ''' 131 | self.render("templates/monitor.html") 132 | 133 | 134 | 135 | class ControllerHandler(tornado.web.RequestHandler): 136 | 137 | def initialize(self, vehicles): 138 | self.vehicles = vehicles 139 | 140 | 141 | def post(self, vehicle_id): 142 | ''' 143 | Receive post requests as user changes the angle 144 | and throttle of the vehicle on a the index webpage 145 | ''' 146 | data = tornado.escape.json_decode(self.request.body) 147 | 148 | angle = data['angle'] 149 | throttle = data['throttle'] 150 | 151 | V = self.vehicles[vehicle_id] 152 | 153 | #Set recording mode 154 | if data['recording'] == 'true': 155 | V['recording'] = True 156 | else: 157 | V['recording'] = False 158 | 159 | 160 | #update vehicle angel based on drive mode 161 | V['drive_mode'] = data['drive_mode'] 162 | 163 | if angle is not "": 164 | V['user_angle'] = int(angle) 165 | else: 166 | V['user_angle'] = 0 167 | 168 | if throttle is not "": 169 | V['user_throttle'] = int(throttle) 170 | else: 171 | V['user_throttle'] = 0 172 | 173 | print(V) 174 | 175 | 176 | 177 | class DriveHandler(tornado.web.RequestHandler): 178 | def initialize(self, session, pilot, vehicles): 179 | #the parrent controller 180 | self.pilot = pilot 181 | self.session = session 182 | self.vehicles = vehicles 183 | 184 | def post(self, vehicle_id): 185 | ''' 186 | Receive post requests from vehicle with camera image. 187 | Return the angle and throttle the car should be goin. 188 | ''' 189 | img = self.request.files['img'][0]['body'] 190 | img = Image.open(io.BytesIO(img)) 191 | img_arr = dk.utils.img_to_arr(img) 192 | 193 | #Hack to take json from a file 194 | #data = json.loads(self.request.files['json'][0]['body'].decode("utf-8") ) 195 | 196 | #Get angle/throttle from pilot loaded by the server. 197 | pilot_angle, pilot_throttle = self.pilot.decide(img_arr) 198 | 199 | V = self.vehicles[vehicle_id] 200 | V['img'] = img 201 | V['pilot_angle'] = pilot_angle 202 | V['pilot_throttle'] = pilot_throttle 203 | 204 | 205 | if V['recording'] == True: 206 | #save image with encoded angle/throttle values 207 | self.session.put(img, 208 | angle=V['user_angle'], 209 | throttle=V['user_throttle'], 210 | milliseconds=V['milliseconds']) 211 | 212 | #depending on the drive mode, return user or pilot values 213 | if V['drive_mode'] == 'user': 214 | angle, throttle = V['user_angle'], V['user_throttle'] 215 | elif V['drive_mode'] == 'auto_angle': 216 | angle, throttle = V['pilot_angle'], V['user_throttle'] 217 | else: 218 | angle, throttle = V['pilot_angle'], V['pilot_throttle'] 219 | 220 | print('%s: A: %s T:%s' %(V['drive_mode'], angle, throttle)) 221 | 222 | #retun angel/throttle values to vehicle with json response 223 | self.write(json.dumps({'angle': str(angle), 'throttle': str(throttle)})) 224 | 225 | 226 | 227 | class CameraMJPEGHandler(tornado.web.RequestHandler): 228 | def initialize(self, vehicles): 229 | self.vehicles = vehicles 230 | 231 | 232 | @tornado.web.asynchronous 233 | @tornado.gen.coroutine 234 | def get(self, vehicle_id, file): 235 | ''' 236 | Show a video of the pictures captured by the vehicle. 237 | ''' 238 | ioloop = tornado.ioloop.IOLoop.current() 239 | self.set_header("Content-type", "multipart/x-mixed-replace;boundary=--boundarydonotcross") 240 | 241 | self.served_image_timestamp = time.time() 242 | my_boundary = "--boundarydonotcross" 243 | while True: 244 | 245 | interval = .2 246 | if self.served_image_timestamp + interval < time.time(): 247 | 248 | 249 | img = self.vehicles[vehicle_id]['img'] 250 | img = dk.utils.img_to_binary(img) 251 | 252 | self.write(my_boundary) 253 | self.write("Content-type: image/jpeg\r\n") 254 | self.write("Content-length: %s\r\n\r\n" % len(img)) 255 | self.write(img) 256 | self.served_image_timestamp = time.time() 257 | yield tornado.gen.Task(self.flush) 258 | else: 259 | yield tornado.gen.Task(ioloop.add_timeout, ioloop.time() + interval) 260 | 261 | -------------------------------------------------------------------------------- /donkey/templates/static/nipple.js: -------------------------------------------------------------------------------- 1 | (function(f){if(typeof exports==="object"&&typeof module!=="undefined"){module.exports=f()}else if(typeof define==="function"&&define.amd){define([],f)}else{var g;if(typeof window!=="undefined"){g=window}else if(typeof global!=="undefined"){g=global}else if(typeof self!=="undefined"){g=self}else{g=this}g.nipplejs = f()}})(function(){var define,module,exports; 2 | 'use strict'; 3 | 4 | // Constants 5 | var isTouch = !!('ontouchstart' in window); 6 | var isPointer = window.PointerEvent ? true : false; 7 | var isMSPointer = window.MSPointerEvent ? true : false; 8 | var events = { 9 | touch: { 10 | start: 'touchstart', 11 | move: 'touchmove', 12 | end: 'touchend' 13 | }, 14 | mouse: { 15 | start: 'mousedown', 16 | move: 'mousemove', 17 | end: 'mouseup' 18 | }, 19 | pointer: { 20 | start: 'pointerdown', 21 | move: 'pointermove', 22 | end: 'pointerup' 23 | }, 24 | MSPointer: { 25 | start: 'MSPointerDown', 26 | move: 'MSPointerMove', 27 | end: 'MSPointerUp' 28 | } 29 | }; 30 | var toBind; 31 | var secondBind = {}; 32 | if (isPointer) { 33 | toBind = events.pointer; 34 | } else if (isMSPointer) { 35 | toBind = events.MSPointer; 36 | } else if (isTouch) { 37 | toBind = events.touch; 38 | secondBind = events.mouse; 39 | } else { 40 | toBind = events.mouse; 41 | } 42 | /////////////////////// 43 | /// UTILS /// 44 | /////////////////////// 45 | 46 | var u = {}; 47 | u.distance = function (p1, p2) { 48 | var dx = p2.x - p1.x; 49 | var dy = p2.y - p1.y; 50 | 51 | return Math.sqrt((dx * dx) + (dy * dy)); 52 | }; 53 | 54 | u.angle = function(p1, p2) { 55 | var dx = p2.x - p1.x; 56 | var dy = p2.y - p1.y; 57 | 58 | return u.degrees(Math.atan2(dy, dx)); 59 | }; 60 | 61 | u.findCoord = function(p, d, a) { 62 | var b = {x: 0, y: 0}; 63 | a = u.radians(a); 64 | b.x = p.x - d * Math.cos(a); 65 | b.y = p.y - d * Math.sin(a); 66 | return b; 67 | }; 68 | 69 | u.radians = function(a) { 70 | return a * (Math.PI / 180); 71 | }; 72 | 73 | u.degrees = function(a) { 74 | return a * (180 / Math.PI); 75 | }; 76 | 77 | u.bindEvt = function (el, type, handler) { 78 | if (el.addEventListener) { 79 | el.addEventListener(type, handler, false); 80 | } else if (el.attachEvent) { 81 | el.attachEvent(type, handler); 82 | } 83 | }; 84 | 85 | u.unbindEvt = function (el, type, handler) { 86 | if (el.removeEventListener) { 87 | el.removeEventListener(type, handler); 88 | } else if (el.detachEvent) { 89 | el.detachEvent(type, handler); 90 | } 91 | }; 92 | 93 | u.trigger = function (el, type, data) { 94 | var evt = new CustomEvent(type, data); 95 | el.dispatchEvent(evt); 96 | }; 97 | 98 | u.prepareEvent = function (evt) { 99 | evt.preventDefault(); 100 | return evt.type.match(/^touch/) ? evt.changedTouches : evt; 101 | }; 102 | 103 | u.getScroll = function () { 104 | var x = (window.pageXOffset !== undefined) ? 105 | window.pageXOffset : 106 | (document.documentElement || document.body.parentNode || document.body) 107 | .scrollLeft; 108 | 109 | var y = (window.pageYOffset !== undefined) ? 110 | window.pageYOffset : 111 | (document.documentElement || document.body.parentNode || document.body) 112 | .scrollTop; 113 | return { 114 | x: x, 115 | y: y 116 | }; 117 | }; 118 | 119 | u.applyPosition = function (el, pos) { 120 | if (pos.x && pos.y) { 121 | el.style.left = pos.x + 'px'; 122 | el.style.top = pos.y + 'px'; 123 | } else if (pos.top || pos.right || pos.bottom || pos.left) { 124 | el.style.top = pos.top; 125 | el.style.right = pos.right; 126 | el.style.bottom = pos.bottom; 127 | el.style.left = pos.left; 128 | } 129 | }; 130 | 131 | u.getTransitionStyle = function (property, values, time) { 132 | var obj = u.configStylePropertyObject(property); 133 | for (var i in obj) { 134 | if (obj.hasOwnProperty(i)) { 135 | if (typeof values === 'string') { 136 | obj[i] = values + ' ' + time; 137 | } else { 138 | var st = ''; 139 | for (var j = 0, max = values.length; j < max; j += 1) { 140 | st += values[j] + ' ' + time + ', '; 141 | } 142 | obj[i] = st.slice(0, -2); 143 | } 144 | } 145 | } 146 | return obj; 147 | }; 148 | 149 | u.getVendorStyle = function (property, value) { 150 | var obj = u.configStylePropertyObject(property); 151 | for (var i in obj) { 152 | if (obj.hasOwnProperty(i)) { 153 | obj[i] = value; 154 | } 155 | } 156 | return obj; 157 | }; 158 | 159 | u.configStylePropertyObject = function (prop) { 160 | var obj = {}; 161 | obj[prop] = ''; 162 | var vendors = ['webkit', 'Moz', 'o']; 163 | vendors.forEach(function (vendor) { 164 | obj[vendor + prop.charAt(0).toUpperCase() + prop.slice(1)] = ''; 165 | }); 166 | return obj; 167 | }; 168 | 169 | u.extend = function (objA, objB) { 170 | for (var i in objB) { 171 | if (objB.hasOwnProperty(i)) { 172 | objA[i] = objB[i]; 173 | } 174 | } 175 | return objA; 176 | }; 177 | 178 | // Overwrite only what's already present 179 | u.safeExtend = function (objA, objB) { 180 | var obj = {}; 181 | for (var i in objA) { 182 | if (objA.hasOwnProperty(i) && objB.hasOwnProperty(i)) { 183 | obj[i] = objB[i]; 184 | } else if (objA.hasOwnProperty(i)) { 185 | obj[i] = objA[i]; 186 | } 187 | } 188 | return obj; 189 | }; 190 | 191 | // Map for array or unique item. 192 | u.map = function (ar, fn) { 193 | if (ar.length) { 194 | for (var i = 0, max = ar.length; i < max; i += 1) { 195 | fn(ar[i]); 196 | } 197 | } else { 198 | fn(ar); 199 | } 200 | }; 201 | /////////////////////// 202 | /// SUPER CLASS /// 203 | /////////////////////// 204 | 205 | function Super () {}; 206 | 207 | // Basic event system. 208 | Super.prototype.on = function (arg, cb) { 209 | var self = this; 210 | var types = arg.split(/[ ,]+/g); 211 | var type; 212 | self._handlers_ = self._handlers_ || {}; 213 | 214 | for (var i = 0; i < types.length; i += 1) { 215 | type = types[i]; 216 | self._handlers_[type] = self._handlers_[type] || []; 217 | self._handlers_[type].push(cb); 218 | } 219 | return self; 220 | }; 221 | 222 | Super.prototype.off = function (type, cb) { 223 | var self = this; 224 | self._handlers_ = self._handlers_ || {}; 225 | 226 | if (type === undefined) { 227 | self._handlers_ = {}; 228 | } else if (cb === undefined) { 229 | self._handlers_[type] = null; 230 | } else if (self._handlers_[type] && 231 | self._handlers_[type].indexOf(cb) >= 0) { 232 | self._handlers_[type].splice(self._handlers_[type].indexOf(cb), 1); 233 | } 234 | 235 | return self; 236 | }; 237 | 238 | Super.prototype.trigger = function (arg, data) { 239 | var self = this; 240 | var types = arg.split(/[ ,]+/g); 241 | var type; 242 | self._handlers_ = self._handlers_ || {}; 243 | 244 | for (var i = 0; i < types.length; i += 1) { 245 | type = types[i]; 246 | if (self._handlers_[type] && self._handlers_[type].length) { 247 | self._handlers_[type].forEach(function (handler) { 248 | handler.call(self, { 249 | type: type, 250 | target: self 251 | }, data); 252 | }); 253 | } 254 | } 255 | }; 256 | 257 | // Configuration 258 | Super.prototype.config = function (options) { 259 | var self = this; 260 | self.options = self.defaults || {}; 261 | if (options) { 262 | self.options = u.safeExtend(self.options, options); 263 | } 264 | }; 265 | 266 | // Bind internal events. 267 | Super.prototype.bindEvt = function (el, type) { 268 | var self = this; 269 | self._domHandlers_ = self._domHandlers_ || {}; 270 | 271 | self._domHandlers_[type] = function () { 272 | if (typeof self['on' + type] === 'function') { 273 | self['on' + type].apply(self, arguments); 274 | } else { 275 | console.warn('[WARNING] : Missing "on' + type + '" handler.'); 276 | } 277 | }; 278 | 279 | u.bindEvt(el, toBind[type], self._domHandlers_[type]); 280 | 281 | if (secondBind[type]) { 282 | // Support for both touch and mouse at the same time. 283 | u.bindEvt(el, secondBind[type], self._domHandlers_[type]); 284 | } 285 | 286 | return self; 287 | }; 288 | 289 | // Unbind dom events. 290 | Super.prototype.unbindEvt = function (el, type) { 291 | var self = this; 292 | self._domHandlers_ = self._domHandlers_ || {}; 293 | 294 | u.unbindEvt(el, toBind[type], self._domHandlers_[type]); 295 | 296 | if (secondBind[type]) { 297 | // Support for both touch and mouse at the same time. 298 | u.unbindEvt(el, secondBind[type], self._domHandlers_[type]); 299 | } 300 | 301 | delete self._domHandlers_[type]; 302 | 303 | return this; 304 | }; 305 | /////////////////////// 306 | /// THE NIPPLE /// 307 | /////////////////////// 308 | 309 | function Nipple (collection, options) { 310 | this.identifier = options.identifier; 311 | this.position = options.position; 312 | this.frontPosition = options.frontPosition; 313 | this.collection = collection; 314 | 315 | // Defaults 316 | this.defaults = { 317 | size: 100, 318 | threshold: 0.1, 319 | color: 'white', 320 | fadeTime: 250, 321 | dataOnly: false, 322 | restOpacity: 0.5, 323 | mode: 'dynamic', 324 | zone: document.body 325 | }; 326 | 327 | this.config(options); 328 | 329 | // Overwrites 330 | if (this.options.mode === 'dynamic') { 331 | this.options.restOpacity = 0; 332 | } 333 | 334 | this.id = Nipple.id; 335 | Nipple.id += 1; 336 | this.buildEl() 337 | .stylize(); 338 | 339 | // Nipple's API. 340 | this.instance = { 341 | el: this.ui.el, 342 | on: this.on.bind(this), 343 | off: this.off.bind(this), 344 | show: this.show.bind(this), 345 | hide: this.hide.bind(this), 346 | add: this.addToDom.bind(this), 347 | remove: this.removeFromDom.bind(this), 348 | destroy: this.destroy.bind(this), 349 | resetDirection: this.resetDirection.bind(this), 350 | computeDirection: this.computeDirection.bind(this), 351 | trigger: this.trigger.bind(this), 352 | position: this.position, 353 | frontPosition: this.frontPosition, 354 | ui: this.ui, 355 | identifier: this.identifier, 356 | id: this.id, 357 | options: this.options 358 | }; 359 | 360 | return this.instance; 361 | }; 362 | 363 | Nipple.prototype = new Super(); 364 | Nipple.constructor = Nipple; 365 | Nipple.id = 0; 366 | 367 | // Build the dom element of the Nipple instance. 368 | Nipple.prototype.buildEl = function (options) { 369 | this.ui = {}; 370 | 371 | if (this.options.dataOnly) { 372 | return this; 373 | } 374 | 375 | this.ui.el = document.createElement('div'); 376 | this.ui.back = document.createElement('div'); 377 | this.ui.front = document.createElement('div'); 378 | 379 | this.ui.el.className = 'nipple collection_' + this.collection.id; 380 | this.ui.back.className = 'back'; 381 | this.ui.front.className = 'front'; 382 | 383 | this.ui.el.setAttribute('id', 'nipple_' + this.collection.id + 384 | '_' + this.id); 385 | 386 | this.ui.el.appendChild(this.ui.back); 387 | this.ui.el.appendChild(this.ui.front); 388 | 389 | return this; 390 | }; 391 | 392 | // Apply CSS to the Nipple instance. 393 | Nipple.prototype.stylize = function () { 394 | if (this.options.dataOnly) { 395 | return this; 396 | } 397 | var animTime = this.options.fadeTime + 'ms'; 398 | var borderStyle = u.getVendorStyle('borderRadius', '50%'); 399 | var transitStyle = u.getTransitionStyle('transition', 'opacity', animTime); 400 | var styles = {}; 401 | styles.el = { 402 | position: 'absolute', 403 | opacity: this.options.restOpacity, 404 | display: 'block', 405 | 'zIndex': 999 406 | }; 407 | 408 | styles.back = { 409 | position: 'absolute', 410 | display: 'block', 411 | width: this.options.size + 'px', 412 | height: this.options.size + 'px', 413 | marginLeft: -this.options.size / 2 + 'px', 414 | marginTop: -this.options.size / 2 + 'px', 415 | background: this.options.color, 416 | 'opacity': '.5' 417 | }; 418 | 419 | styles.front = { 420 | width: this.options.size / 2 + 'px', 421 | height: this.options.size / 2 + 'px', 422 | position: 'absolute', 423 | display: 'block', 424 | marginLeft: -this.options.size / 4 + 'px', 425 | marginTop: -this.options.size / 4 + 'px', 426 | background: this.options.color, 427 | 'opacity': '.5' 428 | }; 429 | 430 | u.extend(styles.el, transitStyle); 431 | u.extend(styles.back, borderStyle); 432 | u.extend(styles.front, borderStyle); 433 | 434 | this.applyStyles(styles); 435 | 436 | return this; 437 | }; 438 | 439 | Nipple.prototype.applyStyles = function (styles) { 440 | // Apply styles 441 | for (var i in this.ui) { 442 | if (this.ui.hasOwnProperty(i)) { 443 | for (var j in styles[i]) { 444 | this.ui[i].style[j] = styles[i][j]; 445 | } 446 | } 447 | } 448 | 449 | return this; 450 | }; 451 | 452 | // Inject the Nipple instance into DOM. 453 | Nipple.prototype.addToDom = function () { 454 | // We're not adding it if we're dataOnly or already in dom. 455 | if (this.options.dataOnly || document.body.contains(this.ui.el)) { 456 | return this; 457 | } 458 | this.options.zone.appendChild(this.ui.el); 459 | return this; 460 | }; 461 | 462 | // Remove the Nipple instance from DOM. 463 | Nipple.prototype.removeFromDom = function () { 464 | if (this.options.dataOnly || !document.body.contains(this.ui.el)) { 465 | return this; 466 | } 467 | this.options.zone.removeChild(this.ui.el); 468 | return this; 469 | }; 470 | 471 | // Entirely destroy this nipple 472 | Nipple.prototype.destroy = function () { 473 | clearTimeout(this.removeTimeout); 474 | clearTimeout(this.showTimeout); 475 | clearTimeout(this.restTimeout); 476 | this.trigger('destroyed', this.instance); 477 | this.removeFromDom(); 478 | this.off(); 479 | }; 480 | 481 | // Fade in the Nipple instance. 482 | Nipple.prototype.show = function (cb) { 483 | var self = this; 484 | 485 | if (self.options.dataOnly) { 486 | return self; 487 | } 488 | 489 | clearTimeout(self.removeTimeout); 490 | clearTimeout(self.showTimeout); 491 | clearTimeout(self.restTimeout); 492 | 493 | self.addToDom(); 494 | 495 | self.restCallback(); 496 | 497 | setTimeout(function () { 498 | self.ui.el.style.opacity = 1; 499 | }, 0); 500 | 501 | self.showTimeout = setTimeout(function () { 502 | self.trigger('shown', self.instance); 503 | if (typeof cb === 'function') { 504 | cb.call(this); 505 | } 506 | }, self.options.fadeTime); 507 | 508 | return self; 509 | }; 510 | 511 | // Fade out the Nipple instance. 512 | Nipple.prototype.hide = function (cb) { 513 | var self = this; 514 | 515 | if (self.options.dataOnly) { 516 | return self; 517 | } 518 | 519 | self.ui.el.style.opacity = self.options.restOpacity; 520 | 521 | clearTimeout(self.removeTimeout); 522 | clearTimeout(self.showTimeout); 523 | clearTimeout(self.restTimeout); 524 | 525 | self.removeTimeout = setTimeout( 526 | function () { 527 | var display = self.options.mode === 'dynamic' ? 'none' : 'block'; 528 | self.ui.el.style.display = display; 529 | if (typeof cb === 'function') { 530 | cb.call(self); 531 | } 532 | 533 | self.trigger('hidden', self.instance); 534 | }, 535 | self.options.fadeTime 536 | ); 537 | self.restPosition(); 538 | 539 | return self; 540 | }; 541 | 542 | Nipple.prototype.restPosition = function (cb) { 543 | var self = this; 544 | self.frontPosition = { 545 | x: 0, 546 | y: 0 547 | }; 548 | var animTime = self.options.fadeTime + 'ms'; 549 | 550 | var transitStyle = {}; 551 | transitStyle.front = u.getTransitionStyle('transition', 552 | ['top', 'left'], animTime); 553 | 554 | var styles = {front: {}}; 555 | styles.front = { 556 | left: self.frontPosition.x + 'px', 557 | top: self.frontPosition.y + 'px' 558 | }; 559 | 560 | self.applyStyles(transitStyle); 561 | self.applyStyles(styles); 562 | 563 | self.restTimeout = setTimeout( 564 | function () { 565 | if (typeof cb === 'function') { 566 | cb.call(self); 567 | } 568 | self.restCallback(); 569 | }, 570 | self.options.fadeTime 571 | ); 572 | }; 573 | 574 | Nipple.prototype.restCallback = function () { 575 | var self = this; 576 | var transitStyle = {}; 577 | transitStyle.front = u.getTransitionStyle('transition', 'none', ''); 578 | self.applyStyles(transitStyle); 579 | self.trigger('rested', self.instance); 580 | }; 581 | 582 | Nipple.prototype.resetDirection = function () { 583 | // Fully rebuild the object to let the iteration possible. 584 | this.direction = { 585 | x: false, 586 | y: false, 587 | angle: false 588 | }; 589 | }; 590 | 591 | Nipple.prototype.computeDirection = function (obj) { 592 | var rAngle = obj.angle.radian; 593 | var angle45 = Math.PI / 4; 594 | var angle90 = Math.PI / 2; 595 | var direction, directionX, directionY; 596 | 597 | // Angular direction 598 | // \ UP / 599 | // \ / 600 | // LEFT RIGHT 601 | // / \ 602 | // /DOWN \ 603 | // 604 | if (rAngle > angle45 && rAngle < (angle45 * 3)) { 605 | direction = 'up'; 606 | } else if (rAngle > -angle45 && rAngle <= angle45) { 607 | direction = 'left'; 608 | } else if (rAngle > (-angle45 * 3) && rAngle <= -angle45) { 609 | direction = 'down'; 610 | } else { 611 | direction = 'right'; 612 | } 613 | 614 | // Plain direction 615 | // UP | 616 | // _______ | RIGHT 617 | // LEFT | 618 | // DOWN | 619 | if (rAngle > -angle90 && rAngle < angle90) { 620 | directionX = 'left'; 621 | } else { 622 | directionX = 'right'; 623 | } 624 | 625 | if (rAngle > 0) { 626 | directionY = 'up'; 627 | } else { 628 | directionY = 'down'; 629 | } 630 | 631 | if (obj.force > this.options.threshold) { 632 | var oldDirection = {}; 633 | for (var i in this.direction) { 634 | if (this.direction.hasOwnProperty(i)) { 635 | oldDirection[i] = this.direction[i]; 636 | } 637 | } 638 | 639 | var same = {}; 640 | 641 | this.direction = { 642 | x: directionX, 643 | y: directionY, 644 | angle: direction 645 | }; 646 | 647 | obj.direction = this.direction; 648 | 649 | for (var i in oldDirection) { 650 | if (oldDirection[i] === this.direction[i]) { 651 | same[i] = true; 652 | } 653 | } 654 | 655 | // If all 3 directions are the same, we don't trigger anything. 656 | if (same.x && same.y && same.angle) { 657 | return obj; 658 | } 659 | 660 | if (!same.x || !same.y) { 661 | this.trigger('plain', obj); 662 | } 663 | 664 | if (!same.x) { 665 | this.trigger('plain:' + directionX, obj); 666 | } 667 | 668 | if (!same.y) { 669 | this.trigger('plain:' + directionY, obj); 670 | } 671 | 672 | if (!same.angle) { 673 | this.trigger('dir dir:' + direction, obj); 674 | } 675 | } 676 | return obj; 677 | }; 678 | /* global Nipple, Super */ 679 | 680 | /////////////////////////// 681 | /// THE COLLECTION /// 682 | /////////////////////////// 683 | 684 | function Collection (manager, options) { 685 | var self = this; 686 | self.nipples = []; 687 | self.idles = []; 688 | self.actives = []; 689 | self.ids = []; 690 | self.pressureIntervals = {}; 691 | self.manager = manager; 692 | self.id = Collection.id; 693 | Collection.id += 1; 694 | 695 | // Defaults 696 | self.defaults = { 697 | zone: document.body, 698 | multitouch: false, 699 | maxNumberOfNipples: 10, 700 | mode: 'dynamic', 701 | position: {top: 0, left: 0}, 702 | catchDistance: 200, 703 | size: 100, 704 | threshold: 0.1, 705 | color: 'white', 706 | fadeTime: 250, 707 | dataOnly: false, 708 | restOpacity: 0.5 709 | }; 710 | 711 | self.config(options); 712 | 713 | // Overwrites 714 | if (self.options.mode === 'static' || self.options.mode === 'semi') { 715 | self.options.multitouch = false; 716 | } 717 | 718 | if (!self.options.multitouch) { 719 | self.options.maxNumberOfNipples = 1; 720 | } 721 | 722 | self.updateBox(); 723 | self.prepareNipples(); 724 | self.bindings(); 725 | self.begin(); 726 | 727 | return self.nipples; 728 | } 729 | 730 | Collection.prototype = new Super(); 731 | Collection.constructor = Collection; 732 | Collection.id = 0; 733 | 734 | Collection.prototype.prepareNipples = function () { 735 | var self = this; 736 | var nips = self.nipples; 737 | 738 | // Public API Preparation. 739 | nips.on = self.on.bind(self); 740 | nips.off = self.off.bind(self); 741 | nips.options = self.options; 742 | nips.destroy = self.destroy.bind(self); 743 | nips.ids = self.ids; 744 | nips.id = self.id; 745 | nips.processOnMove = self.processOnMove.bind(self); 746 | nips.processOnEnd = self.processOnEnd.bind(self); 747 | nips.get = function (id) { 748 | if (id === undefined) { 749 | return nips[0]; 750 | } 751 | for (var i = 0, max = nips.length; i < max; i += 1) { 752 | if (nips[i].identifier === id) { 753 | return nips[i]; 754 | } 755 | } 756 | return false; 757 | }; 758 | }; 759 | 760 | Collection.prototype.bindings = function () { 761 | var self = this; 762 | // Touch start event. 763 | self.bindEvt(self.options.zone, 'start'); 764 | // Avoid native touch actions (scroll, zoom etc...) on the zone. 765 | self.options.zone.style.touchAction = 'none'; 766 | self.options.zone.style.msTouchAction = 'none'; 767 | }; 768 | 769 | Collection.prototype.begin = function () { 770 | var self = this; 771 | var opts = self.options; 772 | 773 | // We place our static nipple 774 | // if needed. 775 | if (opts.mode === 'static') { 776 | var nipple = self.createNipple( 777 | opts.position, 778 | self.manager.getIdentifier() 779 | ); 780 | // Add it to the dom. 781 | nipple.add(); 782 | // Store it in idles. 783 | self.idles.push(nipple); 784 | } 785 | }; 786 | 787 | // Nipple Factory 788 | Collection.prototype.createNipple = function (position, identifier) { 789 | var self = this; 790 | var scroll = u.getScroll(); 791 | var toPutOn = {}; 792 | var opts = self.options; 793 | 794 | if (position.x && position.y) { 795 | toPutOn = { 796 | x: position.x - 797 | (scroll.x + self.box.left), 798 | y: position.y - 799 | (scroll.y + self.box.top) 800 | }; 801 | } else if ( 802 | position.top || 803 | position.right || 804 | position.bottom || 805 | position.left 806 | ) { 807 | 808 | // We need to compute the position X / Y of the joystick. 809 | var dumb = document.createElement('DIV'); 810 | dumb.style.display = 'hidden'; 811 | dumb.style.top = position.top; 812 | dumb.style.right = position.right; 813 | dumb.style.bottom = position.bottom; 814 | dumb.style.left = position.left; 815 | dumb.style.position = 'absolute'; 816 | 817 | opts.zone.appendChild(dumb); 818 | var dumbBox = dumb.getBoundingClientRect(); 819 | opts.zone.removeChild(dumb); 820 | 821 | toPutOn = position; 822 | position = { 823 | x: dumbBox.left + scroll.x, 824 | y: dumbBox.top + scroll.y 825 | }; 826 | } 827 | 828 | var nipple = new Nipple(self, { 829 | color: opts.color, 830 | size: opts.size, 831 | threshold: opts.threshold, 832 | fadeTime: opts.fadeTime, 833 | dataOnly: opts.dataOnly, 834 | restOpacity: opts.restOpacity, 835 | mode: opts.mode, 836 | identifier: identifier, 837 | position: position, 838 | zone: opts.zone, 839 | frontPosition: { 840 | x: 0, 841 | y: 0 842 | } 843 | }); 844 | 845 | if (!opts.dataOnly) { 846 | u.applyPosition(nipple.ui.el, toPutOn); 847 | u.applyPosition(nipple.ui.front, nipple.frontPosition); 848 | } 849 | self.nipples.push(nipple); 850 | self.trigger('added ' + nipple.identifier + ':added', nipple); 851 | self.manager.trigger('added ' + nipple.identifier + ':added', nipple); 852 | 853 | self.bindNipple(nipple); 854 | 855 | return nipple; 856 | }; 857 | 858 | Collection.prototype.updateBox = function () { 859 | var self = this; 860 | self.box = self.options.zone.getBoundingClientRect(); 861 | }; 862 | 863 | Collection.prototype.bindNipple = function (nipple) { 864 | var self = this; 865 | var type; 866 | // Bubble up identified events. 867 | var handler = function (evt, data) { 868 | // Identify the event type with the nipple's id. 869 | type = evt.type + ' ' + data.id + ':' + evt.type; 870 | self.trigger(type, data); 871 | }; 872 | 873 | // When it gets destroyed. 874 | nipple.on('destroyed', self.onDestroyed.bind(self)); 875 | 876 | // Other events that will get bubbled up. 877 | nipple.on('shown hidden rested dir plain', handler); 878 | nipple.on('dir:up dir:right dir:down dir:left', handler); 879 | nipple.on('plain:up plain:right plain:down plain:left', handler); 880 | }; 881 | 882 | Collection.prototype.pressureFn = function (touch, nipple, identifier) { 883 | var self = this; 884 | var previousPressure = 0; 885 | clearInterval(self.pressureIntervals[identifier]); 886 | // Create an interval that will read the pressure every 100ms 887 | self.pressureIntervals[identifier] = setInterval(function () { 888 | var pressure = touch.force || touch.pressure || 889 | touch.webkitForce || 0; 890 | if (pressure !== previousPressure) { 891 | nipple.trigger('pressure', pressure); 892 | self.trigger('pressure ' + 893 | nipple.identifier + ':pressure', pressure); 894 | previousPressure = pressure; 895 | } 896 | }.bind(self), 100); 897 | }; 898 | 899 | Collection.prototype.onstart = function (evt) { 900 | var self = this; 901 | var opts = self.options; 902 | evt = u.prepareEvent(evt); 903 | 904 | // Update the box position 905 | self.updateBox(); 906 | 907 | var process = function (touch) { 908 | // If we can create new nipples 909 | // meaning we don't have more active nipples than we should. 910 | if (self.actives.length < opts.maxNumberOfNipples) { 911 | self.processOnStart(touch); 912 | } 913 | }; 914 | 915 | u.map(evt, process); 916 | 917 | // We ask upstream to bind the document 918 | // on 'move' and 'end' 919 | self.manager.bindDocument(); 920 | return false; 921 | }; 922 | 923 | Collection.prototype.processOnStart = function (evt) { 924 | var self = this; 925 | var opts = self.options; 926 | var indexInIdles; 927 | var identifier = self.manager.getIdentifier(evt); 928 | var pressure = evt.force || evt.pressure || evt.webkitForce || 0; 929 | var position = { 930 | x: evt.pageX, 931 | y: evt.pageY 932 | }; 933 | 934 | var nipple = self.getOrCreate(identifier, position); 935 | 936 | // Update its touch identifier 937 | nipple.identifier = identifier; 938 | 939 | var process = function (nip) { 940 | // Trigger the start. 941 | nip.trigger('start', nip); 942 | self.trigger('start ' + nip.id + ':start', nip); 943 | 944 | nip.show(); 945 | if (pressure > 0) { 946 | self.pressureFn(evt, nip, nip.identifier); 947 | } 948 | // Trigger the first move event. 949 | self.processOnMove(evt); 950 | }; 951 | 952 | // Transfer it from idles to actives. 953 | if ((indexInIdles = self.idles.indexOf(nipple)) >= 0) { 954 | self.idles.splice(indexInIdles, 1); 955 | } 956 | 957 | // Store the nipple in the actives array 958 | self.actives.push(nipple); 959 | self.ids.push(nipple.identifier); 960 | 961 | if (opts.mode !== 'semi') { 962 | process(nipple); 963 | } else { 964 | // In semi we check the distance of the touch 965 | // to decide if we have to reset the nipple 966 | var distance = u.distance(position, nipple.position); 967 | if (distance <= opts.catchDistance) { 968 | process(nipple); 969 | } else { 970 | nipple.destroy(); 971 | self.processOnStart(evt); 972 | return; 973 | } 974 | } 975 | 976 | return nipple; 977 | }; 978 | 979 | Collection.prototype.getOrCreate = function (identifier, position) { 980 | var self = this; 981 | var opts = self.options; 982 | var nipple; 983 | 984 | // If we're in static or semi, we might already have an active. 985 | if (/(semi|static)/.test(opts.mode)) { 986 | // Get the active one. 987 | // TODO: Multi-touche for semi and static will start here. 988 | // Return the nearest one. 989 | nipple = self.idles[0]; 990 | if (nipple) { 991 | self.idles.splice(0, 1); 992 | return nipple; 993 | } 994 | 995 | if (opts.mode === 'semi') { 996 | // If we're in semi mode, we need to create one. 997 | return self.createNipple(position, identifier); 998 | } 999 | 1000 | console.warn('Coudln\'t find the needed nipple.'); 1001 | return false; 1002 | } 1003 | // In dynamic, we create a new one. 1004 | nipple = self.createNipple(position, identifier); 1005 | return nipple; 1006 | }; 1007 | 1008 | Collection.prototype.processOnMove = function (evt) { 1009 | var self = this; 1010 | var opts = self.options; 1011 | var identifier = self.manager.getIdentifier(evt); 1012 | var nipple = self.nipples.get(identifier); 1013 | 1014 | if (!nipple) { 1015 | // This is here just for safety. 1016 | // It shouldn't happen. 1017 | console.error('Found zombie joystick with ID ' + identifier); 1018 | self.manager.removeIdentifier(identifier); 1019 | return; 1020 | } 1021 | 1022 | nipple.identifier = identifier; 1023 | 1024 | var size = nipple.options.size / 2; 1025 | var pos = { 1026 | x: evt.pageX, 1027 | y: evt.pageY 1028 | }; 1029 | 1030 | var dist = u.distance(pos, nipple.position); 1031 | var angle = u.angle(pos, nipple.position); 1032 | var rAngle = u.radians(angle); 1033 | var force = dist / size; 1034 | 1035 | // If distance is bigger than nipple's size 1036 | // we clamp the position. 1037 | if (dist > size) { 1038 | dist = size; 1039 | pos = u.findCoord(nipple.position, dist, angle); 1040 | } 1041 | 1042 | nipple.frontPosition = { 1043 | x: pos.x - nipple.position.x, 1044 | y: pos.y - nipple.position.y 1045 | }; 1046 | 1047 | if (!opts.dataOnly) { 1048 | u.applyPosition(nipple.ui.front, nipple.frontPosition); 1049 | } 1050 | 1051 | // Prepare event's datas. 1052 | var toSend = { 1053 | identifier: nipple.identifier, 1054 | position: pos, 1055 | force: force, 1056 | pressure: evt.force || evt.pressure || evt.webkitForce || 0, 1057 | distance: dist, 1058 | angle: { 1059 | radian: rAngle, 1060 | degree: angle 1061 | }, 1062 | instance: nipple 1063 | }; 1064 | 1065 | // Compute the direction's datas. 1066 | toSend = nipple.computeDirection(toSend); 1067 | 1068 | // Offset angles to follow units circle. 1069 | toSend.angle = { 1070 | radian: u.radians(180 - angle), 1071 | degree: 180 - angle 1072 | }; 1073 | 1074 | // Send everything to everyone. 1075 | nipple.trigger('move', toSend); 1076 | self.trigger('move ' + nipple.id + ':move', toSend); 1077 | }; 1078 | 1079 | Collection.prototype.processOnEnd = function (evt) { 1080 | var self = this; 1081 | var opts = self.options; 1082 | var identifier = self.manager.getIdentifier(evt); 1083 | var nipple = self.nipples.get(identifier); 1084 | var removedIdentifier = self.manager.removeIdentifier(nipple.identifier); 1085 | 1086 | if (!nipple) { 1087 | return; 1088 | } 1089 | 1090 | if (!opts.dataOnly) { 1091 | nipple.hide(function () { 1092 | if (opts.mode === 'dynamic') { 1093 | nipple.trigger('removed', nipple); 1094 | self.trigger('removed ' + nipple.id + ':removed', nipple); 1095 | self.manager 1096 | .trigger('removed ' + nipple.id + ':removed', nipple); 1097 | nipple.destroy(); 1098 | } 1099 | }); 1100 | } 1101 | 1102 | // Clear the pressure interval reader 1103 | clearInterval(self.pressureIntervals[nipple.identifier]); 1104 | 1105 | // Reset the direciton of the nipple, to be able to trigger a new direction 1106 | // on start. 1107 | nipple.resetDirection(); 1108 | 1109 | nipple.trigger('end', nipple); 1110 | self.trigger('end ' + nipple.id + ':end', nipple); 1111 | 1112 | // Remove identifier from our bank. 1113 | if (self.ids.indexOf(nipple.identifier) >= 0) { 1114 | self.ids.splice(self.ids.indexOf(nipple.identifier), 1); 1115 | } 1116 | 1117 | // Clean our actives array. 1118 | if (self.actives.indexOf(nipple) >= 0) { 1119 | self.actives.splice(self.actives.indexOf(nipple), 1); 1120 | } 1121 | 1122 | if (/(semi|static)/.test(opts.mode)) { 1123 | // Transfer nipple from actives to idles 1124 | // if we're in semi or static mode. 1125 | self.idles.push(nipple); 1126 | } else if (self.nipples.indexOf(nipple) >= 0) { 1127 | // Only if we're not in semi or static mode 1128 | // we can remove the instance. 1129 | self.nipples.splice(self.nipples.indexOf(nipple), 1); 1130 | } 1131 | 1132 | // We unbind move and end. 1133 | self.manager.unbindDocument(); 1134 | 1135 | // We add back the identifier of the idle nipple; 1136 | if (/(semi|static)/.test(opts.mode)) { 1137 | self.manager.ids[removedIdentifier.id] = removedIdentifier.identifier; 1138 | } 1139 | }; 1140 | 1141 | // Remove destroyed nipple from the lists 1142 | Collection.prototype.onDestroyed = function(evt, nipple) { 1143 | var self = this; 1144 | if (self.nipples.indexOf(nipple) >= 0) { 1145 | self.nipples.splice(self.nipples.indexOf(nipple), 1); 1146 | } 1147 | if (self.actives.indexOf(nipple) >= 0) { 1148 | self.actives.splice(self.actives.indexOf(nipple), 1); 1149 | } 1150 | if (self.idles.indexOf(nipple) >= 0) { 1151 | self.idles.splice(self.idles.indexOf(nipple), 1); 1152 | } 1153 | if (self.ids.indexOf(nipple.identifier) >= 0) { 1154 | self.ids.splice(self.ids.indexOf(nipple.identifier), 1); 1155 | } 1156 | 1157 | // Remove the identifier from our bank 1158 | self.manager.removeIdentifier(nipple.identifier); 1159 | 1160 | // We unbind move and end. 1161 | self.manager.unbindDocument(); 1162 | }; 1163 | 1164 | // Cleanly destroy the manager 1165 | Collection.prototype.destroy = function () { 1166 | var self = this; 1167 | self.unbindEvt(self.options.zone, 'start'); 1168 | 1169 | // Destroy nipples. 1170 | self.nipples.forEach(function(nipple) { 1171 | nipple.destroy(); 1172 | }); 1173 | 1174 | // Clean 3DTouch intervals. 1175 | for (var i in self.pressureIntervals) { 1176 | if (self.pressureIntervals.hasOwnProperty(i)) { 1177 | clearInterval(self.pressureIntervals[i]); 1178 | } 1179 | } 1180 | 1181 | // Notify the manager passing the instance 1182 | self.trigger('destroyed', self.nipples); 1183 | // We unbind move and end. 1184 | self.manager.unbindDocument(); 1185 | // Unbind everything. 1186 | self.off(); 1187 | }; 1188 | /* global u, Super, Collection */ 1189 | 1190 | /////////////////////// 1191 | /// MANAGER /// 1192 | /////////////////////// 1193 | 1194 | function Manager (options) { 1195 | var self = this; 1196 | self.ids = {}; 1197 | self.index = 0; 1198 | self.collections = []; 1199 | 1200 | self.config(options); 1201 | self.prepareCollections(); 1202 | 1203 | // Listen for resize, to reposition every joysticks 1204 | var resizeTimer; 1205 | u.bindEvt(window, 'resize', function (evt) { 1206 | clearTimeout(resizeTimer); 1207 | resizeTimer = setTimeout(function () { 1208 | var pos; 1209 | var scroll = u.getScroll(); 1210 | self.collections.forEach(function (collection) { 1211 | collection.forEach(function (nipple) { 1212 | pos = nipple.el.getBoundingClientRect(); 1213 | nipple.position = { 1214 | x: scroll.x + pos.left, 1215 | y: scroll.y + pos.top 1216 | }; 1217 | }); 1218 | }); 1219 | }, 100); 1220 | }); 1221 | 1222 | return self.collections; 1223 | }; 1224 | 1225 | Manager.prototype = new Super(); 1226 | Manager.constructor = Manager; 1227 | 1228 | Manager.prototype.prepareCollections = function () { 1229 | var self = this; 1230 | // Public API Preparation. 1231 | self.collections.create = self.create.bind(self); 1232 | // Listen to anything 1233 | self.collections.on = self.on.bind(self); 1234 | // Unbind general events 1235 | self.collections.off = self.off.bind(self); 1236 | // Destroy everything 1237 | self.collections.destroy = self.destroy.bind(self); 1238 | // Get any nipple 1239 | self.collections.get = function (id) { 1240 | var nipple; 1241 | self.collections.every(function (collection) { 1242 | if (nipple = collection.get(id)) { 1243 | return false; 1244 | } 1245 | return true; 1246 | }); 1247 | return nipple; 1248 | }; 1249 | }; 1250 | 1251 | Manager.prototype.create = function (options) { 1252 | return this.createCollection(options); 1253 | }; 1254 | 1255 | // Collection Factory 1256 | Manager.prototype.createCollection = function (options) { 1257 | var self = this; 1258 | var collection = new Collection(self, options); 1259 | 1260 | self.bindCollection(collection); 1261 | self.collections.push(collection); 1262 | 1263 | return collection; 1264 | }; 1265 | 1266 | Manager.prototype.bindCollection = function (collection) { 1267 | var self = this; 1268 | var type; 1269 | // Bubble up identified events. 1270 | var handler = function (evt, data) { 1271 | // Identify the event type with the nipple's identifier. 1272 | type = evt.type + ' ' + data.id + ':' + evt.type; 1273 | self.trigger(type, data); 1274 | }; 1275 | 1276 | // When it gets destroyed we clean. 1277 | collection.on('destroyed', self.onDestroyed.bind(self)); 1278 | 1279 | // Other events that will get bubbled up. 1280 | collection.on('shown hidden rested dir plain', handler); 1281 | collection.on('dir:up dir:right dir:down dir:left', handler); 1282 | collection.on('plain:up plain:right plain:down plain:left', handler); 1283 | }; 1284 | 1285 | Manager.prototype.bindDocument = function () { 1286 | var self = this; 1287 | // Bind only if not already binded 1288 | if (!self.binded) { 1289 | self.bindEvt(document, 'move') 1290 | .bindEvt(document, 'end'); 1291 | self.binded = true; 1292 | } 1293 | }; 1294 | 1295 | Manager.prototype.unbindDocument = function (force) { 1296 | var self = this; 1297 | // If there are no touch left 1298 | // unbind the document. 1299 | if (!Object.keys(self.ids).length || force === true) { 1300 | self.unbindEvt(document, 'move') 1301 | .unbindEvt(document, 'end'); 1302 | self.binded = false; 1303 | } 1304 | }; 1305 | 1306 | Manager.prototype.getIdentifier = function (evt) { 1307 | var id; 1308 | // If no event, simple increment 1309 | if (!evt) { 1310 | id = this.index; 1311 | } else { 1312 | // Extract identifier from event object. 1313 | // Unavailable in mouse events so replaced by latest increment. 1314 | id = evt.identifier === undefined ? evt.pointerId : evt.identifier; 1315 | if (id === undefined) { 1316 | id = this.latest || 0; 1317 | } 1318 | } 1319 | 1320 | if (this.ids[id] === undefined) { 1321 | this.ids[id] = this.index; 1322 | this.index += 1; 1323 | } 1324 | 1325 | // Keep the latest id used in case we're using an unidentified mouseEvent 1326 | this.latest = id; 1327 | return this.ids[id]; 1328 | }; 1329 | 1330 | Manager.prototype.removeIdentifier = function (identifier) { 1331 | var removed = {}; 1332 | for (var id in this.ids) { 1333 | if (this.ids[id] === identifier) { 1334 | removed.id = id; 1335 | removed.identifier = this.ids[id]; 1336 | delete this.ids[id]; 1337 | break; 1338 | } 1339 | } 1340 | return removed; 1341 | }; 1342 | 1343 | Manager.prototype.onmove = function (evt) { 1344 | var self = this; 1345 | self.onAny('move', evt); 1346 | return false; 1347 | }; 1348 | 1349 | Manager.prototype.onend = function (evt) { 1350 | var self = this; 1351 | self.onAny('end', evt); 1352 | return false; 1353 | }; 1354 | 1355 | Manager.prototype.onAny = function (which, evt) { 1356 | var self = this; 1357 | var id; 1358 | var processFn = 'processOn' + which.charAt(0).toUpperCase() + 1359 | which.slice(1); 1360 | evt = u.prepareEvent(evt); 1361 | var processColl = function (e, id, coll) { 1362 | if (coll.ids.indexOf(id) >= 0) { 1363 | coll[processFn](e); 1364 | // Mark the event to avoid cleaning it later. 1365 | e._found_ = true; 1366 | } 1367 | }; 1368 | var processEvt = function (e) { 1369 | id = self.getIdentifier(e); 1370 | u.map(self.collections, processColl.bind(null, e, id)); 1371 | // If the event isn't handled by any collection, 1372 | // we need to clean its identifier. 1373 | if (!e._found_) { 1374 | self.removeIdentifier(id); 1375 | } 1376 | }; 1377 | 1378 | u.map(evt, processEvt); 1379 | 1380 | return false; 1381 | }; 1382 | 1383 | // Cleanly destroy the manager 1384 | Manager.prototype.destroy = function () { 1385 | var self = this; 1386 | self.unbindDocument(true); 1387 | self.ids = {}; 1388 | self.index = 0; 1389 | self.collections.forEach(function(collection) { 1390 | collection.destroy(); 1391 | }); 1392 | self.off(); 1393 | }; 1394 | 1395 | // When a collection gets destroyed 1396 | // we clean behind. 1397 | Manager.prototype.onDestroyed = function (evt, coll) { 1398 | var self = this; 1399 | if (self.collections.indexOf(coll) < 0) { 1400 | return false; 1401 | } 1402 | self.collections.splice(self.collections.indexOf(coll), 1); 1403 | }; 1404 | var factory = new Manager(); 1405 | return { 1406 | create: function (options) { 1407 | return factory.create(options); 1408 | }, 1409 | factory: factory 1410 | }; 1411 | 1412 | }); --------------------------------------------------------------------------------