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, 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,
throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
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 = 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'])
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_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):
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 = KerasLinear()
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_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_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'])
#Part to save multiple image arrays from camera
class ImageArrays:
def __init__(self):
tmp = np.zeros((120, 160, 3))
self.images = [tmp for i in range(3)]
def run(self, image):
self.images.pop(0)
self.images.append(image)
return np.array(self.images)
image_arrays = ImageArrays()
V.add(image_arrays,
inputs=['cam/image_array'],
outputs=['cam/image_arrays'])
# Run the pilot if the mode is not user.
kl = KerasLinear()
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_chaos=False):
#Initialized car
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_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 = 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'])
controller_left = PCA9685(cfg.DD_STEERING_CHANNEL_LEFT)
controller_right = PCA9685(cfg.DD_STEERING_CHANNEL_RIGHT)
diffdrive = L298N(controller_left=controller_left,
controller_right=controller_right)
V.add(diffdrive, inputs=['throttle', 'angle'])
#add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#th = dk.parts.TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
#V.add(tub, inputs=inputs, run_condition='recording')
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=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.")