def train(cfg, tub_names, model_name):
'''
use the specified data in tub_names to train an artifical neural network
saves the output trained model as model_name
'''
X_keys = ['cam/image_array']
y_keys = ['user/angle', 'user/throttle']
def rt(record):
record['user/angle'] = dk.utils.linear_bin(record['user/angle'])
return record
kl = KerasCategorical()
print('tub_names', tub_names)
if not tub_names:
tub_names = os.path.join(cfg.DATA_PATH, '*')
tubgroup = TubGroup(tub_names)
train_gen, val_gen = tubgroup.get_train_val_gen(X_keys, y_keys, record_transform=rt,
batch_size=cfg.BATCH_SIZE,
train_frac=cfg.TRAIN_TEST_SPLIT)
model_path = os.path.expanduser(model_name)
total_records = len(tubgroup.df)
total_train = int(total_records * cfg.TRAIN_TEST_SPLIT)
total_val = total_records - total_train
print('train: %d, validation: %d' % (total_train, total_val))
steps_per_epoch = total_train // cfg.BATCH_SIZE
print('steps_per_epoch', steps_per_epoch)
kl.train(train_gen,
val_gen,
saved_model_path=model_path,
steps=steps_per_epoch,
train_split=cfg.TRAIN_TEST_SPLIT)
def train(cfg, tub_names, new_model_path, base_model_path=None):
"""
use the specified data in tub_names to train an artifical neural network
saves the output trained model as model_name
"""
X_keys = ['cam/image_array']
y_keys = ['user/angle', 'user/throttle']
def train_record_transform(record):
""" convert categorical steering to linear and apply image augmentations """
record['user/angle'] = dk.util.data.linear_bin(record['user/angle'])
# TODO add augmentation that doesn't use opencv
return record
def val_record_transform(record):
""" convert categorical steering to linear """
record['user/angle'] = dk.util.data.linear_bin(record['user/angle'])
return record
new_model_path = os.path.expanduser(new_model_path)
kl = KerasCategorical()
if base_model_path is not None:
base_model_path = os.path.expanduser(base_model_path)
kl.load(base_model_path)
print('tub_names', tub_names)
if not tub_names:
tub_names = os.path.join(cfg.DATA_PATH, '*')
tubgroup = TubGroup(tub_names)
train_gen, val_gen = tubgroup.get_train_val_gen(
X_keys,
y_keys,
train_record_transform=train_record_transform,
val_record_transform=val_record_transform,
batch_size=cfg.BATCH_SIZE,
train_frac=cfg.TRAIN_TEST_SPLIT)
total_records = len(tubgroup.df)
total_train = int(total_records * cfg.TRAIN_TEST_SPLIT)
total_val = total_records - total_train
print('train: %d, validation: %d' % (total_train, total_val))
steps_per_epoch = total_train // cfg.BATCH_SIZE
print('steps_per_epoch', steps_per_epoch)
kl.train(train_gen,
val_gen,
saved_model_path=new_model_path,
steps=steps_per_epoch,
train_split=cfg.TRAIN_TEST_SPLIT)
def get_model_by_type(model_type, cfg):
from donkeycar.parts.keras import KerasRNN_LSTM, KerasBehavioral, KerasCategorical, KerasIMU, KerasLinear, Keras3D_CNN, KerasLocalizer, KerasLatent
if model_type is None:
model_type = "categorical"
input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
if model_type == "localizer" or cfg.TRAIN_LOCALIZER:
kl = KerasLocalizer(num_outputs=2,
num_behavior_inputs=len(cfg.BEHAVIOR_LIST),
num_locations=cfg.NUM_LOCATIONS,
input_shape=input_shape)
elif model_type == "behavior" or cfg.TRAIN_BEHAVIORS:
kl = KerasBehavioral(num_outputs=2,
num_behavior_inputs=len(cfg.BEHAVIOR_LIST),
input_shape=input_shape)
elif model_type == "imu":
kl = KerasIMU(num_outputs=2, num_imu_inputs=6, input_shape=input_shape)
elif model_type == "linear":
kl = KerasLinear(input_shape=input_shape)
elif model_type == "3d":
kl = Keras3D_CNN(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "rnn":
kl = KerasRNN_LSTM(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "categorical":
kl = KerasCategorical(
input_shape=input_shape,
throttle_range=cfg.MODEL_CATEGORICAL_MAX_THROTTLE_RANGE)
elif model_type == "latent":
kl = KerasLatent(input_shape=input_shape)
else:
raise Exception("unknown model type: %s" % model_type)
return kl
def train(cfg, tub_names, model_name, base_model=None):
'''
use the specified data in tub_names to train an artifical neural network
saves the output trained model as model_name
'''
X_keys = ['cam/image_array']
y_keys = ['user/angle', 'user/throttle']
def rt(record):
record['user/angle'] = dk.utils.linear_bin(record['user/angle'])
return record
kl = KerasCategorical()
#kl = KerasLinear()
print(base_model)
if base_model is not None:
base_model = os.path.expanduser(base_model)
kl.load(base_model)
print('tub_names', tub_names)
if not tub_names:
tub_names = os.path.join(cfg.DATA_PATH, '*')
tubgroup = TubGroup(tub_names)
train_gen, val_gen = tubgroup.get_train_val_gen(
X_keys,
y_keys,
record_transform=rt,
batch_size=cfg.BATCH_SIZE,
train_frac=cfg.TRAIN_TEST_SPLIT)
model_path = os.path.expanduser(model_name)
total_records = len(tubgroup.df)
total_train = int(total_records * cfg.TRAIN_TEST_SPLIT)
total_val = total_records - total_train
print('train: %d, validation: %d' % (total_train, total_val))
steps_per_epoch = total_train // cfg.BATCH_SIZE
print('steps_per_epoch', steps_per_epoch)
kl.train(train_gen,
val_gen,
saved_model_path=model_path,
steps=steps_per_epoch,
train_split=cfg.TRAIN_TEST_SPLIT)
def get_model_by_type(model_type: str, cfg: 'Config') -> 'KerasPilot':
'''
given the string model_type and the configuration settings in cfg
create a Keras model and return it.
'''
from donkeycar.parts.keras import KerasPilot, KerasCategorical, \
KerasLinear, KerasInferred
from donkeycar.parts.tflite import TFLitePilot
if model_type is None:
model_type = cfg.DEFAULT_MODEL_TYPE
print("\"get_model_by_type\" model Type is: {}".format(model_type))
input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
kl: KerasPilot
if model_type == "linear":
kl = KerasLinear(input_shape=input_shape)
elif model_type == "categorical":
kl = KerasCategorical(
input_shape=input_shape,
throttle_range=cfg.MODEL_CATEGORICAL_MAX_THROTTLE_RANGE)
elif model_type == 'inferred':
kl = KerasInferred(input_shape=input_shape)
elif model_type == "tflite_linear":
kl = TFLitePilot()
elif model_type == "tensorrt_linear":
# Aggressively lazy load this. This module imports pycuda.autoinit
# which causes a lot of unexpected things to happen when using TF-GPU
# for training.
from donkeycar.parts.tensorrt import TensorRTLinear
kl = TensorRTLinear(cfg=cfg)
else:
raise Exception("Unknown model type {:}, supported types are "
"linear, categorical, inferred, tflite_linear, "
"tensorrt_linear".format(model_type))
return kl
def get_model_by_type(model_type, cfg):
from donkeycar.parts.keras import KerasRNN_LSTM, KerasBehavioral, KerasCategorical, KerasIMU, KerasLinear, Keras3D_CNN
if model_type is None:
model_type = "categorical"
input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
if model_type == "behavior" or cfg.TRAIN_BEHAVIORS:
kl = KerasBehavioral(num_outputs=2, num_behavior_inputs=len(cfg.BEHAVIOR_LIST), input_shape=input_shape)
elif model_type == "imu":
kl = KerasIMU(num_outputs=2, num_imu_inputs=6, input_shape=input_shape)
elif model_type == "linear":
kl = KerasLinear(input_shape=input_shape)
elif model_type == "3d":
kl = Keras3D_CNN(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "rnn":
kl = KerasRNN_LSTM(seq_length=cfg.SEQUENCE_LENGTH, input_shape=input_shape)
elif model_type == "categorical":
kl = KerasCategorical(input_shape=input_shape)
else:
raise Exception("unknown model type: %s" % model_type)
return kl
def drive(cfg, model_path=None, use_joystick=False, use_reader=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
elif use_reader or cfg.USE_READER_AS_DEFAULT:
pi = pigpio.pi()
ctr = reader(pi,
12,
18,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def autodrive(cfg, model_path=None):
'''Initialize semi-autonomous driving with local_angle and custom throttle option
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = MockCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# # Settings for rover to run pilot_condition
# mode = 'local_angle'
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, cfg.CONSTANT_THROTTLE
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = MockController(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = MockController(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# #add tub to save data
# inputs=['cam/image_array',
# 'user/angle', 'user/throttle',
# 'pilot/angle', 'pilot/throttle',
# 'user/mode']
# types=['image_array',
# 'float', 'float',
# 'float', 'float',
# 'str']
# th = TubHandler(path=cfg.DATA_PATH)
# tub = th.new_tub_writer(inputs=inputs, types=types)
# V.add(tub, inputs=inputs, run_condition='recording')
# debugging inpots/outputs
#attrs = dir(V)
#print(attrs)
for items in V.parts:
print(items)
# Initialize IotClient
#iot = IotClient(cfg, V)
#Start the vehicle
V.start()
try:
V.run(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
except KeyboardInterrupt:
print('pausing')
V.pause()
def drive(cfg,
model_path=None,
use_joystick=False,
use_tx=False,
use_pirf=False):
'''
Start the drive loop
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
# Initialize car
V = dk.vehicle.Vehicle()
if cfg.USE_WEB_CAMERA:
cam = Webcam(resolution=cfg.CAMERA_RESOLUTION)
else:
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
# modify max_throttle closer to 1.0 to have more power
# modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
steering_axis=cfg.JOYSTICK_STEERING_AXIS,
btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON,
btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON,
btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON,
btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON,
btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON,
btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON,
btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON,
btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON,
btn_toggle_const_throttle=cfg.
JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON,
verbose=cfg.JOYSTICK_VERBOSE)
elif use_tx or cfg.USE_TX_AS_DEFAULT:
ctr = TxController(throttle_tx_min=cfg.TX_THROTTLE_MIN,
throttle_tx_max=cfg.TX_THROTTLE_MAX,
steering_tx_min=cfg.TX_STEERING_MIN,
steering_tx_max=cfg.TX_STEERING_MAX,
throttle_tx_thresh=cfg.TX_THROTTLE_TRESH,
verbose=cfg.TX_VERBOSE)
fpv = FPVWebController()
V.add(fpv, inputs=['cam/image_array'], threaded=True)
elif use_pirf or cfg.USE_PI_RF_AS_DEFAULT:
ctr = PiRfController(throttle_tx_min=cfg.PI_RF_THROTTLE_MIN,
throttle_tx_max=cfg.PI_RF_THROTTLE_MAX,
steering_tx_min=cfg.PI_RF_STEERING_MIN,
steering_tx_max=cfg.PI_RF_STEERING_MAX,
throttle_tx_thresh=cfg.PI_RF_THROTTLE_TRESH,
steering_pin=cfg.PI_RF_STEERING_PIN,
throttle_pin=cfg.PI_RF_THROTTLE_PIN,
verbose=cfg.PI_RF_VERBOSE)
fpv = FPVWebController()
V.add(fpv, inputs=['cam/image_array'], threaded=True)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
emergencyCtrl = EmergencyController()
V.add(emergencyCtrl,
inputs=['user/mode'],
outputs=['user/mode'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
if not use_tx and not use_pirf:
# Run the pilot if the mode is not user and not Tx.
kl = KerasCategorical()
#kl = KerasLinear()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
if cfg.USE_PWM_ACTUATOR:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle', 'user/mode'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'pilot/angle', 'pilot/throttle'
]
types = [
'image_array', 'float', 'float', 'str', 'numpy.float32',
'numpy.float32'
]
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
cam.camera.rotation = 180 #flip camera
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
#GPIO.clean_up()
GPIO.setmode(GPIO.BOARD)
PCA9685_pwm = Adafruit_PCA9685.PCA9685()
PCA9685_pwm.set_pwm_freq(cfg.MOTFREQ)
time.sleep(1)
throttle_controller = PCA9685L298N(cfg.THROTTLE_CHANNEL,PCA9685_pwm)
throttle = PWMThrottleL298N(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM,
pin1 = cfg.THROTIN1,
pin2 = cfg.THROTIN2,
io = GPIO)
steering_controller = PCA9685L298N(cfg.STEERING_CHANNEL,PCA9685_pwm)
steering = PWMSteeringL298N(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM,
pin1 = cfg.STEERIN1,
pin2 = cfg.STEERIN2,
io = GPIO)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types=['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def plot_predictions(self, cfg, tub_paths, model_path):
'''
Plot model predictions for angle and throttle against data from tubs.
'''
import matplotlib.pyplot as plt
import pandas as pd
from donkeycar.parts.datastore import TubGroup
from donkeycar.parts.keras import KerasCategorical
tg = TubGroup(tub_paths)
model_path = os.path.expanduser(model_path)
model = KerasCategorical()
model.load(model_path)
gen = tg.get_batch_gen(None,
None,
batch_size=len(tg.df),
shuffle=False,
df=tg.df)
arr = next(gen)
user_angles = []
user_throttles = []
pilot_angles = []
pilot_throttles = []
for tub in tg.tubs:
num_records = tub.get_num_records()
for iRec in tub.get_index(shuffled=False):
record = tub.get_record(iRec)
img = record["cam/image_array"]
user_angle = float(record["user/angle"])
user_throttle = float(record["user/throttle"])
# user_angle = float(record["pilot/angle"])
# user_throttle = float(record["pilot/throttle"])
pilot_angle, pilot_throttle = model.run(img)
user_angles.append(user_angle)
user_throttles.append(user_throttle)
pilot_angles.append(pilot_angle)
pilot_throttles.append(pilot_throttle)
angles_df = pd.DataFrame({
'user_angle': user_angles,
'pilot_angle': pilot_angles
})
throttles_df = pd.DataFrame({
'user_throttle': user_throttles,
'pilot_throttle': pilot_throttles
})
fig = plt.figure()
title = "Model Predictions\nTubs: {}\nModel: {}".format(
tub_paths, model_path)
fig.suptitle(title)
ax1 = fig.add_subplot(211)
ax2 = fig.add_subplot(212)
angles_df.plot(ax=ax1)
throttles_df.plot(ax=ax2)
ax1.legend(loc=4)
ax2.legend(loc=4)
plt.show()
def turn(cfg):
'''Initialize semi-autonomous driving with local_angle and custom throttle option
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode, keypress_mode):
if mode == 'user':
return False
elif mode == 'local_angle' and keypress_mode == 'pause':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part,
inputs=['user/mode', 'keypress/mode'],
outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, cfg.CONSTANT_THROTTLE
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# debugging inpots/outputs
#attrs = dir(V)
#print(attrs)
for items in V.parts:
print(items)
#Start the vehicle
V.start()
iterations = 0
try:
while iterations < 20:
print("----------------------------")
print("Iteration: " + str(iterations))
# Execute the three point turn
V.three_point_turn(rate_hz=cfg.DRIVE_LOOP_HZ)
iterations = iterations + 1
print("----------------------------")
time.sleep(100)
except KeyboardInterrupt:
pass
# Execute the circle turn
#V.circle_turn(rate_hz=cfg.DRIVE_LOOP_HZ)
# Stop vehicle gracefully (if we ever get here)
V.stop()
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
center_led = rgb_led(red_channel=9, green_channel=10, blue_channel=11)
status_led = status_indicator(status_led=center_led)
V.add(center_led, outputs=['none'])
V.add(status_led, inputs=['user/mode', 'recording'])
#add pi_perfchecker
#loop_time = driveLoopTime()
#loop_time.console=True
#core_temp = coreTemp()
#V.add(core_temp)
#V.add(loop_time,inputs = ['timein'], outputs = ['timein','displaytext'])
#throtled_status = throttled()
#V.add(throtled_status, outputs=['displaytext'],threaded=True)
loop_time = driveLoopTime()
V.add(loop_time, inputs=['timein'], outputs=['timein', 'displaytext'])
loop_time_display = ImgPutText(org=(100, 20))
clock = Timestamp()
V.add(clock, outputs=['timestamp'])
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
#cam = PiCamera(resolution=(960,1280))
V.add(cam, outputs=['cam/image_array'], threaded=True)
#ncs_ty = tinyyolo(basedir = cfg.MODELS_PATH, draw_on_img = True, probability_threshold = 0.15,debug=False)
#V.add(ncs_ty, inputs=['cam/image_array'],outputs=['cam/image_array','ncs/found_objs'],threaded=True)
V.add(loop_time_display,
inputs=['cam/image_array', 'displaytext'],
outputs=['cam/image_array'])
#img_resize = ImgResize()
#V.add(img_resize,inputs=['cam/image_array'],outputs=['cam/image_array'])
#ncs_gn = googlenet(basedir=cfg.MODELS_PATH, debug=True)
#V.add(ncs_gn, inputs=['cam/image_array'],outputs=['ncs/image_array', 'classificaiton'],threaded=True)
#ncs_inception = inception(basedir=cfg.MODELS_PATH, probability_threshold=0.01, debug=True)
#V.add(ncs_inception, inputs=['cam/image_array'],outputs=['cam/image_array', 'classificaiton'],threaded=True)
#face_detect = detect(basedir=cfg.MODELS_PATH, debug=True)
# V.add(face_detect, inputs=['cam/image_array'],outputs=['face/image_array', 'face_center'],threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis='rz', #throttle_axis='y',
steering_axis='x',
panning_axis='rx',
tilting_axis='ry')
"""
when running from JS, let us display the nsc images on the web page
"""
"""
ctr_webview = LocalWebController()
V.add(ctr_webview,
inputs=['cam/image_array'],
#inputs=['cam/image_array'],
outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'],
#outputs=['outargs'],
threaded=True)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
"""
V.add(
ctr,
inputs=['cam/image_array'],
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording',
'user/pan', 'user/tilt'
],
#outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
ctr_webview = LocalWebController()
V.add(
ctr_webview,
inputs=['cam/image_array'],
#inputs=['cam/image_array'],
outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'],
#outputs=['outargs'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
#kl = KerasLinear()
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle, user_pan, user_tilt, face_pan, face_tilt):
if mode == 'user':
return user_angle, user_throttle, user_pan, user_tilt
elif mode == 'face':
return user_angle, user_throttle, face_pan, face_tilt
elif mode == 'local_angle':
return pilot_angle, user_throttle, face_pan, face_tilt
else:
return pilot_angle, pilot_throttle, face_pan, face_tilt
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle', 'user/pan', 'user/tilt', 'face/pan',
'face/tilt'
],
outputs=['angle', 'throttle', 'pan', 'tilt'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
panning_controller = PCA9685(cfg.PAN_CHANNEL)
panning = PWMPanning(controller=panning_controller,
left_pulse=cfg.PAN_LEFT_PWM,
zero_pulse=cfg.PAN_CENTER_PWM,
right_pulse=cfg.PAN_RIGHT_PWM)
tilting_controller = PCA9685(cfg.TILT_CHANNEL)
tilting = PWMTilting(controller=tilting_controller,
max_pulse=cfg.TILT_UP_PWM,
zero_pulse=cfg.TILT_DRIVING_PWM,
min_pulse=cfg.TILT_DOWN_PWM)
wagging_controller = PCA9685(cfg.TAIL_CHANNEL)
wagging = PWMWagging(controller=wagging_controller,
max_pulse=cfg.TAIL_UP_PWM,
zero_pulse=cfg.TAIL_CENTER_PWM,
min_pulse=cfg.TAIL_DOWN_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#looping pan and tilt out to donkeyface, then back into this donkeypet
donkeyPet_sender = send_to_donkey(send_to_address='10.11.44.20')
V.add(donkeyPet_sender,
inputs=['pan', 'tilt', 'user/mode', 'recording', 'timestamp'],
outputs=['donkeyface_output'])
donkeyPet_receiver = rcv_from_donkey(listen_to_port=10500, debug=False)
V.add(donkeyPet_receiver,
inputs=['rcv_in'],
outputs=['face/pan', 'face/tilt', 'face/irq'])
feels_on_target = target_status(debug=True)
V.add(feels_on_target,
inputs=['face/irq', 'tail', 'user/mode'],
outputs=['tail'])
V.add(panning, inputs=['face/pan'])
V.add(tilting, inputs=['face/tilt'])
V.add(wagging, inputs=["tail"])
# add tub to save data
#pan and tilt setup
tub_inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
tub_types = ['image_array', 'float', 'float', 'str', 'str']
#non pan and tilt test
#tub_inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp','test_var']
#tub_types = ['image_array', 'float', 'float', 'str', 'str','str']
# multiple tubs
# th = TubHandler(path=cfg.DATA_PATH)
# tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=tub_inputs, types=tub_types)
V.add(tub, inputs=tub_inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def test_categorical():
kc = KerasCategorical()
assert kc.model is not None
def get_model_by_type(model_type, cfg, image_dim=None):
'''
given the string model_type and the configuration settings in cfg
create a Keras model and return it.
@param model_type A string that matches one of the supported model types.
@param cfg A DonkeyCar style config object.
@param image_dim A config object or an ImageDim namedtuple and will override what's in cfg.
'''
from donkeycar.parts.keras import KerasRNN_LSTM, KerasBehavioral, KerasCategorical, KerasIMU, KerasLinear, Keras3D_CNN, KerasLocalizer, KerasLatent
from donkeycar.parts.tflite import TFLitePilot
if model_type is None:
model_type = cfg.DEFAULT_MODEL_TYPE
print("\"get_model_by_type\" model Type is: {}".format(model_type))
if image_dim is None:
input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
else:
input_shape = (image_dim.IMAGE_H, image_dim.IMAGE_W,
image_dim.IMAGE_DEPTH)
roi_crop = (cfg.ROI_CROP_TOP, cfg.ROI_CROP_BOTTOM)
if model_type == "tflite_linear":
kl = TFLitePilot()
elif model_type == "localizer" or cfg.TRAIN_LOCALIZER:
kl = KerasLocalizer(num_outputs=2,
num_behavior_inputs=len(cfg.BEHAVIOR_LIST),
num_locations=cfg.NUM_LOCATIONS,
input_shape=input_shape)
elif model_type == "behavior" or cfg.TRAIN_BEHAVIORS:
kl = KerasBehavioral(num_outputs=2,
num_behavior_inputs=len(cfg.BEHAVIOR_LIST),
input_shape=input_shape)
elif model_type == "imu":
kl = KerasIMU(num_outputs=2, num_imu_inputs=6, input_shape=input_shape)
elif model_type == "linear":
kl = KerasLinear(input_shape=input_shape, roi_crop=roi_crop)
elif model_type == "tensorrt_linear":
# Aggressively lazy load this. This module imports pycuda.autoinit which causes a lot of unexpected things
# to happen when using TF-GPU for training.
from donkeycar.parts.tensorrt import TensorRTLinear
kl = TensorRTLinear(cfg=cfg)
elif model_type == "coral_tflite_linear":
from donkeycar.parts.coral import CoralLinearPilot
kl = CoralLinearPilot()
elif model_type == "3d":
kl = Keras3D_CNN(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "rnn":
kl = KerasRNN_LSTM(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "categorical":
kl = KerasCategorical(
input_shape=input_shape,
throttle_range=cfg.MODEL_CATEGORICAL_MAX_THROTTLE_RANGE,
roi_crop=roi_crop)
elif model_type == "latent":
kl = KerasLatent(input_shape=input_shape)
elif model_type == "fastai":
from donkeycar.parts.fastai import FastAiPilot
kl = FastAiPilot()
else:
raise Exception("unknown model type: %s" % model_type)
return kl
if not hasattr(env, 'saver'):
return print("Eroor: cannot find saver op")
print('Loading saved model...')
return env.saver.restore(sess,'model/{}'.format(name))
print("Training model...")
model = default_model()
img_arr = img_arr.reshape((1,) + img_arr.shape)
angle_binned, throttle = self.model.predict(img_arr)
#print('throttle', throttle)
#angle_certainty = max(angle_binned[0])
angle_unbinned = dk.utils.linear_unbin(angle_binned)
return angle_unbinned, throttle[0][0]
"""
if __name__ == '__main__':
args = docopt(__doc__)
# 模型的路径
model_path = "model_path"
# 存放图片数据的路径
x_path = "x_data_path"
if args['model_path']:
model_path = args['--model']
if args['x_path']:
x_path = args['--x_path']
kl = KerasCategorical()
model = kl.load(model_path)
x_adv = fgm(model, x_image) #生成FGSM对抗样本
def run(self, args):
'''
Start a websocket SocketIO server to talk to a donkey simulator
'''
import socketio
from donkeycar.parts.simulation import SteeringServer
from donkeycar.parts.keras import KerasCategorical, KerasLinear,\
Keras3D_CNN, KerasRNN_LSTM
args, parser = self.parse_args(args)
cfg = load_config(args.config)
if cfg is None:
return
#TODO: this logic should be in a pilot or model handler part.
if args.type == "categorical":
kl = KerasCategorical()
elif args.type == "linear":
kl = KerasLinear(num_outputs=2)
elif args.type == "rnn":
kl = KerasRNN_LSTM(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH,
num_outputs=2)
elif args.type == "3d":
kl = Keras3D_CNN(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH,
num_outputs=2)
else:
print("didn't recognize type:", args.type)
return
#can provide an optional image filter part
img_stack = None
#load keras model
kl.load(args.model)
#start socket server framework
sio = socketio.Server()
top_speed = float(args.top_speed)
#start sim server handler
ss = SteeringServer(sio,
kpart=kl,
top_speed=top_speed,
image_part=img_stack)
#register events and pass to server handlers
@sio.on('telemetry')
def telemetry(sid, data):
ss.telemetry(sid, data)
@sio.on('connect')
def connect(sid, environ):
ss.connect(sid, environ)
ss.go(('0.0.0.0', 9090))
def drive(cfg, model_path=None, use_joystick=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
#this part stacks the last 3 images into channels of a single output image
img_stack = ImgStack()
V.add(img_stack,
inputs=['cam/image_array'],
outputs=['img_stack'])
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['img_stack'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_contion only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['img_stack'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = JankyJoystick(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = JankyJoystickcfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
def playback(cfg, tub_names, model_name=None):
if not tub_names:
tub_names = os.path.join(cfg.DATA_PATH, '*')
tubgroup = TubGroup(tub_names)
if not model_name is None:
from donkeycar.parts.keras import KerasCategorical
kl = KerasCategorical()
kl.load(model_name)
pilot_angles = []
pilot_throttles = []
print('tub_names', tub_names)
tub_paths = utils.expand_path_arg(tub_names)
print('TubGroup:tubpaths:', tub_paths)
user_angles = []
user_throttles = []
tubs = [Tub(path) for path in tub_paths]
for tub in tubs:
num_records = tub.get_num_records()
print(num_records)
for iRec in tub.get_index(shuffled=False):
record = tub.get_record(iRec)
#record = tubs.get_record(random.randint(1,num_records+1))
img = record["cam/image_array"]
user_angle = float(record["user/angle"])
user_throttle = float(record["user/throttle"])
user_angles.append(user_angle)
user_throttles.append(user_throttle)
if not model_name is None:
pilot_angle, pilot_throttle = kl.run(img)
pilot_angles.append(pilot_angle)
pilot_throttles.append(pilot_throttle)
record = tubs[0].get_record(random.randint(1,num_records+1))
user_angle = float(record["user/angle"])
user_throttle = float(record["user/throttle"])
print(img.shape)
print('-----')
print(user_angle)
print(user_throttle)
plt.figure()
plt.imshow(img)
plt.plot([80,80+10*user_throttle*np.cos(user_angle)],[120,120+100*user_throttle*np.sin(user_angle)])
plt.figure()
plt.plot(user_angles)
plt.plot(user_throttles)
fig = plt.figure()
ax1 = plt.subplot2grid((2,2),(0,0))
record = tubs[0].get_record(1)
img = record["cam/image_array"]
imPlot = ax1.imshow(img,animated=True)
floorImg = lookAtFloorImg(img)
print(floorImg)
ax3 = plt.subplot2grid((2,2),(0,1))
imPlot2 = ax3.imshow(floorImg,animated=True)
ax2 = plt.subplot2grid((2,2),(1,0), colspan=2)
line1, = ax2.plot(user_angles)
line2, = ax2.plot(user_throttles)
if not model_name is None:
line4, = ax2.plot(pilot_angles)
line5, = ax2.plot(pilot_throttles)
line3, = ax2.plot([0,0],[-1,1])
def animate(i):
record = tubs[0].get_record(i)
img = record["cam/image_array"]
imPlot.set_array(img)
imPlot2.set_array(lookAtFloorImg(img))
line3.set_data([i,i],[-1,1])
#print(i)
#sys.stdout.flush()
return imPlot,
# Init only required for blitting to give a clean slate.
def init():
record = tubs[0].get_record(1)
img = record["cam/image_array"]
imPlot.set_array(img)
line3.set_data([0,0],[-1,1])
return imPlot,
ani = animation.FuncAnimation(fig, animate, np.arange(1, tubs[0].get_num_records()),
interval=100, blit=False)
plt.show()
def drive(cfg, model_path=None, use_joystick=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
#Camera
#cam = MockCamera(resolution=cfg.CAMERA_RESOLUTION)
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
#Cropper
#ImgCrop works by cropping num pixels in from side of each border
mask = ImgCrop(200, 80, 0, 0) #top, bottom, left, right, but it's all backwards because OpenCV
#V.add(mask, inputs=['cam/image_array'], outputs=['cam/filtered1'])
#Greyscale filter
#grey_filter = ImgGreyscale()
#V.add(grey_filter, inputs=['cam/filtered1'], outputs=['cam/filtered2'])
#Gaussian blur
#gaus_blur = ImgGaussianBlur()
#V.add(gaus_blur, inputs=['cam/filtered2'], outputs=['cam/filtered3'])
#Adaptive threshold
#adaptive_thresh = AdaptiveThreshold()
#V.add(adaptive_thresh, inputs=['cam/filtered3'], outputs=['cam/filtered4'])
#Birds eye viewpoint transformation
#birds_eye = BirdsEyePerspectiveTxfrm()
#V.add(birds_eye, inputs=['cam/filtered4'], outputs=['cam/filtered_final'])
#Draw line
#draw_line = DrawLine((0, 200), (480, 200))
#V.add(draw_line, inputs=['cam/filtered4'], outputs=['cam/filtered_final'])
#Controller
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
#Lukes notes: This is another kind of a splitter, more like a flag, that gets piped into KerasCategorical to decide whether or not the autopilot has control over steering parts (technically, it is used as a run condition for the autopilot)
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/filtered_final'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
#Lukes notes: This is essentially a splitter - it links input channels to output (control) channels based on which mode the car was run in
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types=['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def get_model_by_type(model_type, cfg):
'''
given the string model_type and the configuration settings in cfg
create a Keras model and return it.
'''
from donkeycar.parts.keras import KerasRNN_LSTM, KerasBehavioral, \
KerasCategorical, KerasIMU, KerasLinear, Keras3D_CNN, \
KerasLocalizer, KerasLatent
from donkeycar.parts.tflite import TFLitePilot
if model_type is None:
model_type = cfg.DEFAULT_MODEL_TYPE
print("\"get_model_by_type\" model Type is: {}".format(model_type))
input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
roi_crop = (cfg.ROI_CROP_TOP, cfg.ROI_CROP_BOTTOM)
if model_type in [DAVE2, CHAUFFEUR, EPOCH, RAMBO, DEFAULT_DONKEY]:
# return get_own_model(model_type, (140, 320, 3))()
input_size = (
int(input_shape[0] - roi_crop[0] - roi_crop[1]),
int(input_shape[1]),
int(input_shape[2])
)
return get_own_model(model_type, input_size)()
if model_type == "tflite_linear":
kl = TFLitePilot()
elif model_type == "localizer" or cfg.TRAIN_LOCALIZER:
kl = KerasLocalizer(num_locations=cfg.NUM_LOCATIONS, input_shape=input_shape)
elif model_type == "behavior" or cfg.TRAIN_BEHAVIORS:
kl = KerasBehavioral(num_outputs=2, num_behavior_inputs=len(cfg.BEHAVIOR_LIST), input_shape=input_shape)
elif model_type == "imu":
kl = KerasIMU(num_outputs=2, num_imu_inputs=6, input_shape=input_shape, roi_crop=roi_crop)
elif model_type == "linear":
kl = KerasLinear(input_shape=input_shape, roi_crop=roi_crop)
elif model_type == "tensorrt_linear":
# Aggressively lazy load this. This module imports pycuda.autoinit which causes a lot of unexpected things
# to happen when using TF-GPU for training.
from donkeycar.parts.tensorrt import TensorRTLinear
kl = TensorRTLinear(cfg=cfg)
elif model_type == "coral_tflite_linear":
from donkeycar.parts.coral import CoralLinearPilot
kl = CoralLinearPilot()
elif model_type == "3d":
kl = Keras3D_CNN(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH, roi_crop=roi_crop)
elif model_type == "rnn":
kl = KerasRNN_LSTM(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH,
seq_length=cfg.SEQUENCE_LENGTH, roi_crop=roi_crop)
elif model_type == "categorical":
kl = KerasCategorical(input_shape=input_shape, throttle_range=cfg.MODEL_CATEGORICAL_MAX_THROTTLE_RANGE,
roi_crop=roi_crop)
elif model_type == "latent":
kl = KerasLatent(input_shape=input_shape)
elif model_type == "fastai":
from donkeycar.parts.fastai import FastAiPilot
kl = FastAiPilot()
else:
raise Exception("unknown model type: %s" % model_type)
return kl
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#setup AWS IoT
if cfg.IOT_ENABLED:
aws = AWSHandler(vehicle_id=cfg.VEHICLE_ID,
endpoint=cfg.AWS_ENDPOINT,
ca=cfg.CA_PATH,
private_key=cfg.PRIVATE_KEY_PATH,
certificate=cfg.CERTIFICATE_PATH)
iot = aws.new_iot_publisher(intputs=inputs, types=types)
V.add(iot, inputs=inputs, run_condition='recording')
#run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
memory = RedisMemory()
V = dk.vehicle.Vehicle(mem=memory)
clock = Timestamp()
V.add(clock, outputs='timestamp')
# ***** CAMERA *****
print("Starting camera")
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
# ***** Web Controller *****
print("Starting web controller")
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# ***** SPEKTRUM/MOVE32 REMOTE *****
print("Starting Spektrum/Move32")
#rc = SpektrumRemoteReceiver(cfg.SPEKTRUM_OFFSET, cfg.SPEKTRUM_SCALE, cfg.SPEKTRUM_DEFAULT, cfg.SPEKTRUM_SERIALPORT)
rc = Move32Receiver(cfg.MOVE32_OFFSET, cfg.MOVE32_SCALE,
cfg.MOVE32_DEFAULT, cfg.MOVE32_SERIALPORT,
cfg.MOVE32_RXTYPE, cfg.MOVE32_RXAUTO,
cfg.MOVE32_TIMEOUT)
V.add(rc,
threaded=True,
outputs=['rc0', 'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7'])
def rc_convert_func(*args):
angle = args[0]
throttle = args[1]
mode = 'manual' if args[
2] > 0.3 else 'auto' if args[2] < -0.3 else 'auto_angle'
recording = args[3] <= 0.3
return angle, throttle, mode, recording
V.add(Lambda(rc_convert_func),
inputs=['rc0', 'rc2', 'rc5', 'rc4'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'])
# ***** user/mode -> run_pilot *****
V.add(Lambda(lambda mode: mode.lower() != 'manual'),
inputs=['user/mode'],
outputs=['run_pilot'])
# ***** cam/image_array -> pilot/angle,pilot_throttle *****
# Run the pilot if the mode is not user.
print("Starting KerasCategorical")
kl = KerasCategorical()
if model_path:
print("Loading model...")
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# ***** user/*, pilot/* -> angle, throttle *****
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'manual':
return user_angle, user_throttle
elif mode == 'auto_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
# ***** throttle, angle -> motor_left, motor_right *****
ackermann_to_diff_converter = AckermannToDifferentialDriveConverter(
cfg.ACKERMANN_LENGTH, cfg.ACKERMANN_WIDTH)
V.add(ackermann_to_diff_converter,
inputs=['angle', 'throttle'],
outputs=['motor_left', 'motor_right'])
# ***** motor_left, motor_right -> DRIVE *****
motors_part = DifferentialDriveActuator_MotorHat(
cfg.MOTORHAT_ADDR, cfg.MOTORHAT_LEFT_FRONT_ID,
cfg.MOTORHAT_LEFT_REAR_ID, cfg.MOTORHAT_RIGHT_FRONT_ID,
cfg.MOTORHAT_RIGHT_REAR_ID)
V.add(motors_part, inputs=['motor_left', 'motor_right'])
# ***** output debug data *****
debug_keys = [
'user/mode',
'recording',
'run_pilot',
"angle",
"throttle",
"motor_left",
"motor_right",
] #'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7', 'rc8']
def debug_func(*args):
print(args[0], " ", args[1], " ", args[2],
" ".join("{:5.2f}".format(e) for e in args[3:]))
V.add(Lambda(debug_func), inputs=debug_keys)
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def autodrive(cfg, model_path=None):
'''Initialize semi-autonomous driving with local_angle and custom throttle option
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode, keypress_mode):
if mode == 'user':
return False
elif mode == 'local_angle' and keypress_mode == 'pause':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part,
inputs=['user/mode', 'keypress/mode'],
outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, cfg.CONSTANT_THROTTLE
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# debugging inpots/outputs
#attrs = dir(V)
#print(attrs)
for items in V.parts:
print(items)
# Initialize IotClient
iot = IotClient(cfg, V)
#Start the vehicle
V.start()
# Loop forever so IotClient can do it's thing
try:
while True:
if iot.moving() is True:
if iot.get_destination(
) is not 0: # Only load new model if we have to
print("Using model at " +
cfg.MODEL_MAP[iot.get_model_num()])
print("Loading new model...")
V.get("KerasCategorical").load(
cfg.MODEL_MAP[iot.get_model_num()])
else:
print("Going back to kitchen using model " +
cfg.MODEL_MAP[iot.get_model_num()])
try:
V.run(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
print("V.run returned...I should never see this")
except KeyboardInterrupt:
print('Pausing rover')
V.pause()
# V.three_point_turn()
#iot.update_shadow_after_stop()
iot.update_shadow_demo_lite()
time.sleep(1)
except KeyboardInterrupt:
pass
# Stop vehicle gracefully (if we ever get here)
V.stop()
def drive(cfg, model_path=None):
V = dk.vehicle.Vehicle()
V.mem.put(['square/angle', 'square/throttle'], (100, 100))
# display square box given by cooridantes.
cam = SquareBoxCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam,
inputs=['square/angle', 'square/throttle'],
outputs=['cam/image_array'])
# display the image and read user values from a local web controller
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle',
'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_contion only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# See if we should even run the pilot module.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'pilot_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
clock = Timestamp()
V.add(clock, outputs=['timestamp'])
# transform angle and throttle values to coordinate values
def f(x):
return int(x * 100 + 100)
l = Lambda(f)
V.add(l, inputs=['user/angle'], outputs=['square/angle'])
V.add(l, inputs=['user/throttle'], outputs=['square/throttle'])
# add tub to save data
inputs=['cam/image_array',
'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle',
'square/angle', 'square/throttle',
'user/mode',
'timestamp']
types=['image_array',
'float', 'float',
'float', 'float',
'float', 'float',
'str',
'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle for 20 seconds
V.start(rate_hz=50, max_loop_count=10000)
def drive(cfg, model_path=None, use_joystick=False, use_tx=False):
global myConfig
global throttle
global ctr
global V
'''
Start the drive loop
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
# Initialize car
V = dk.vehicle.Vehicle()
configCtrl = ConfigController(cfg.CONFIG_PATH)
logger = logging.getLogger(myConfig['DEBUG']['PARTS']['MAIN']['NAME'])
logger.setLevel(CONFIG2LEVEL[myConfig['DEBUG']['PARTS']['MAIN']['LEVEL']])
V.add(configCtrl, threaded=True)
def get_tsc():
return int(round(time.time() * 1000))
logger.info("Init timestamper")
get_tsc_part = Lambda(get_tsc)
V.add(get_tsc_part, outputs=['ms'])
logger.info("Init Cam part")
if cfg.USE_WEB_CAMERA:
cam = Webcam(resolution=cfg.CAMERA_RESOLUTION,
fps=cfg.CAMERA_FPS,
framerate=cfg.CAMERA_FRAMERATE)
else:
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
logger.info("Init Controller part")
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
# modify max_throttle closer to 1.0 to have more power
# modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
steering_axis=cfg.JOYSTICK_STEERING_AXIS,
btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON,
btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON,
btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON,
btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON,
btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON,
btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON,
btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON,
btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON,
btn_toggle_const_throttle=cfg.
JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON,
verbose=cfg.JOYSTICK_VERBOSE)
V.add(
ctr,
inputs=['cam/image_array', 'pilot/annoted_img'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
elif use_tx or cfg.USE_TX_AS_DEFAULT:
#This is Tx controller (pilot Donkey from a RC Tx transmiter/receiver)
ctr = TxController(verbose=cfg.TX_VERBOSE)
V.add(ctr,
inputs=[
'user/mode', 'vehicle_armed', 'cam/image_array',
'pilot/annoted_img'
],
outputs=[
'user/angle', 'user/throttle', 'recording', 'lane', 'ch5',
'ch6', 'speedometer', 'sensor_left', 'sensor_right'
],
threaded=True)
actionctr = TxAuxCh()
V.add(actionctr,
inputs=['user/mode', 'vehicle_armed', 'ch5', 'ch6', 'recording'],
outputs=['user/mode', 'vehicle_armed', 'flag', 'recording'],
threaded=False)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(
ctr,
inputs=['cam/image_array', 'pilot/annoted_img'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
if cfg.USE_THROTTLEINLINE:
logger.info("Init throttleInLine part")
throttleinline = ThrottleInLine(cfg.THROTTLEINLINE_ANGLE_MIN,
cfg.THROTTLEINLINE_ANGLE_MAX)
V.add(throttleinline,
inputs=['cam/image_array'],
outputs=['pilot/throttle_boost', 'pilot/annoted_img'],
threaded=True)
logger.info("Init emergency part")
emergencyCtrl = EmergencyController()
V.add(emergencyCtrl,
inputs=['user/mode'],
outputs=['user/mode'],
threaded=True)
perfMngt = dumpPerf()
V.add(perfMngt, inputs=['user/mode'], threaded=False)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
logger.info("Init pilot part")
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
logger.info("Init Model part")
# Run the pilot if the mode is not user and not Tx.
if (myConfig['MODEL']['MODEL_IN_USE'] == 0):
kl = KerasCategorical()
if (myConfig['MODEL']['MODEL_IN_USE'] == 1):
kl = KerasCategorical1()
#kl = KerasLinear()
if model_path:
if (os.path.exists(model_path)):
logger.info("IA : Load integrated model")
kl.load(model_path)
else:
# Model reconstruction from JSON file
logger.info("IA : Load Weights + Model Architecture model")
kl.load2(model_path)
if (myConfig['MODEL']['MODEL_IN_USE'] == 0):
V.add(kl,
inputs=['cam/image_array'],
outputs=[
'pilot/angle', 'pilot/throttle', 'pilot/fullspeed',
'pilot/lane', 'pilot/angle_bind'
],
run_condition='run_pilot')
if (myConfig['MODEL']['MODEL_IN_USE'] == 1):
V.add(kl,
inputs=['cam/image_array', 'speedometer'],
outputs=[
'pilot/angle', 'pilot/throttle', 'pilot/fullspeed',
'pilot/brake', 'pilot/angle_bind'
],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle, throttle_boost):
if mode == 'user':
return user_angle, user_throttle
else:
if cfg.USE_THROTTLEINLINE:
if throttle_boost:
pilot_throttle = pilot_throttle * cfg.THROTTLEINLINE_BOOST_FACTOR
logger.debug("Apply Boost")
if mode == 'local_angle':
return pilot_angle, user_throttle
else:
logger.debug(
'drive_mode: Pilot return angle={:01.2f} throttle={:01.2f}'
.format(pilot_angle, pilot_throttle))
if (pilot_angle >
myConfig['POST_PILOT']['STEERING_TRIM_RIGHT_THRES']):
pilot_angle = pilot_angle * myConfig['POST_PILOT'][
'STEERING_TRIM_RIGHT_FACTOR']
if (pilot_angle <
-myConfig['POST_PILOT']['STEERING_TRIM_LEFT_THRES']):
pilot_angle = pilot_angle * myConfig['POST_PILOT'][
'STEERING_TRIM_LEFT_FACTOR']
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle', 'pilot/throttle_boost'
],
outputs=['angle', 'throttle'])
if cfg.USE_PWM_ACTUATOR:
logger.info("Init Actuator part")
if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1:
steering_controller = ctr
else:
steering_controller = PCA9685(channel=cfg.STEERING_CHANNEL,
busnum=cfg.STEERING_I2C_BUS)
steering = PWMSteering(controller=steering_controller)
if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1:
throttle_controller = ctr
else:
throttle_controller = PCA9685(channel=cfg.THROTTLE_CHANNEL,
busnum=cfg.THROTTLE_I2C_BUS)
throttle = PWMThrottle(controller=throttle_controller)
V.add(steering, inputs=['angle'])
V.add(throttle,
inputs=[
'throttle', 'user/mode', 'vehicle_armed', 'pilot/fullspeed',
None, 'pilot/lane', 'sensor_left', 'sensor_right'
])
if cfg.BATTERY_USE_MONITOR:
logger.info("Init Battery Monitor part")
battery_controller = BatteryController(nbCells=cfg.BATTERY_NCELLS)
V.add(battery_controller, outputs=['battery'], threaded=True)
# add tub to save data
inputs = [
'cam/image_array', 'ms', 'user/angle', 'user/throttle', 'user/mode',
'pilot/angle', 'pilot/throttle', 'flag', 'speedometer', 'lane'
]
types = [
'image_array', 'int', 'float', 'float', 'str', 'numpy.float32',
'numpy.float32', 'str', 'float', 'int'
]
logger.info("Init Tub Handler part")
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
if use_tx or cfg.USE_TX_AS_DEFAULT:
fpv = FPVWebController()
V.add(fpv,
inputs=[
'cam/image_array', 'pilot/annoted_img', 'user/angle',
'user/throttle', 'user/mode', 'pilot/angle',
'pilot/throttle', 'pilot/throttle_boost', 'pilot/fullspeed',
'pilot/angle_bind'
],
threaded=True)
logger.info("Start main loop")
# run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def test_categorical_with_model():
kc = KerasCategorical(default_categorical())
assert kc.model is not None
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
def get_timestamp():
return time.time()
clock = Lambda(get_timestamp)
V.add(clock, outputs=['timestamp'])
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
curr_angle = 0.0
def smooth_angle(in_angle):
nonlocal curr_angle
delta = in_angle - curr_angle
if abs(delta) < 0.02:
curr_angle = in_angle
else:
curr_angle = curr_angle + delta * 0.15
print('smoothed angle:', curr_angle)
return curr_angle
angleSmoother = Lambda(smooth_angle)
V.add(angleSmoother, inputs=['user/angle'], outputs=['user/angle'])
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'],
outputs=['run_pilot'])
# Add velocity detector
vd = Lambda(make_velocity_detector())
V.add(vd, inputs=['cam/image_array'], outputs=['user/velocity'])
""""
def print_velocity(v):
print('velocity:', v)
pv = Lambda(print_velocity)
V.add(pv, inputs=['user/velocity'])
"""
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/velocity'],
run_condition='run_pilot')
def calc_throttle(inferred_v, actual_v):
throttle = 0.38
"""
if inferred_v > actual_v:
throttle = min((inferred_v - actual_v) * 1.0, 0.5)
print('V: inferred={} actual={} => throttle={}'.format(inferred_v, actual_v, throttle));
"""
if (inferred_v > 5):
throttle = 0.48
elif (inferred_v > 4):
throttle = 0.44
elif (inferred_v > 3):
throttle = 0.42
elif (inferred_v > 2):
throttle = 0.41
elif (inferred_v > 1):
throttle = 0.39
return throttle
ct = Lambda(calc_throttle)
V.add(ct, inputs=['pilot/velocity', 'user/velocity'],
outputs=['pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
# manual steer, manual throttle
return user_angle, user_throttle
elif mode == 'local_angle':
# auto steer, manual throttle
return pilot_angle, user_throttle
else:
# auto steer, auto throttle
print('PILOT: angle={} throttle={}'.format(pilot_angle, pilot_throttle))
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/velocity', 'user/throttle', 'user/mode', 'timestamp']
types = ['image_array', 'float', 'float', 'float', 'str', 'float']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
ctr = SerialController()
V.add(ctr,
inputs=[],
outputs=['user/angle', 'user/throttle', 'user/mode'],
threaded=True)
V.add(LocalWebController(use_chaos=use_chaos),
inputs=['cam/image_array'],
outputs=['a', 'b', 'c', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def train(cfg, tub_names, model_name, model_type):
'''
use the specified data in tub_names to train an artifical neural network
saves the output trained model as model_name
'''
X_keys = ['cam/image_array']
y_keys = ['user/angle', 'user/throttle']
binning = dk.utils.linear_bin
if model_type == "hres_cat":
binning = dk.utils.linear_bin_hres
def rt(record):
record['user/angle'] = binning(record['user/angle'])
return record
kl = KerasCategorical()
if model_type == 'linear':
kl = KerasLinear()
if model_type == 'categorical':
kl = KerasCategorical()
if model_type == 'hres_cat':
kl = KerasHresCategorical()
print('tub_names', tub_names)
if not tub_names:
tub_names = os.path.join(cfg.DATA_PATH, '*')
tubgroup = TubGroup(tub_names)
train_gen, val_gen = tubgroup.get_train_val_gen(
X_keys,
y_keys,
record_transform=rt,
batch_size=cfg.BATCH_SIZE,
train_frac=cfg.TRAIN_TEST_SPLIT)
if model_type == 'linear':
train_gen, val_gen = tubgroup.get_train_val_gen(
X_keys,
y_keys,
batch_size=cfg.BATCH_SIZE,
train_frac=cfg.TRAIN_TEST_SPLIT)
model_path = os.path.expanduser(model_name)
total_records = len(tubgroup.df)
total_train = int(total_records * cfg.TRAIN_TEST_SPLIT)
total_val = total_records - total_train
print('train: %d, validation: %d' % (total_train, total_val))
steps_per_epoch = total_train // cfg.BATCH_SIZE
print('steps_per_epoch', steps_per_epoch)
print(val_gen)
history, save_best = kl.train(train_gen=train_gen,
val_gen=val_gen,
saved_model_path=model_path,
steps=steps_per_epoch,
train_split=cfg.TRAIN_TEST_SPLIT)
plt.plot(history.history['loss'])
plt.plot(history.history['val_loss'])
plt.title('model loss : %f' % save_best.best)
plt.ylabel('loss')
plt.xlabel('epoch')
plt.legend(['train', 'test'], loc='upper left')
plt.savefig(model_path + '_' + model_type +
'_loss_%f.png' % save_best.best)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
class Timer:
def __init__(self, loops_timed):
self.start_time = time.time()
self.loop_counter = 0
self.loops_timed = loops_timed
def run(self):
self.loop_counter += 1
if self.loop_counter == self.loops_timed:
seconds = time.time() - self.start_time
print("{} loops takes {} seconds".format(
self.loops_timed, seconds))
self.loop_counter = 0
self.start_time = time.time()
timer = Timer(50)
V.add(timer)
#Part to save multiple image arrays from camera
class ImageArrays:
def __init__(self, steps):
tmp = np.zeros((120, 160, 3))
self.steps = steps
self.storage = [tmp for i in range(self.steps * 2 + 1)]
self.active_images = [tmp for i in range(3)]
def run(self, image):
self.storage.pop(0)
self.storage.append(image)
for i in range(3):
self.active_images[i] = self.storage[i * self.steps]
return np.array(self.active_images)
image_arrays = ImageArrays(10)
V.add(image_arrays,
inputs=['cam/image_array'],
outputs=['cam/image_arrays'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_arrays'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single'):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
if model_type is None:
model_type = "categorical"
stereo_cam = camera_type == "stereo"
#Initialize car
V = dk.vehicle.Vehicle()
if stereo_cam:
from donkeycar.parts.camera import Webcam
camA = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, iCam=0)
V.add(camA, outputs=['cam/image_array_a'], threaded=True)
camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, iCam=1)
V.add(camB, outputs=['cam/image_array_b'], threaded=True)
def stereo_pair(image_a, image_b):
if image_a is not None and image_b is not None:
width, height, _ = image_a.shape
grey_a = dk.utils.rgb2gray(image_a)
grey_b = dk.utils.rgb2gray(image_b)
# Added by Felix
depth = capture.stereo_depth(image_a, image_b, leftMapX,
leftMapY, rightMapX, rightMapY,
leftROI, rightROI, imageSize)
stereo_image = np.zeros([width, height, 3],
dtype=np.dtype('B'))
stereo_image[..., 0] = np.reshape(grey_a, (width, height))
stereo_image[..., 1] = np.reshape(grey_b, (width, height))
stereo_image[..., 2] = np.reshape(depth, (width, height))
# --------------
else:
stereo_image = []
return np.array(stereo_image)
image_sterero_pair_part = Lambda(stereo_pair)
V.add(image_sterero_pair_part,
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
else:
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.CAMERA_TYPE == "PICAM":
from donkeycar.parts.camera import PiCamera
cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H)
elif cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
cam = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#this throttle filter will allow one tap back for esc reverse
th_filter = ThrottleFilter()
V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
def led_cond(mode, recording, num_records, behavior_state):
'''
returns a blink rate. 0 for off. -1 for on. positive for rate.
'''
if num_records is not None and num_records % 10 == 0:
print("recorded", num_records, "records")
if behavior_state is not None and model_type == 'behavior':
r, g, b = cfg.BEHAVIOR_LED_COLORS[behavior_state]
led.set_rgb(r, g, b)
return -1 #solid on
if recording:
return -1 #solid on
elif mode == 'user':
return 1
elif mode == 'local_angle':
return 0.5
elif mode == 'local':
return 0.1
return 0
led_cond_part = Lambda(led_cond)
V.add(
led_cond_part,
inputs=['user/mode', 'recording', "tub/num_records", 'behavior/state'],
outputs=['led/blink_rate'])
if cfg.HAVE_RGB_LED:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(led, inputs=['led/blink_rate'])
#IMU
if cfg.HAVE_IMU:
imu = Mpu6050()
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
#Behavioral state
if model_type == "behavior":
bh = BehaviorPart(cfg.BEHAVIOR_LIST)
V.add(bh,
outputs=[
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
])
try:
ctr.set_button_down_trigger('L1', bh.increment_state)
except:
pass
kl = KerasBehavioral(num_outputs=2,
num_behavior_inputs=len(cfg.BEHAVIOR_LIST))
inputs = ['cam/image_array', "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
kl = KerasIMU(num_outputs=2, num_imu_inputs=6)
inputs = [
'cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'
]
else:
if model_type == "linear":
kl = KerasLinear()
elif model_type == "3d":
kl = Keras3D_CNN(seq_length=cfg.SEQUENCE_LENGTH)
elif model_type == "rnn":
kl = KerasRNN_LSTM(seq_length=cfg.SEQUENCE_LENGTH)
else:
kl = KerasCategorical()
inputs = ['cam/image_array']
if model_path:
kl.load(model_path)
V.add(kl,
inputs=inputs,
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
if cfg.TRAIN_BEHAVIORS:
inputs += [
'behavior/state', 'behavior/label', "behavior/one_hot_state_array"
]
types += ['int', 'str', 'vector']
if cfg.HAVE_IMU:
inputs += [
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
]
types += ['float', 'float', 'float', 'float', 'float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if type(ctr) is LocalWebController:
print("You can now go to <your pi ip address>:8887 to drive your car.")
elif type(ctr) is JoystickController:
print("You can now move your joystick to drive your car.")
#tell the controller about the tub
ctr.set_tub(tub)
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)