def __init__(self, dev_conf_path=None, app_conf_path=None):
from donkeypart_tub_loader import TubLoader, TubPrinter
tubitems = [
'cam/image_array', 'user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle', 'angle', 'throttle'
]
self.vehicle = dk.vehicle.Vehicle()
self.vehicle.add(TubLoader('tub'), outputs=tubitems)
self.vehicle.add(PubTelemetry(dev_conf_path=dev_conf_path,
pub_count=0),
inputs=tubitems)
self.vehicle.add(TubPrinter(), inputs=tubitems)
def wait():
delay = 3
#print('wait {} seconds'.format(str(delay)))
sleep(delay)
self.vehicle.add(Lambda(wait))
self.vehicle.add(SubTelemetry(app_conf_path=app_conf_path,
dev_conf_path=dev_conf_path),
outputs=tubitems)
self.vehicle.add(TubPrinter(), inputs=tubitems)
def add_steering_model(vehicle, steering_path, lidar_clip, camera_input,
lidar_input, steering_model_output):
# Process image
image_transformation = ImageTransformation([
image_transformer.normalize,
image_transformer.generate_crop_fn(0, 40, 160, 80),
image_transformer.edges
])
vehicle.add(image_transformation,
inputs=[camera_input],
outputs=['ai/_image'])
# Process lidar
clip_max = lidar_clip
def lidar_preprocess(lidar):
lidar = tf.clip_by_value(lidar,
clip_value_min=0,
clip_value_max=clip_max)
lidar = 1 - (lidar / clip_max)
return lidar
lidar_preprocess = Lambda(lidar_preprocess)
vehicle.add(lidar_preprocess,
inputs=[lidar_input],
outputs=['lidar/processed'])
# Predict on transformed image
steering_model = OneOutputModel()
steering_model.load(steering_path)
vehicle.add(steering_model,
inputs=['ai/_image', 'lidar/processed'],
outputs=[steering_model_output])
def vehicle():
v = dk.Vehicle()
def f():
return 1
l = Lambda(f, outputs=['test_out'])
v.register(l)
return v
def test_vehicle_register_part(vehicle):
def f_mock(input):
pass
vehicle.register(Lambda(f_mock, inputs=["input1"], outputs=["output1"]))
logging.info(str(vehicle.parts))
assert len(vehicle.parts) == 2
assert vehicle.parts[1]['inputs'] == ['input1']
assert vehicle.parts[1]['outputs'] == ['output1']
def vehicle():
v = dk.Vehicle()
def f():
return 1
l = Lambda(f)
v.add(l, outputs=['test_out'])
return v
def test_add_part():
v = dk.Vehicle()
def f():
return 1
l = Lambda(f)
v.add(l, outputs=['test_out'])
assert len(v.parts) == 1
def _configure(self, cfg):
"""
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.
"""
self._configure_camera(cfg)
self.register(ComponentRoadPart2())
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
self.register(LocalWebController())
self._configure_arduino(cfg)
self._configure_angle_part(cfg)
self._configure_throttle_controller(cfg)
# Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == DRIVE_MODE_USER:
return user_angle, user_throttle
elif mode == DRIVE_MODE_LOCAL_ANGLE:
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode,
inputs=[USER_MODE, USER_ANGLE, USER_THROTTLE, PILOT_ANGLE, PILOT_THROTTLE],
outputs=[ANGLE, THROTTLE])
self.register(drive_mode_part)
self._configure_car_hardware(cfg)
self._configure_indicators(cfg)
logger.info("You can now go to <your pi ip address>:8887 to drive your car.")
import donkeycar as dk
from donkeycar.parts.actuator import ServoBlaster, PWMSteering
from donkeycar.parts.transform import Lambda
import math
cfg = dk.load_config()
V = dk.Vehicle()
V.timer = time.time()
def steering_control():
t = time.time() - V.timer
val = math.cos(t)
return val
controls = Lambda(steering_control)
V.add(controls, outputs=["angle"])
steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin
#PWM pulse values should be in the range of 100 to 200
assert (cfg.STEERING_LEFT_PWM <= 200)
assert (cfg.STEERING_RIGHT_PWM <= 200)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
V.add(steering, inputs=['angle'])
V.start(rate_hz=cfg.DRIVE_LOOP_HZ)
def drive(cfg, model_path=None, model_wrapper=None, debug=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()
if not debug:
from donkeycar.parts.camera import PiCamera
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
else:
print("Debug : ignoring camera.")
def fake_cam():
from PIL import Image
import numpy as np
dir_path = os.path.dirname(os.path.realpath(__file__))
img = Image.open(os.path.join(dir_path, 'img.jpg'))
a = np.array(img)
return a
fake_cam_part = Lambda(fake_cam)
V.add(fake_cam_part, outputs=['cam/image_array'])
ctr = LocalWebController(use_chaos=False)
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.
if model_path:
model = load_model(model_path)
model_wrapper_class = getattr(model_wrappers, model_wrapper)
model_instance = model_wrapper_class(model=model)
V.add(model_instance, inputs=['cam/image_array'],
outputs=['pilot/angle1', 'pilot/throttle1'],
run_condition='run_pilot')
def bound_throttle(pilot_angle1, pilot_throttle1):
if pilot_angle1 is None:
pilot_angle1 = 0
if pilot_throttle1 is None:
pilot_throttle1 = 0
pilot_throttle = min(1, pilot_throttle1)
pilot_throttle = max(0, pilot_throttle)
return pilot_angle1, pilot_throttle
bound_throttle_part = Lambda(bound_throttle)
V.add(bound_throttle_part,
inputs=['pilot/angle1', 'pilot/throttle1'],
outputs=['pilot/angle', 'pilot/throttle'])
# add tub to save data
set_path = os.path.join(ROOT_PATH, "set_{}".format(datetime.now().strftime("%Y%m%d%H%M%S")))
os.makedirs(set_path)
inputs = ['cam/image_array', 'user/angle', 'user/throttle'] # 'user/mode'
rec = Recorder(path=set_path)
V.add(rec, inputs=inputs, run_condition='recording')
# 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 not debug:
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'])
else:
print("Debug : ignoring controller part.")
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
def test_lambda_one_arg():
l = Lambda(f)
b = l.run(1)
assert b == 2
def drive_vis(cfg, model_path=None, use_chaos=False):
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)
# Run the pilot if the mode is not user.
kl = KerasLinear()
if model_path:
kl.load(model_path)
top_view_transform = TopViewTransform(cfg.CAMERA_RESOLUTION)
V.add(Lambda(top_view_transform.wrap),
inputs=['cam/image_array'],
outputs=['cam/image_array_proj'])
V.add(kl,
inputs=['cam/image_array_proj'],
outputs=['pilot/angle', 'pilot/throttle'])
ctr = LocalWebControllerVis(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array_proj', 'pilot/angle', 'pilot/throttle'],
outputs=['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
# 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'])
driver = ArduinoDriver()
V.add(driver,
inputs=['user/mode', 'pilot/angle', 'pilot/throttle'],
outputs=['user/angle', 'user/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 _get_sample_lambda():
def f():
return 1
f.update = f
return Lambda(f)
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_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 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 drive(cfg, model_path=None, model_type=None):
V = dk.vehicle.Vehicle()
assert (cfg.CAMERA_TYPE == 'PICAM')
from donkeycar.parts.camera import PiCamera
if (model_type == 'streamline'):
cam = NickCamera(100, 20, True)
else:
cam = NickCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
grayscale=False)
V.add(cam, outputs=['cam/image_array'], threaded=True)
cont_class = MyJoystickController
ctr = cont_class(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
def pilot_condition(mode):
return not (mode == 'user')
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
if model_path:
if (model_type == 'streamline'):
kl = KerasStreamline()
else:
kl = dk.utils.get_model_by_type(model_type, cfg)
assert ('.h5' in model_path)
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
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'])
assert (cfg.DRIVE_TRAIN_TYPE == 'SERVO_ESC')
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
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'])
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,
outputs=['tub/num_records'],
run_condition='recording')
print('you can now move your joystick to drive your car')
ctr.set_tub(tub)
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 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.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs=['timestamp'])
cam = CSICamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=False)
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 = PS4JoystickController(
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)
# 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'])
# Experiment with different models here
## Default Categorical
#kl = Keras_Categorical()
#V.add(kl,
# inputs=['cam/image_array'],
# outputs=['pilot/angle', 'pilot/throttle'],
# run_condition='run_pilot')
## Default Categorical on Simple Model (Faster Inference)
#kl = Keras_Simple_Categorical()
#V.add(kl,
# inputs=['cam/image_array'],
# outputs=['pilot/angle', 'pilot/throttle'],
# run_condition='run_pilot')
## Frame Stacking (GrayScale Frames) on Default Model
# Frame stacking
img_stack = ImgStack()
V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack'])
kl = Keras_StackedFrame_Categorical()
V.add(kl,
inputs=['cam/frame_stack'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# LSTM Model
# Time Sequence Frames
#ts_frames = TimeSequenceFrames()
#V.add(ts_frames, inputs=['cam/image_array'], outputs=['cam/ts_frames'])
#kl = Keras_LSTM_Categorical()
#V.add(kl,
# inputs=['cam/ts_frames'],
# outputs=['pilot/angle', 'pilot/throttle'],
# run_condition='run_pilot')
# DQN Q Network Model (Can be zero shot or finetuned)
# Frame stacking
#img_stack = ImgStack()
#V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack'])
#kl = Keras_Q_Categorical()
#V.add(kl,
# inputs=['cam/frame_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
return pilot_angle, pilot_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):
'''
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()
#add led part
#right_led = rgb_led_0(red_channel = 5, green_channel = 6, blue_channel = 7)
center_led = rgb_led(red_channel = 9, green_channel = 10, blue_channel = 11)
#left_led = rgb_led_0(red_channel = 13, green_channel = 14, blue_channel = 15)
#turn_signals = turn_signal(left_led = left_led, right_led = right_led)
status_led = status_indicator(status_led = center_led)
#V.add(right_led, outputs=['none'])
V.add(center_led, outputs=['none'])
#V.add(left_led, outputs=['none'])
#add pi_perfchecker
loop_time = driveLoopTime()
#loop_time.console=True
#core_temp = coreTemp()
V.add(loop_time,inputs = ['timein'], outputs = ['timein','displaytext'])
#V.add(core_temp)
#throtled_status = throttled()
#V.add(throtled_status, outputs=['displaytext'],threaded=True)
#pitft=display_text()
#V.add(pitft, inputs=['displaytext'], outputs=['pitft/screen'], threaded=True)
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
#boostBright=ImgBoostBright()
#V.add(boostBright, 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=['ncs/image_array', 'classificaiton'],threaded=True)
ncs_ty = tinyyolo(basedir = cfg.MODELS_PATH, draw_on_img = True, probability_threshold = 0.07,debug=False)
V.add(ncs_ty, inputs=['cam/image_array'],outputs=['ncs/image_array','ncs/found_objs'],threaded=True)
loop_time_display = ImgPutText()
#V.add(loop_time_display,inputs=['ncs/image_array','displaytext'], outputs=['ncs/image_array'])
#classify = ImgPutText()
#V.add(classify,inputs=['cam/image_array','classificaiton'],outputs=['ncs/image_array'])
# draw a line showing where training input is cropped
#l1 = ImgDrawLine()
#l1.start = (0,39)
#l1.end = (160,39)
#l1.color = (0,255,0)
#l1.width=10
#V.add(l1, inputs=['ncs/image_array'],outputs=['ncs/image_array'])
#driverInfo = ImgPutInfo()
#throttleText.text='SPD:'
#V.add(driverInfo, inputs=['cam/image_array','throttle', 'angle'],outputs=['cam/image_array'])
#greyScale = ImgGreyscale()
#imgCanny = ImgCanny()
# moving croping to network input layer rather than image..
# Allows me to see and process the entire image but only conside
# the bottom third for stearing input to the network..
#imgCrop = ImgCrop(top=40,bottom=0,left=0,right=0)
#V.add(imgCrop, inputs=['cam/image_array'],outputs=['cam/image_array'])
#V.add(greyScale, inputs=['cam/image_array'],outputs=['cam/image_array'])
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
# auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,throttle_axis='rz',steering_axis='x')
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='y',
steering_axis='x',
panning_axis='z',
tilting_axis='rz')
ctr_webview = LocalWebController()
V.add(ctr_webview,
#inputs=['ncs/image_array'],
inputs=['ncs/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
#ctr.auto_record_on_throttle = False
V.add(ctr,
inputs=['ncs/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording','user/pan','user/tilt'],
threaded=True)
# add the LED controller part
#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 = KerasCroppedCategorical()
# Reverting to non cropped.. adding pan\tilt to camerra
kl = KerasCroppedCategorical()
if model_path:
kl.load(model_path)
kResizeImg= ImgResize()
V.add(kResizeImg, inputs=['cam/image_array'], outputs=['cam/image_array'])
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
#return pilot_angle, 0.30
else:
#overite throttle
#return pilot_angle, pilot_throttle
#return pilot_angle, 0.80
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)
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 = PWMThrottle(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 = PWMThrottle(controller=wagging_controller,
max_pulse=cfg.TAIL_UP_PWM,
zero_pulse=cfg.TAIL_CENTER_PWM,
min_pulse=cfg.TAIL_DOWN_PWM)
#throttleText.text = throttle
# add govenor part here. Governer has overridding power when drive mode is pilot
#break_for_dog = break_for('dog', .3)
#V.add(break_for_dog, inputs=['user/mode','angle', 'throttle','ncs/found_objs'], outputs=['angle','throttle'])
#V.add(turn_signals, inputs=['angle'])
#'user/angle', 'user/throttle', 'user/mode', 'recording'
V.add(status_led, inputs=['user/mode', 'recording'])
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
V.add(panning, inputs=['user/pan'])
V.add(tilting, inputs=['user/tilt'])
#V.add(wagging, inputs=['throttle'])
#add tub to save data
#inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
# need local_angle and local_pilot to save values to tub
inputs=['cam/image_array', 'angle', '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 drive(cfg, model_path=None, 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 = 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 = KerasLinear()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
driver = ArduinoDriver()
V.add(driver,
inputs=['user/mode', 'pilot/angle', 'pilot/throttle'],
outputs=['user/angle', 'user/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,
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 _get_sample_lambda():
def f():
return 1
return Lambda(f)
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.
'''
#Initialisation the CAR
V = dk.vehicle.Vehicle()
# Initialisation of the CAMERA
if cfg.OS == cfg.LINUX_OS:
if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK:
print("Camera {} can't be used.".format(cfg.CAMERA_MACBOOK))
sys.exit()
elif cfg.CAMERA_ID == cfg.CAMERA_MOCK:
cam = MockCamera()
else:
cam = PiCamera(camera=cfg.CAMERA_ID,
resolution=cfg.CAMERA_RESOLUTION)
elif cfg.OS == cfg.MAC_OS:
if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK:
cam = MacWebcam()
else:
cam = VideoStream(camera=cfg.CAMERA_ID,
framerate=cfg.DRIVE_LOOP_HZ)
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, modes, and more.
ctr = LocalWebController()
if (cfg.LANE_DETECTOR):
ctr_inputs = ['cam/image_overlaid_lanes', 'ld/runtime']
#Lane Detector
#Detects the lane the car is in and outputs a best estimate for the 2 lines
lane_detector = LanesDetector(cfg.CAMERA_ID, cfg.LD_PARAMETERS)
V.add(lane_detector,
inputs=['cam/image_array', 'algorithm/reset'],
outputs=['cam/image_overlaid_lanes', 'ld/runtime'])
else:
ctr_inputs = ['cam/image_array']
V.add(
ctr,
ctr_inputs,
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording'
#'algorithm/reset'
],
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()
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.OS == cfg.LINUX_OS:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
elif cfg.OS == cfg.MAC_OS:
steering_controller = MockController()
throttle_controller = MockController()
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
center_pulse=cfg.STEERING_CENTER_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle = PWMThrottle(
controller=throttle_controller,
cal_max_pulse=cfg.THROTTLE_FORWARD_CAL_PWM,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
cal_min_pulse=cfg.THROTTLE_REVERSE_CAL_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_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')
print("You can now go to <your pi ip address>:8887 to drive your car.")
#run the vehicle for 20 seconds
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)
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,
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:
if cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = "categorical"
if model_type == "streamline":
camera_type = "streamline"
#Initialize car
V = dk.vehicle.Vehicle()
if camera_type == "stereo":
if cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
camA = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
camA = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
else:
raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE))
V.add(camA, outputs=['cam/image_array_a'], threaded=True)
V.add(camB, outputs=['cam/image_array_b'], threaded=True)
def stereo_pair(image_a, image_b):
'''
This will take the two images and combine them into a single image
One in red, the other in green, and diff in blue channel.
'''
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)
grey_c = grey_a - grey_b
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(grey_c, (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'])
#elif camera_type == "streamline":
# print("using grayscale pi cam")
# assert(cfg.CAMERA_TYPE == "PICAM")
# from donkeycar.parts.camera import PiCamera
# cam = PiCamera(image_w=cfg.STREAMLINE_IMAGE_W, image_h=cfg.STREAMLINE_IMAGE_H, image_d=1)
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,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
cam = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
cam = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
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
from donkeycar.parts.controller import PS3JoystickController, PS4JoystickController
#cont_class = PS3JoystickController
cont_class = MyJoystickController
if cfg.CONTROLLER_TYPE == "ps4":
cont_class = PS4JoystickController
ctr = cont_class(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
if cfg.USE_NETWORKED_JS:
from donkeycar.parts.controller import JoyStickSub
netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
V.add(netwkJs, threaded=True)
ctr.js = netwkJs
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, recording_alert, behavior_state,
reloaded_model):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if reloaded_model:
led.set_rgb(cfg.MODEL_RELOADED_LED_R, cfg.MODEL_RELOADED_LED_G,
cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
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
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)
led_cond_part = Lambda(led_cond)
V.add(led_cond_part,
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'reloaded/model'
],
outputs=['led/blink_rate'])
V.add(led, inputs=['led/blink_rate'])
def get_record_alert_color(num_records):
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
def record_tracker(num_records):
if num_records is None:
return 0
if record_tracker.last_num_rec_print != num_records or record_tracker.force_alert:
record_tracker.last_num_rec_print = num_records
if num_records % 10 == 0:
print("recorded", num_records, "records")
if num_records % cfg.REC_COUNT_ALERT == 0 or record_tracker.force_alert:
record_tracker.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
record_tracker.force_alert = 0
if record_tracker.dur_alert > 0:
record_tracker.dur_alert -= 1
if record_tracker.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
record_tracker.last_num_rec_print = 0
record_tracker.dur_alert = 0
record_tracker.force_alert = 0
rec_tracker_part = Lambda(record_tracker)
V.add(rec_tracker_part,
inputs=["tub/num_records"],
outputs=['records/alert'])
if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
#then we are not using the circle button. hijack that to force a record count indication
def show_record_acount_status():
record_tracker.last_num_rec_print = 0
record_tracker.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#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 cfg.TRAIN_BEHAVIORS:
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
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.
inputs = [
'cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = ['cam/image_array']
def load_model(kl, model_path):
start = time.time()
try:
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except:
print('problems loading model', model_path)
def load_weights(kl, weights_path):
start = time.time()
try:
print('loading model weights', weights_path)
kl.model.load_weights(weights_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except:
print('problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
import keras
with open(json_fnm, 'r') as handle:
contents = handle.read()
kl.model = keras.models.model_from_json(contents)
print('finished loading json in %s sec.' % (str(time.time() - start)))
if model_path:
#When we have a model, first create an appropriate Keras part
#if model_type == "streamline":
#kl = KerasStreamline()
#else:
kl = dk.utils.get_model_by_type(model_type, cfg)
if '.h5' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
print(filename, "was changed!")
load_model(kl, filename)
fw_part = FileWatcher(model_path,
reload_model,
wait_for_write_stop=10.0)
V.add(fw_part, outputs=['reloaded/model'])
elif '.json' in model_path:
#when we have a .json extension
#load the model from their and look for a matching
#.wts file with just weights
load_model_json(kl, model_path)
weights_path = model_path.replace('.json', '.weights')
load_weights(kl, weights_path)
def reload_weights(filename):
print(filename, "was changed!")
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
fw_part = FileWatcher(model_path,
reload_weights,
wait_for_write_stop=1.0)
V.add(fw_part, outputs=['reloaded/model'])
else:
#previous default behavior
load_model(kl, 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'])
#Drive train setup
if cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC":
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
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'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE":
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT,
cfg.HBRIDGE_PIN_RIGHT)
throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM
left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD,
cfg.HBRIDGE_PIN_LEFT_BWD)
right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD,
cfg.HBRIDGE_PIN_RIGHT_BWD)
two_wheel_control = TwoWheelSteeringThrottle()
V.add(two_wheel_control,
inputs=['throttle', 'angle'],
outputs=['left_motor_speed', 'right_motor_speed'])
V.add(left_motor, inputs=['left_motor_speed'])
V.add(right_motor, inputs=['right_motor_speed'])
elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM":
from donkeycar.parts.actuator import ServoBlaster, PWMSteering
steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin
#PWM pulse values should be in the range of 100 to 200
assert (cfg.STEERING_LEFT_PWM <= 200)
assert (cfg.STEERING_RIGHT_PWM <= 200)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(motor, 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 isinstance(ctr, 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)
def test_lambda_two_args():
l = Lambda(f2)
b = l.run(1, 1)
assert b == 3
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 drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
（手動・自動）運転する。
多くの部品(part)から作業用のロボット車両を構築する。
各partはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded`に応じて
`run` メソッドまたは `run_threaded` メソッドを呼び出す。
すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレートで順次更新され、
各partが適時に処理を終了すると仮定する。
partには名前付きの出力と入力が存在する(どちらかがない場合や複数存在する場合もある)。
Vehicle フレームワークは、名前付き出力を同じ名前の入力を要求するpartに渡すことを処理する。
引数
cfg 個別車両設定オブジェクト、`config.py`がロードされたオブジェクト。
model_path 自動運転時のモデルファイルパスを指定する（デフォルトはNone）。
use_joystick ジョイスティックを使用するかどうかの真偽値（デフォルトはFalse）。
use_chaos 手動運転中に周期的なランダム操舵を加えるかどうかの真偽値（デフォルトはFalse）。
"""
# Vehicle オブジェクトの生成
V = dk.vehicle.Vehicle()
# Timestamp part の生成
clock = Timestamp()
# Timestamp part をVehicleループへ追加
# 入力：
# なし
# 出力：
# 'timestamp' 現在時刻
V.add(clock, outputs=['timestamp'])
# PiCamera part の生成
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
# 別スレッド実行される PiCamera part をVehicleループへ追加
# 入力：
# なし
# 出力：
# 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ
V.add(cam, outputs=['cam/image_array'], threaded=True)
# manage.py デフォルトのジョイスティックpart生成
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
# steering_scale=cfg.JOYSTICK_STEERING_SCALE,
# throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
# auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
# ジョイスティック part の生成
from elecom.part import JoystickController
ctr = JoystickController(config_path='elecom/jc-u3912t.yml')
else:
# ステアリング、スロットル、モードなどを管理するWebサーバを作成する
# Web Controller part の生成
ctr = LocalWebController(use_chaos=use_chaos)
# 別スレッド実行される Web Controller part もしくはジョイスティック part をVehiecleループへ追加
# 入力：
# 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ
# 出力：
# 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値
# 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値
# 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま)
# 'recording' tubデータとして保管するかどうかの真偽値
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# オートパイロットモジュールを実行すべきかどうかを確認する関数を定義する。
def pilot_condition(mode):
'''
オートパイロットモジュール実行判定関数。
引数で指定されたモードを判別して、オートパイロットモジュールを実行するべきかどうか
真偽値を返却する関数。
引数
mode Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動)
戻り値
boolean オートパイロットモジュールを実行するかどうかの真偽値
'''
print('mode=' + mode)
if mode == 'user':
# 全手動時のみ実行しない
return False
else:
return True
# オートパイロットモジュール実行判定関数を part 化したオブジェクトを生成
pilot_condition_part = Lambda(pilot_condition)
# オートパイロットモジュール実行判定 part を Vehiecle ループへ追加
# 入力：
# 'user/mode' Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動)
# 出力：
# 'run_pilot' オートパイロットモジュールを実行するかどうかの真偽値
V.add(pilot_condition_part,
inputs=['user/mode'],
outputs=['run_pilot'])
# Userモードでない場合、オートパイロットを実行する
# CNNベースの線形回帰モデル(オートパイロット) part を生成する。
kl = KerasLinear()
# 関数driveの引数 model_part 指定がある場合
if model_path:
# 学習済みモデルファイルを読み込む
kl.load(model_path)
# run_condition が真の場合のみ実行されるオートパイロット part をVehicleループへ追加する
# 入力：
# 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ
# 出力：
# 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値
# 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# 車両にどの値を入力にするかを判別する
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
'''
引数で指定された項目から、車両への入力とするステアリング値、スロットル値を確定する関数。
引数
mode Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま)
user_angle Web/Joystickにより手動指定した次に取るべきステアリング値
user_throttle Web/Joystickにより手動指定した次に取るべきスロットル値
pilot_angle オートパイロットが指定した次に取るべきステアリング値
pilot_throttle オートパイロットが指定した次に取るべきスロットル値
戻り値
angle 車両への入力とするステアリング値
throttle 車両への入力とするスロットル値
'''
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
# 車両にどの値を入力にするかを判別する関数を part 化したオブジェクトを生成
drive_mode_part = Lambda(drive_mode)
# 車両にどの値を入力にするかを判別する part を Vehicle ループへ追加
# 入力
# 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま)
# 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値
# 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値
# 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値
# 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値
# 戻り値
# 'angle' 車両への入力とするステアリング値
# 'throttle' 車両への入力とするスロットル値
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)
# 実車両へステアリング値を指示する part を生成
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
# 実車両のスロットルECSを操作するオブジェクトを生成
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
# 実車両へスロットル値を指示する part を生成
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
# 実車両へステアリング値を指示する part を Vehiecle ループへ追加
# 入力：
# 'angle' 車両への入力とするステアリング値
# 出力：
# なし(実車両の操舵へ)
V.add(steering, inputs=['angle'])
# 実車両へスロットル値を指示する part を Vehiecle ループへ追加
# 入力：
# 'throttle' 車両への入力とするスロットル値
# 出力：
# なし(実車両のスロットル操作へ)
V.add(throttle, inputs=['throttle'])
# 保存データを tub ディレクトリに追加
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp']
types = ['image_array', 'float', 'float', 'str', 'str']
# 複数 tub ディレクトリの場合
# th = TubHandler(path=cfg.DATA_PATH)
# tub = th.new_tub_writer(inputs=inputs, types=types)
# 単一 tub ディレクトリの場合
# tub ディレクトリへ書き込む part を生成
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
# 'recording'が正であれば tub ディレクトリへ書き込む part を Vehiecle ループへ追加
# 入力
# 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ
# 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値
# 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値
# 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま)
# 'timestamp' 現在時刻
V.add(tub, inputs=inputs, run_condition='recording')
# テレメトリーデータの送信
#tele = PubTelemetry('iotf/emperor.ini', pub_count=20*5)
#V.add(tele, inputs=['cam/image_array', 'user/mode', 'user/angle', 'user/throttle',
# 'pilot/angle', 'pilot/throttle', 'angle', 'throttle'])
# 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)
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, 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)
