def test_tubreader():
with tempfile.TemporaryDirectory() as tempfolder:
path = os.path.join(tempfolder, 'new')
inputs = ['name', 'age', 'pic']
types = ['str', 'float', 'str']
writer = TubWriter(path, inputs=inputs, types=types)
writer.run('will', 323, 'asdfasdf')
assert writer.get_num_records() == 1
reader = TubReader(path)
assert reader.get_num_records() == 1
record = reader.run('name', 'age', 'pic')
assert set(record) == set(['will', 323, 'asdfasdf'])
def drive(cfg):
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)
# 略
V.mem['recording'] = True
# JoyStickやWebControllerを改変し、以下の値をVehicleフレームワークの
# メモリ上に格納する
# .../value -1.0～1.0までの入力値→DCモータに加わる電圧になる
# .../status 'move':前後動 'free':動力なし 'brake':制動停止
# 以下両モータ半速前進する入力値をダミーとして投入
V.mem['user/left/value'] = 0.5
V.mem['user/left/status'] = 'move' # 'free' 'brake'
V.mem['user/right/value'] = 0.5
V.mem['user/right/status'] = 'move' # 'free' 'brake'
# 対象Raspberry Pi上のpigpiodと通信するオブジェクト
# 複数モータを使用する場合は、同じオブジェクトをそれぞれコンストラクタへ渡す
# 但し、GPIOピンは重複させることはできない
pi = pigpio.pi()
# DCモータ1基ごとに1つのDCMotorインスタンスとして管理
from donkeypart_dc130ra import DCMotor
left_motor = DCMotor(pi, cfg.LEFT_MOTOR_IN1_GPIO, cfg.LEFT_MOTOR_IN2_GPIO)
right_motor = DCMotor(pi, cfg.RIGHT_MOTOR_IN1_GPIO,
cfg.RIGHT_MOTOR_IN2_GPIO)
# Vehicleフレームワークへ登録
V.add(left_motor, inputs=['user/left/value', 'user/left/status'])
V.add(right_motor, inputs=['user/right/value', 'user/right/status'])
# Tubデータ・フォーマットも変更しなくてはならない
inputs = [
'cam/image_array', 'user/left/value', 'user/left/status',
'user/right/status', 'user/right/status', 'timestamp'
]
types = ['image_array', 'float', 'str', 'float', 'str', 'str']
# 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 test_tub_create(self):
tub = TubWriter(self.path, inputs=self.inputs, types=self.types)
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_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
def get_timestamp():
return time.time()
clock = Lambda(get_timestamp)
V.add(clock, outputs=['timestamp'])
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
curr_angle = 0.0
def smooth_angle(in_angle):
nonlocal curr_angle
delta = in_angle - curr_angle
if abs(delta) < 0.02:
curr_angle = in_angle
else:
curr_angle = curr_angle + delta * 0.15
print('smoothed angle:', curr_angle)
return curr_angle
angleSmoother = Lambda(smooth_angle)
V.add(angleSmoother, inputs=['user/angle'], outputs=['user/angle'])
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'],
outputs=['run_pilot'])
# Add velocity detector
vd = Lambda(make_velocity_detector())
V.add(vd, inputs=['cam/image_array'], outputs=['user/velocity'])
""""
def print_velocity(v):
print('velocity:', v)
pv = Lambda(print_velocity)
V.add(pv, inputs=['user/velocity'])
"""
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/velocity'],
run_condition='run_pilot')
def calc_throttle(inferred_v, actual_v):
throttle = 0.38
"""
if inferred_v > actual_v:
throttle = min((inferred_v - actual_v) * 1.0, 0.5)
print('V: inferred={} actual={} => throttle={}'.format(inferred_v, actual_v, throttle));
"""
if (inferred_v > 5):
throttle = 0.48
elif (inferred_v > 4):
throttle = 0.44
elif (inferred_v > 3):
throttle = 0.42
elif (inferred_v > 2):
throttle = 0.41
elif (inferred_v > 1):
throttle = 0.39
return throttle
ct = Lambda(calc_throttle)
V.add(ct, inputs=['pilot/velocity', 'user/velocity'],
outputs=['pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
# manual steer, manual throttle
return user_angle, user_throttle
elif mode == 'local_angle':
# auto steer, manual throttle
return pilot_angle, user_throttle
else:
# auto steer, auto throttle
print('PILOT: angle={} throttle={}'.format(pilot_angle, pilot_throttle))
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/velocity', 'user/throttle', 'user/mode', 'timestamp']
types = ['image_array', 'float', 'float', 'float', 'str', 'float']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
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, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
center_led = rgb_led(red_channel=9, green_channel=10, blue_channel=11)
status_led = status_indicator(status_led=center_led)
V.add(center_led, outputs=['none'])
V.add(status_led, inputs=['user/mode', 'recording'])
#add pi_perfchecker
#loop_time = driveLoopTime()
#loop_time.console=True
#core_temp = coreTemp()
#V.add(core_temp)
#V.add(loop_time,inputs = ['timein'], outputs = ['timein','displaytext'])
#throtled_status = throttled()
#V.add(throtled_status, outputs=['displaytext'],threaded=True)
loop_time = driveLoopTime()
V.add(loop_time, inputs=['timein'], outputs=['timein', 'displaytext'])
loop_time_display = ImgPutText(org=(100, 20))
clock = Timestamp()
V.add(clock, outputs=['timestamp'])
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
#cam = PiCamera(resolution=(960,1280))
V.add(cam, outputs=['cam/image_array'], threaded=True)
#ncs_ty = tinyyolo(basedir = cfg.MODELS_PATH, draw_on_img = True, probability_threshold = 0.15,debug=False)
#V.add(ncs_ty, inputs=['cam/image_array'],outputs=['cam/image_array','ncs/found_objs'],threaded=True)
V.add(loop_time_display,
inputs=['cam/image_array', 'displaytext'],
outputs=['cam/image_array'])
#img_resize = ImgResize()
#V.add(img_resize,inputs=['cam/image_array'],outputs=['cam/image_array'])
#ncs_gn = googlenet(basedir=cfg.MODELS_PATH, debug=True)
#V.add(ncs_gn, inputs=['cam/image_array'],outputs=['ncs/image_array', 'classificaiton'],threaded=True)
#ncs_inception = inception(basedir=cfg.MODELS_PATH, probability_threshold=0.01, debug=True)
#V.add(ncs_inception, inputs=['cam/image_array'],outputs=['cam/image_array', 'classificaiton'],threaded=True)
#face_detect = detect(basedir=cfg.MODELS_PATH, debug=True)
# V.add(face_detect, inputs=['cam/image_array'],outputs=['face/image_array', 'face_center'],threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis='rz', #throttle_axis='y',
steering_axis='x',
panning_axis='rx',
tilting_axis='ry')
"""
when running from JS, let us display the nsc images on the web page
"""
"""
ctr_webview = LocalWebController()
V.add(ctr_webview,
inputs=['cam/image_array'],
#inputs=['cam/image_array'],
outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'],
#outputs=['outargs'],
threaded=True)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
"""
V.add(
ctr,
inputs=['cam/image_array'],
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording',
'user/pan', 'user/tilt'
],
#outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
ctr_webview = LocalWebController()
V.add(
ctr_webview,
inputs=['cam/image_array'],
#inputs=['cam/image_array'],
outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'],
#outputs=['outargs'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
#kl = KerasLinear()
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle, user_pan, user_tilt, face_pan, face_tilt):
if mode == 'user':
return user_angle, user_throttle, user_pan, user_tilt
elif mode == 'face':
return user_angle, user_throttle, face_pan, face_tilt
elif mode == 'local_angle':
return pilot_angle, user_throttle, face_pan, face_tilt
else:
return pilot_angle, pilot_throttle, face_pan, face_tilt
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle', 'user/pan', 'user/tilt', 'face/pan',
'face/tilt'
],
outputs=['angle', 'throttle', 'pan', 'tilt'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
panning_controller = PCA9685(cfg.PAN_CHANNEL)
panning = PWMPanning(controller=panning_controller,
left_pulse=cfg.PAN_LEFT_PWM,
zero_pulse=cfg.PAN_CENTER_PWM,
right_pulse=cfg.PAN_RIGHT_PWM)
tilting_controller = PCA9685(cfg.TILT_CHANNEL)
tilting = PWMTilting(controller=tilting_controller,
max_pulse=cfg.TILT_UP_PWM,
zero_pulse=cfg.TILT_DRIVING_PWM,
min_pulse=cfg.TILT_DOWN_PWM)
wagging_controller = PCA9685(cfg.TAIL_CHANNEL)
wagging = PWMWagging(controller=wagging_controller,
max_pulse=cfg.TAIL_UP_PWM,
zero_pulse=cfg.TAIL_CENTER_PWM,
min_pulse=cfg.TAIL_DOWN_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#looping pan and tilt out to donkeyface, then back into this donkeypet
donkeyPet_sender = send_to_donkey(send_to_address='10.11.44.20')
V.add(donkeyPet_sender,
inputs=['pan', 'tilt', 'user/mode', 'recording', 'timestamp'],
outputs=['donkeyface_output'])
donkeyPet_receiver = rcv_from_donkey(listen_to_port=10500, debug=False)
V.add(donkeyPet_receiver,
inputs=['rcv_in'],
outputs=['face/pan', 'face/tilt', 'face/irq'])
feels_on_target = target_status(debug=True)
V.add(feels_on_target,
inputs=['face/irq', 'tail', 'user/mode'],
outputs=['tail'])
V.add(panning, inputs=['face/pan'])
V.add(tilting, inputs=['face/tilt'])
V.add(wagging, inputs=["tail"])
# add tub to save data
#pan and tilt setup
tub_inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
tub_types = ['image_array', 'float', 'float', 'str', 'str']
#non pan and tilt test
#tub_inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp','test_var']
#tub_types = ['image_array', 'float', 'float', 'str', 'str','str']
# multiple tubs
# th = TubHandler(path=cfg.DATA_PATH)
# tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=tub_inputs, types=tub_types)
V.add(tub, inputs=tub_inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def 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_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 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 = PS4JoystickController(steering_scale=cfg.JOYSTICK_STEERING_SCALE,
# auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr = SerialDevice()
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)
## Create the controller part
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'])
## MUST: choose one of the below controllers
actuator_type = cfg.ACTUATOR_MODE # normal, seesaw, serial
if actuator_type == 'serial':
pass
#steering_controller = SerialDevice(cfg.STEERING_CHANNEL)
#throttle_controller = SerialDevice(cfg.THROTTLE_CHANNEL)
elif actuator_type == 'seesaw':
steering_controller = RoboHATMM1(cfg.STEERING_CHANNEL)
throttle_controller = RoboHATMM1(cfg.THROTTLE_CHANNEL)
else:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
## This Creates the magic PWM parts for the Controllers above.
if actuator_type is not 'serial':
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
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:
V.add(SerialDevice(), inputs['angle', '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'])
if cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
cam = Webcam()
else:
from donkeycar.parts.camera import PiCamera
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)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording', 'pan', 'tilt'],
threaded=True)
if cfg.USE_PAN:
pan_controller = PCA9685(cfg.PAN_CHANNEL)
pan = PWMPanTilt(controller=pan_controller,
left_pulse=cfg.PAN_LEFT_PWM,
right_pulse=cfg.PAN_RIGHT_PWM,
dir=cfg.PAN_DIR)
V.add(pan, inputs=['pan'])
if cfg.USE_TILT:
tilt_controller = PCA9685(cfg.TILT_CHANNEL)
tilt = PWMPanTilt(controller=tilt_controller,
left_pulse=cfg.TILT_DOWN_PWM,
right_pulse=cfg.TILT_UP_PWM,
dir=cfg.TILT_DIR)
V.add(tilt, inputs=['tilt'])
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:
print(user_throttle)
user_throttle = user_throttle * cfg.THROTTLE_PILOT_WEIGHT
if(user_throttle < cfg.THROTTLE_PILOT_LIMIT):
user_throttle = cfg.THROTTLE_PILOT_LIMIT
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,
dir=cfg.STEERING_DIR)
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,
dir=cfg.THROTTLE_DIR)
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']
if cfg.USE_SINGLE_TUB == False:
#multiple tubs
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
else:
# 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):
"""
たくさんのパーツから作業用のロボットVehicleを構築します。
各パーツはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded`
に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出します。
すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレート
(デフォルト：20MHz)で順次更新され、各パーツが適時に処理を終了すると仮定して
ループを無限に繰り返します。
パーツにはラベル付きの `inputs` と `outputs` があります。
フレームワークは、ラベル付き `outputs` の値を、
同じ名前の `inputs` を要求する別のパーツに渡します。
引数：
cfg config.py を読み込んだオブジェクト
model_path 学習済みモデルファイルのパス
use_joystick ジョイスティックを使用するかどうかの真偽値
use_chaos 操舵に一定のランダム操作を加えるかどうかの真偽値
"""
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:
# このWebコントローラでは、ステアリング、スロットル、モードなどを管理する
# Webサーバを作成
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# パイロットモジュールを走らせるべきかどうかを毎回判別させるためのパーツ
# この関数の結果の真偽値で自動運転パーツ(ConvLSTM2DPilot)を実行するかを決定させる
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()
kl = ConvLSTM2DPilot()
if model_path:
print("start loading trained model file")
kl.load(model_path)
print("finish loading trained model file")
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'])
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 test_tub_path(self):
tub = TubWriter(self.path, inputs=self.inputs, types=self.types)
print(tub.types, tub.inputs)
tub.run('will', 323, 'asdfasdf')
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_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_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_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.")
