def test_udp_broadcast(ip):
if ip is None:
print("udp broadcast test..")
p = UDPValuePub('camera')
from donkeycar.parts.camera import PiCamera
from donkeycar.parts.image import ImgArrToJpg
cam = PiCamera(160, 120, 3, framerate=4)
img_conv = ImgArrToJpg()
time.sleep(1)
while True:
cam_img = cam.run()
jpg = img_conv.run(cam_img)
print("sending", len(jpg), "bytes")
p.run(jpg)
time.sleep(0.5)
else:
print("udp listen test..", ip)
s = UDPValueSub('camera')
while True:
res = s.run()
time.sleep(0.1)
def test_tcp_client_server(ip):
if ip is None:
p = TCPServeValue("camera")
from donkeycar.parts.camera import PiCamera
from donkeycar.parts.image import ImgArrToJpg
cam = PiCamera(160, 120, 3, framerate=4)
img_conv = ImgArrToJpg()
while True:
cam_img = cam.run()
jpg = img_conv.run(cam_img)
p.run(jpg)
time.sleep(0.1)
else:
c = TCPClientValue("camera", ip)
while True:
c.run()
time.sleep(0.01)
def test_mqtt_pub_sub(ip):
if ip is None:
print("publishing test..")
p = MQTTValuePub('donkey/camera')
from donkeycar.parts.camera import PiCamera
from donkeycar.parts.image import ImgArrToJpg
cam = PiCamera(160, 120, 3, framerate=4)
img_conv = ImgArrToJpg()
while True:
cam_img = cam.run()
jpg = img_conv.run(cam_img)
p.run(jpg)
time.sleep(0.1)
else:
print("subscribing test..")
s = MQTTValueSub('donkey/camera')
while True:
res = s.run()
print("got:", res)
time.sleep(0.1)
def train_drive_reinforcement(cfg, args, script_mode):
'''
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.
'''
model_path = args['--model']
use_joystick = args['--js']
meta = args['--meta']
tub_in = args['--tub']
model_type = args['--type']
camera_type = args['--camera']
vae_path = args['--vae']
auto_mode = 0
if args['--auto']:
auto_mode = 1
env_type = 'simulator'
if args['--env']:
env_type = args['--env']
print('VAE_PATH: %s' % vae_path)
global r
global ctr
global cam
if cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif cfg.CAMERA_TYPE == "D435":
from donkeycar.parts.realsense435i import RealSense435i
cam = RealSense435i(enable_rgb=cfg.REALSENSE_D435_RGB,
enable_depth=cfg.REALSENSE_D435_DEPTH,
enable_imu=cfg.REALSENSE_D435_IMU,
device_id=cfg.REALSENSE_D435_ID)
V.add(cam,
inputs=[],
outputs=[
'cam/image_array', 'cam/depth_array', 'imu/acl_x',
'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
],
threaded=True)
else:
if False and cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
host=cfg.SIM_HOST,
env_name=cfg.DONKEY_GYM_ENV_NAME,
conf=cfg.GYM_CONF,
delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle']
elif 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,
framerate=cfg.CAMERA_FRAMERATE,
vflip=cfg.CAMERA_VFLIP,
hflip=cfg.CAMERA_HFLIP)
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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "IMAGE_LIST":
from donkeycar.parts.camera import ImageListCamera
cam = ImageListCamera(path_mask=cfg.PATH_MASK)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
V.add(
ctr,
inputs=['cam/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(port=cfg.WEB_CONTROL_PORT,
mode=cfg.WEB_INIT_MODE)
V.add(
ctr,
inputs=['cam/image_array', 'tub/num_records'],
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import IMU
imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG)
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
# Use the FPV preview, which will show the cropped image output, or the full frame.
if False and cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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 = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs = [
inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = [inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
#Choose what inputs should change the car.
class DriveMode:
def run(self, 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 * cfg.AI_THROTTLE_MULT
return user_angle, user_throttle
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK":
pass
elif 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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
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'], threaded=True)
V.add(motor, inputs=["throttle"])
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM":
from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM
steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN,
freq=cfg.STEERING_PWM_FREQ,
inverted=cfg.STEERING_PWM_INVERTED)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN,
freq=cfg.THROTTLE_PWM_FREQ,
inverted=cfg.THROTTLE_PWM_INVERTED)
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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
# OLED setup
if cfg.USE_SSD1306_128_32:
from donkeycar.parts.oled import OLEDPart
auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE
oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM,
auto_record_on_throttle=auto_record_on_throttle)
V.add(oled_part,
inputs=['recording', 'tub/num_records', 'user/mode'],
outputs=[],
threaded=True)
#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.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH:
inputs += ['cam/depth_array']
types += ['gray16_array']
if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." %
cfg.WEB_CONTROL_PORT)
else:
print(
"You can now go to <your hostname.local>:%d to drive your car."
% cfg.WEB_CONTROL_PORT)
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
if script_mode == 'train':
_thread.start_new_thread(
observe_and_learn,
(cfg, model_path, vae_path, auto_mode, env_type))
_thread.start_new_thread(save_reconstruction, (cfg, ))
elif script_mode == 'drive':
_thread.start_new_thread(drive_model,
(cfg, model_path, vae_path, env_type))
elif script_mode == 'optimize':
_thread.start_new_thread(
optimize_model, (cfg, model_path, vae_path, auto_mode, env_type))
elif script_mode == 'train_vae':
print('collecting data for vae training...')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
V.add(cam, inputs=inputs, outputs=['camera/arr'], threaded=threaded)
#V.add(cam, outputs=["camera/arr"], threaded=True)
img_to_jpg = ImgArrToJpg()
V.add(img_to_jpg, inputs=["camera/arr"], outputs=["camera/jpg"])
pub_cam = MQTTValuePub("donkey/%s/camera" % cfg.DONKEY_UNIQUE_NAME,
broker=cfg.MQTT_BROKER)
V.add(pub_cam, inputs=["camera/jpg"])
sub_controls = MQTTValueSub("donkey/%s/controls" % cfg.DONKEY_UNIQUE_NAME,
def_value=(0., 0.),
broker=cfg.MQTT_BROKER)
V.add(sub_controls, outputs=["angle", "throttle"])
# steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM)
# steering = PWMSteering(controller=steering_controller,
# left_pulse=cfg.STEERING_LEFT_PWM,
# right_pulse=cfg.STEERING_RIGHT_PWM)
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', meta=[]):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"]="-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
#实例化小车
V = dk.vehicle.Vehicle()
range = Sensor(pi, cfg.RANGE_GPIOS)
# range.update_loop_body()
# V.add(range, outputs=['range/cms'], threaded=True)
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif cfg.CAMERA_TYPE == "D435":
from donkeycar.parts.realsense435i import RealSense435i
cam = RealSense435i(
enable_rgb=cfg.REALSENSE_D435_RGB,
enable_depth=cfg.REALSENSE_D435_DEPTH,
enable_imu=cfg.REALSENSE_D435_IMU,
device_id=cfg.REALSENSE_D435_ID)
V.add(cam, inputs=[],
outputs=['cam/image_array', 'cam/depth_array',
'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'],
threaded=True)
else:
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle']
elif cfg.CAMERA_TYPE == "PICAM":
from donkeycar.parts.camera import PiCamera
print("NBNBNBNBNBNBNNBNBNBNBNBNB",666666)
cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, vflip=cfg.CAMERA_VFLIP, hflip=cfg.CAMERA_HFLIP)
# pos = cam.update()
# print(pos)
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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "IMAGE_LIST":
from donkeycar.parts.camera import ImageListCamera
cam = ImageListCamera(path_mask=cfg.PATH_MASK)
else:
raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
V.add(ctr,
inputs=['cam/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(port=cfg.WEB_CONTROL_PORT, mode=cfg.WEB_INIT_MODE)
V.add(ctr,
inputs=['cam/image_array', 'tub/num_records'],
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg), inputs=['user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc"],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import IMU
imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG)
V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
# Use the FPV preview, which will show the cropped image output, or the full frame.
if cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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 = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert(cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs=[inf_input,
'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z']
else:
inputs=[inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)) )
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)) )
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running")
V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running")
outputs=['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs,
outputs=outputs,
run_condition='run_pilot')
if cfg.STOP_SIGN_DETECTOR:
from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector
V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE, cfg.STOP_SIGN_SHOW_BOUNDING_BOX), inputs=['cam/image_array', 'pilot/throttle'], outputs=['pilot/throttle', 'cam/image_array'])
#Choose what inputs should change the car.
imgW = 160
imgH = 120
def JudgeLR(p):
#设置缓冲距离
SILENT_LR_DISTANCE = imgW/12
#img中心点
center = [imgW/2, imgH/2]
#如果目标位于图片中心左侧，小车左转
p1=(p[0]+p[2])/2
p2=center[0]-SILENT_LR_DISTANCE
p3=(p[0]+p[2])/2
p4=center[0]+SILENT_LR_DISTANCE
if p1<p2:
print(-0.5)
return -1
#如果目标位于图片中心右侧，小车右转
elif p3>p4:
print(0.5)
return 1
def JudgeFB(p):
#设置缓冲距离
SILENT_FB_DISTANCE = imgH/12
#img中心点
center = [imgW/2, imgH/2]
#如果目标上边框>imgH的1/3，小车前进
p5=p[3]/imgH
if p5>1/3:
return 0.75
else:
return 0
class DriveMode:
def run(self, mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
global force_stop
global music_ing
#获取超声传感器测距信息distance
distance = range.distance_get()
#获取目标检测结果
with open("/home/pi/projects/donkeycar/parts/pos_data.txt", "rb") as f:
try:
outputxmin = pickle.load(f)
outputymin = pickle.load(f)
outputxmax = pickle.load(f)
outputymax = pickle.load(f)
# music_controller = pickle.load(f)
music_on = pickle.load(f)
print(music_on)
except EOFError:
return None
p=[outputxmin,outputymin,outputxmax,outputymax]
print("music_ing",music_ing)
print("nusic_on",music_on)
# if music_controller == 1:
# os.system('mpg123 music.mp3')
# elif music_controller == 0:
# os.system('q')
imgW = 160
imgH = 120
#设置缓冲距离
SILENT_LR_DISTANCE = imgW/12
#img中心点
center = [imgW/2, imgH/2]
p1=(p[0]+p[2])/2
p2=center[0]-SILENT_LR_DISTANCE
p3=(p[0]+p[2])/2
p4=center[0]+SILENT_LR_DISTANCE
d1=imgH-p[3]
d2=p1-center[0]
angle = -0.3
if p[0]<0:#若未检测到行人
user_throttle = 0.63
user_angle = angle
print('未检测到行人!!')
print("角度",angle)
if distance < 40:
print('distance:',distance)
user_throttle = 0.0
user_angle = -0.3
else:#若检测到行人
if distance >= 40:
#根据超声测距控制油门
throttle = (0.15/110)*distance+0.58
if throttle<0.7:
user_throttle = throttle
else:
user_throttle = 0.7
#根据图像控制油门
# user_throttle = 0.65
# user_angle = -0.3
# #如果目标位于图片中心左侧，小车左转
# if p1<p2:
# print("左转",-0.5)
# user_angle = -0.5
# #如果目标位于图片中心右侧，小车右转
# elif p3>p4:
# print("右转",0.2)
# user_angle = 0.2
# # elif d1>imgH/3:
# # print('加速')
# # user_throttle = 0.65
# # elif d1<imgH/3 and d1>imgH/4:
# # print('减速')
# # user_throttle = 0.65
user_angle = (0.7/70)*d2-0.3
if user_angle<-0.3
angle = -0.3+(abs(user_angle+0.3))/3
elif user_angle>-0.3
angle = -0.3-(abs(user_angle+0.3))/3
elif distance < 40:
print('distance:',distance)
user_throttle = 0.0
user_angle = -0.3
angle = user_angle
if music_on == 1 and music_ing==0:
pygame.mixer.music.load("music1.mp3")
pygame.mixer.music.play(-1)
music_ing = 1
user_throttle = 0
elif music_on == 1 and music_ing == 1:
user_throttle = 0
elif music_on == 0 and music_ing == 1:
pygame.mixer.music.stop()
music_ing = 0
if mode == 'user':
print('油门',user_throttle)
print('角度',user_angle)
print(p)
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle if pilot_angle else 0.0, user_throttle
# else:
# if distance < 30:
# print('2:',distance)
# pilot_throttle = 0.0
# return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0
else:
if p[0]<0:#若未检测到行人
pilot_throttle = 0.0
pilot_angle = 0.0
print('未检测到行人',p)
return pilot_angle,pilot_throttle
else:
print('检测到行人！！！')
if distance >= 30:
return JudgeLR(p),JudgeFB(p)
elif distance < 30:
print('2:',distance)
pilot_throttle = 0.0
return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0
V.add(DriveMode(),
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher,
inputs=['user/mode', 'throttle'],
outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK":
pass
elif 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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
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'], threaded=True)
V.add(motor, inputs=["throttle"])
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM":
from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM
steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN, freq=cfg.STEERING_PWM_FREQ, inverted=cfg.STEERING_PWM_INVERTED)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN, freq=cfg.THROTTLE_PWM_FREQ, inverted=cfg.THROTTLE_PWM_INVERTED)
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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
# OLED setup
if cfg.USE_SSD1306_128_32:
from donkeycar.parts.oled import OLEDPart
auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE
oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM, auto_record_on_throttle=auto_record_on_throttle)
V.add(oled_part, inputs=['recording', 'tub/num_records', 'user/mode'], outputs=[], threaded=True)
#add tub to save data
inputs=['cam/image_array',
'user/angle', 'user/throttle',
'range/cms', #新增传感器数据
'user/mode']
types=['image_array',
'float', 'float',
'float', #对应新增传感器数据的类型
'str']
if cfg.TRAIN_BEHAVIORS:
inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]
types += ['int', 'str', 'vector']
if cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH:
inputs += ['cam/depth_array']
types += ['gray16_array']
if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
# th = TubHandler(path=cfg.DATA_PATH)
# tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
# V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." % cfg.WEB_CONTROL_PORT)
else:
print("You can now go to <your hostname.local>:%d to drive your car." % cfg.WEB_CONTROL_PORT)
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
# tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
# V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
# ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
#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,
model_type=None,
camera_type='single',
meta=[]):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
host=cfg.SIM_HOST,
env_name=cfg.DONKEY_GYM_ENV_NAME,
conf=cfg.GYM_CONF,
delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle', 'brake']
V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
V.add(
ctr,
inputs=['cam/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(port=cfg.WEB_CONTROL_PORT,
mode=cfg.WEB_INIT_MODE)
V.add(
ctr,
inputs=['cam/image_array', 'tub/num_records'],
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
# Use the FPV preview, which will show the cropped image output, or the full frame.
if cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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"]
else:
inputs = ['cam/image_array']
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
outputs = ['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
if cfg.STOP_SIGN_DETECTOR:
from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector
V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE,
cfg.STOP_SIGN_SHOW_BOUNDING_BOX),
inputs=['cam/image_array', 'pilot/throttle'],
outputs=['pilot/throttle', 'cam/image_array'])
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, user_brake, pilot_angle,
pilot_throttle, pilot_brake):
if mode == 'user':
return user_angle, user_throttle, user_brake
elif mode == 'local_angle':
return pilot_angle if pilot_angle else 0.0, user_throttle, user_brake
else:
return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0, pilot_brake if pilot_brake else 0.0
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'user/brake',
'pilot/angle', 'pilot/throttle', 'pilot/brake'
],
outputs=['angle', 'throttle', 'brake'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#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.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
# do we want to store new records into own dir or append to existing
tub_path = TubHandler(path=cfg.DATA_PATH).create_tub_path() if \
cfg.AUTO_CREATE_NEW_TUB else cfg.DATA_PATH
tub_writer = TubWriter(tub_path, inputs=inputs, types=types, metadata=meta)
V.add(tub_writer,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." %
cfg.WEB_CONTROL_PORT)
else:
print(
"You can now go to <your hostname.local>:%d to drive your car."
% cfg.WEB_CONTROL_PORT)
elif isinstance(ctr, JoystickController):
print("You can now move your joystick to drive your car.")
ctr.set_tub(tub_writer.tub)
ctr.print_controls()
#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,
model_path_1=None,
use_joystick=False,
model_type=None,
camera_type='single',
meta=[]):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif cfg.CAMERA_TYPE == "D435":
from donkeycar.parts.realsense435i import RealSense435i
cam = RealSense435i(enable_rgb=cfg.REALSENSE_D435_RGB,
enable_depth=cfg.REALSENSE_D435_DEPTH,
enable_imu=cfg.REALSENSE_D435_IMU,
device_id=cfg.REALSENSE_D435_ID)
V.add(cam,
inputs=[],
outputs=[
'cam/image_array', 'cam/depth_array', 'imu/acl_x',
'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
],
threaded=True)
#将图像转化为矩阵
else:
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
host=cfg.SIM_HOST,
env_name=cfg.DONKEY_GYM_ENV_NAME,
conf=cfg.GYM_CONF,
delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle']
elif 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,
framerate=cfg.CAMERA_FRAMERATE,
vflip=cfg.CAMERA_VFLIP,
hflip=cfg.CAMERA_HFLIP)
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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "IMAGE_LIST":
from donkeycar.parts.camera import ImageListCamera
cam = ImageListCamera(path_mask=cfg.PATH_MASK)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
V.add(
ctr,
inputs=['cam/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(port=cfg.WEB_CONTROL_PORT,
mode=cfg.WEB_INIT_MODE)
V.add(
ctr,
inputs=['cam/image_array', 'tub/num_records'],
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']) #滤除油门小于的数据，关于倒车zpy
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import IMU
imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG)
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
#目标检测部分zpy
class Detect():
def __init__(self, cfg):
self.switch = 0
self.cfg = cfg
def run(self, img):
#print('Adding part Detect')
#test=cv2.imread('/home/pi/mycar/picture/test1.jpg',1)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower_green = np.array([35, 100, 46])
upper_green = np.array([77, 255, 255])
mask = cv2.inRange(hsv, lower_green, upper_green)
masked = cv2.bitwise_and(img, img, mask=mask)
template1 = cv2.imread('/home/pi/mycar/picture/GandB.jpg', 1)
template2 = cv2.imread('/home/pi/mycar/picture/green2.jpg', 1)
temp_b = cv2.cvtColor(template1, cv2.COLOR_BGR2GRAY)
#template=cv2.cvtColor(template,COLOR_BGR2GRAY)
res1 = cv2.matchTemplate(img, template1, cv2.TM_CCOEFF_NORMED)
min_val1, max_val1, min_loc1, max_loc1 = cv2.minMaxLoc(res1)
res2 = cv2.matchTemplate(img, template2, cv2.TM_CCOEFF_NORMED)
min_val2, max_val2, min_loc2, max_loc2 = cv2.minMaxLoc(res2)
#进门1，出门0
if max_val1 > 0.65 and max_val2 < 0.65:
self.switch = 1
print("\tmatch-1\t", max_val1, max_val2)
return self.switch
elif max_val1 < 0.65 and max_val2 > 0.65:
self.switch = 0
print("\tmatch-2\t", max_val1, max_val2)
return self.switch
else:
print(max_val1, '---', max_val2)
return self.switch
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot') #利用imgprocess对输入的图像数据进行处理zpy
#在自动驾驶模式下加入detect部分
V.add(Detect(cfg),
inputs=['cam/image_array'],
outputs=['match'],
run_condition='run_pilot')
# Use the FPV preview, which will show the cropped image output, or the full frame.
if cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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 = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs = [
inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = [inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
if model_path_1:
#When we have a model, first create an appropriate Keras part
kl_1 = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path_1 or '.uff' in model_path_1 or 'tflite' in model_path_1 or '.pkl' in model_path_1:
#when we have a .h5 extension
#load everything from the model file
load_model(kl_1, model_path_1)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path_1:
#when we have a .json extension
#load the model from there and look for a matching
#.wts file with just weights
load_model_json(kl, model_path_1)
weights_path = model_path_1.replace('.json', '.weights')
load_weights(kl, weights_path)
def reload_weights(filename):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
#outputs=['pilot/angle', 'pilot/throttle']
outputs_0 = ['pilot/angle_0', 'pilot/throttle_0']
outputs_1 = ['pilot/angle_1', 'pilot/throttle_1']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
#从模型中得到转向油门数据zpy
V.add(kl, inputs=inputs, outputs=outputs_0, run_condition='run_pilot')
V.add(kl_1,
inputs=inputs,
outputs=outputs_1,
run_condition='run_pilot')
if cfg.STOP_SIGN_DETECTOR:
from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector
V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE,
cfg.STOP_SIGN_SHOW_BOUNDING_BOX),
inputs=['cam/image_array', 'pilot/throttle'],
outputs=['pilot/throttle', 'cam/image_array'])
#zpy更改模式
class MyPilot:
def __init__(self):
self.angle = 0
self.throttle = 0
self.angle_0 = 0
self.angle_1 = 0
self.throttle_0 = 0
self.throttle_1 = 0
self.mode = 0
self.state = [False, False, False, False]
self.match = 0
def run_threaded(self, match, a0, t0, a1, t1):
self.angle_0 = a0
self.angle_1 = a1
self.throttle_0 = t0
self.throttle_1 = t1
#self.mode = mode
self.match = match
return self.angle, self.throttle
def update(self):
while True:
self.angle, self.throttle = self.run(self.match, self.angle_0,
self.angle_1,
self.throttle_0,
self.throttle_1)
#print(self.angle)
def run(self, match, angle_0, throttle_0, angle_1, throttle_1):
#print(match,"\tm0",angle_0,throttle_0,"\tm1",angle_1,throttle_1)
if match == 0:
#print("mode:",pilot_mode," angle:",angle_0," throttle:",throttle_0)
return angle_0, throttle_0
elif match == 1:
#print("mode:",pilot_mode," angle:",angle_1," throttle:",throttle_1)
return angle_1, throttle_1
else:
return 0, 0
V.add(MyPilot(),
inputs=[
'match', 'pilot/angle_0', 'pilot/throttle_0', 'pilot/angle_1',
'pilot/throttle_1'
],
outputs=['mypilot/angle', 'mypilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle if pilot_angle else 0.0, user_throttle
else:
return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'mypilot/angle',
'mypilot/throttle'
],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK":
pass
elif 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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
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'], threaded=True)
V.add(motor, inputs=["throttle"])
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM":
from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM
steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN,
freq=cfg.STEERING_PWM_FREQ,
inverted=cfg.STEERING_PWM_INVERTED)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN,
freq=cfg.THROTTLE_PWM_FREQ,
inverted=cfg.THROTTLE_PWM_INVERTED)
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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
# OLED setup
if cfg.USE_SSD1306_128_32:
from donkeycar.parts.oled import OLEDPart
auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE
oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM,
auto_record_on_throttle=auto_record_on_throttle)
V.add(oled_part,
inputs=['recording', 'tub/num_records', 'user/mode'],
outputs=[],
threaded=True)
#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.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH:
inputs += ['cam/depth_array']
types += ['gray16_array']
if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." %
cfg.WEB_CONTROL_PORT)
else:
print(
"You can now go to <your hostname.local>:%d to drive your car."
% cfg.WEB_CONTROL_PORT)
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
#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,
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_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = "categorical"
#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'])
else:
inputs = []
threaded = True
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
elif 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,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
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
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)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
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, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*cfg.LOC_COLORS[track_loc])
return -1
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 and not cfg.DONKEY_GYM:
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', "pilot/loc"
],
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 Exception as e:
print(e)
print('ERR>> 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 Exception as e:
print(e)
print('ERR>> 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
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)
outputs = ['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, 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.DONKEY_GYM:
pass
elif 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 cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
types +=['float', 'float', 'float',
'float', 'float', 'float']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
print("You can now go to <your pis hostname.local>: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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
def drive(cfg,
model_path=None,
use_joystick=False,
use_hedge=False,
use_aws=False,
use_map=False,
use_debug=False,
model_type=None,
camera_type='single',
write_both_images=False,
meta=[]):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
# pigpio 初期化
try:
import pigpio
except:
raise
pgio = pigpio.pi()
#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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif write_both_images or (cfg.CAMERA_TYPE != "MAP" and use_map == False):
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
#print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
elif 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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(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.memキー名
if write_both_images:
from parts.datastore import FWD_CAMERA_KEY
outputs = [FWD_CAMERA_KEY]
else:
outputs = ['cam/image_array']
V.add(cam, inputs=inputs, outputs=outputs, threaded=threaded)
'''
Marvelmind 位置情報システム
'''
# Marvelmind USNav データ
hedge_items = []
hedge_types = []
usnav_items = [
'usnav/id',
'usnav/x',
'usnav/y',
'usnav/z',
'usnav/angle',
'usnav/timestamp',
]
usnav_types = [
'str',
'float',
'float',
'float',
'float',
'float',
]
hedge_items += usnav_items
hedge_types += usnav_types
# Marvelmind USNav Raw データ
usnav_raw_items = [
'dist/id', 'dist/b1', 'dist/b1d', 'dist/b2', 'dist/b2d', 'dist/b3',
'dist/b3d', 'dist/b4', 'dist/b4d', 'dist/timestamp'
]
usnav_raw_types = [
'str',
'str',
'float',
'str',
'float',
'str',
'float',
'str',
'float',
'float',
]
hedge_items += usnav_raw_items
hedge_types += usnav_raw_types
# Marvelmind IMU データ
imu_items = [
'imu/x',
'imu/y',
'imu/z',
'imu/qw',
'imu/qx',
'imu/qy',
'imu/qz',
'imu/vx',
'imu/vy',
'imu/vz',
'imu/ax',
'imu/ay',
'imu/az',
'imu/gx',
'imu/gy',
'imu/gz',
'imu/mx',
'imu/my',
'imu/mz',
'imu/timestamp',
]
imu_types = [
'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float',
'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float',
'float', 'float', 'float', 'float'
]
# Marvelmind 全データ
hedge_items += imu_items
hedge_types += imu_types
# Veichle上の Marvelmind 全データ初期化
for i in range(len(hedge_items)):
_item = hedge_items[i]
_type = hedge_types[i]
if _type == 'str':
V.mem[_item] = '0'
elif _type == 'float':
V.mem[_item] = 0.0
else:
raise ValueError('unknown type:{}'.format(_type))
if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
'''
Marvelmind システムを使用する場合
'''
print('Using Marvelmind Mobile Beacon')
# Marvelmind モバイルビーコンパーツ追加
from parts import HedgehogController
hedge = HedgehogController(tty=cfg.HEDGE_SERIAL_TTY, adr=cfg.HEDGE_ID)
V.add(hedge, outputs=hedge_items)
'''
IMU(MPU9250/MPU6050)
'''
mpu6050_items = [
'imu/acl_x',
'imu/acl_y',
'imu/acl_z',
'imu/gyr_x',
'imu/gyr_y',
'imu/gyr_z',
'imu/recent',
'imu/mpu_timestamp',
]
mpu6050_types = [
'float',
'float',
'float',
'float',
'float',
'float',
'str',
'float',
]
mpu9250_items = [
'imu/acl_x',
'imu/acl_y',
'imu/acl_z',
'imu/gyr_x',
'imu/gyr_y',
'imu/gyr_z',
'imu/mgt_x',
'imu/mgt_y',
'imu/mgt_z',
'imu/temp',
'imu/recent',
'imu/mpu_timestamp',
]
mpu9250_types = [
'float',
'float',
'float',
'float',
'float',
'float',
'float',
'float',
'float',
'float',
'str',
'float',
]
if cfg.HAVE_IMU:
mpu_items = []
mpu_types = []
if cfg.IMU_TYPE == 'mpu6050':
'''
MPU6050を使用する場合
'''
mpu_items += mpu6050_items
mpu_types += mpu6050_types
from parts.sensors.imu import Mpu6050
imu = Mpu6050(pgio=pgio,
bus=cfg.MPU6050_I2C_BUS,
address=cfg.MPU6050_I2C_ADDRESS,
depth=cfg.MPU6050_DEPTH,
debug=use_debug)
elif cfg.IMU_TYPE == 'mpu9250':
'''
MPU9250を使用する場合
'''
mpu_items += mpu9250_items
mpu_types += mpu9250_types
from parts.sensors.imu import Mpu9250
imu = Mpu9250(pgio=pgio,
bus=cfg.MPU9250_I2C_BUS,
mpu9250_address=cfg.MPU9250_I2C_ADDRESS,
ak8963_address=cfg.AK8963_I2C_ADDRESS,
depth=cfg.MPU9250_DEPTH,
debug=use_debug)
else:
raise ValueError('unknown IMU_TYPE = {}'.format(str(cfg.IMU_TYPE)))
# Veihcle上のIMUデータを初期化
for i in range(len(mpu_items)):
_item = mpu_items[i]
_type = mpu_types[i]
if _type == 'str':
V.mem[_item] = '{}'
elif _type == 'float':
V.mem[_item] = 0.0
else:
raise ValueError('unknown type:{}'.format(_type))
# IMUパーツの追加
V.add(imu, outputs=mpu_items)
'''
2D マップ画像 (usnav/x, usnav/y, imu/recent を使用する)
'''
map_items = [
'usnav/x',
'usnav/y',
'imu/recent',
]
map_types = [
'float',
'float',
'str',
]
if use_map or cfg.CAMERA_TYPE == "MAP":
'''
2D マップ画像でカメラの代替とする場合
'''
if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
'''
Marvelmindが有効である場合
'''
if (cfg.HAVE_IMU and cfg.IMU_TYPE == 'mpu9250'):
'''
MPU9250を使用する場合
'''
# 2D マップ生成パーツを追加
from parts import MapImageCreator
creator = MapImageCreator(
base_image_path=cfg.MAP_BASE_IMAGE_PATH, debug=use_debug)
V.add(creator, inputs=map_items, outputs=['cam/image_array'])
else:
raise ValueError('2D map needs mpu9250 data')
else:
raise ValueError('2D map needs Marvelmind USNav data')
'''
ジョイスティック
'''
user_items = [
'user/angle',
'user/throttle',
'user/lift_throttle',
'user/mode',
]
user_types = [
'float',
'float',
'float',
'str',
]
joystick_items = []
joystick_types = []
joystick_items += user_items
joystick_types += user_types
joystick_items += ['recording']
joystick_types += ['boolean']
# ジョイスティックパーツのアウトプット値なのでここでは初期化せず
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
'''
ジョイスティックを使用する場合
'''
# フォークリフト用ジョイスティックパーツ取得ファクトリ関数を呼び出す
from parts import get_js_controller
ctr = get_js_controller(cfg)
if cfg.USE_NETWORKED_JS:
'''
ネットワーク経由で操作する場合(Donkeycar標準)
'''
from donkeycar.parts.controller import JoyStickSub
netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
V.add(netwkJs, threaded=True)
ctr.js = netwkJs
# リフト値の追加
V.add(ctr,
inputs=['cam/image_array'],
outputs=joystick_items,
threaded=True)
else:
'''
Webコントローラを使用する
'''
# user/lift_throttle 値が常に0となるWebコントローラパーツを生成
from parts import LocalWebForkliftController
ctr = LocalWebForkliftController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=joystick_items,
threaded=True)
# 変数ctrはこの後でも使用する
'''
スロットルフィルタ(ワンタップESC後進)
'''
# 入力値はジョイスティックパーツがアウトプット
#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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
# run_pilot 値はboolean(偽：手動運転、真：一部もしくは全て自動運転)
'''
3色LED表示
'''
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
#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)
from parts import RGB_LED
led = RGB_LED(pgio, cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
outputs=['led/blink_rate'])
V.add(led, inputs=['led/blink_rate'])
def get_record_alert_color(num_records):
"""
Tubデータ件数を色PWMタプル(r, g, b)：各0-100に変換する。
件数範囲、PWMタプルはconfig.py上のRECORD_ALERT_COLOR_ARR
を参照している。
引数：
num_records Tubデータ件数
戻り値
色PWMタプル (r, g, b)形式(0-100)
"""
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
'''
レコードトラッカ
'''
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
# ジョイスティックのボタンにレコード件数表示用のボタン割当
# F710
if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift':
ctr.set_button_down_trigger('back', show_record_acount_status)
# JC-U3912T
elif cfg.CONTROLLER_TYPE == 'JC-U3912T':
ctr.set_button_down_trigger('7', show_record_acount_status)
# default
else:
ctr.set_button_down_trigger('circle', show_record_acount_status)
'''
Sombero HAT
'''
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
_ = Sombrero()
# この段階でカメラ or 2Dマップ画像が cam/image_array に格納されている
'''
画像前処理
'''
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
'''
coral を使用する場合は画像をそのまま使用
'''
inf_input = 'cam/image_array'
else:
'''
自動運転時のみ正規化・CROP処理する
'''
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
#Behavioral state
if cfg.TRAIN_BEHAVIORS:
'''
behaviorモデルを使用する場合
'''
bh = BehaviorPart(cfg.BEHAVIOR_LIST)
V.add(bh,
outputs=[
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
])
try:
# L1ボタンにbehavior状態更新を割当
ctr.set_button_down_trigger('L1', bh.increment_state)
except:
pass
inputs = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
'''
IMU（MPU9250/MPU6050）モデル
'''
#assert(cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
if cfg.HAVE_IMU:
# 機械学習モデルのインプットにMPU9250/6050の加速度、角速度を追加
inputs = [
'cam/image_array',
'imu/acl_x',
'imu/acl_y',
'imu/acl_z',
'imu/gyr_x',
'imu/gyr_y',
'imu/gyr_z',
]
elif cfg.HAVE_HEDGE and (
use_hedge or cfg.USE_HEDGE_AS_DEFAULT) and cfg.USE_HEDGE_IMU:
# 機械学習モデルのインプットにMarvelmindの加速度、角速度を追加
inputs = [
'cam/image_array', 'imu/ax', 'imu/ay', 'imu/az', 'imu/vx',
'imu/vy', 'imu/vz'
]
else:
raise ValueError('can not use imu model without imu data')
else:
# 機械学習モデルのインプットは画像のみ
inputs = [inf_input]
'''
機械学習モデル
'''
def load_model(kl, model_path):
"""
機械学習モデルに学習済みパラメータをロードする。
引数：
kl 機械学習モデルオブジェクト
model_path 学習済みパラーメータファイルのパス
戻り値：
なし
"""
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
def load_weights(kl, weights_path):
"""
機械学習モデルに学習済みパラメータをロードする。
引数：
kl 機械学習モデルオブジェクト
model_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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
try:
from tensorflow.python import keras
except:
raise
try:
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)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
'''
モデルファイル更新監視
'''
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
outputs = ['pilot/angle', 'pilot/throttle']
'''
ローカライザモデルの場合の出力編集
'''
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
'''
モデルがない場合の自動運転結果初期値設定
'''
V.mem['pilot/throttle'] = 0.0
V.mem['pilot/angle'] = 0.0
V.mem['pilot/lift_throttle'] = 0.0
'''
運転モードから、モータ入力値決定
'''
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, user_lift_throttle,
pilot_angle, pilot_throttle, pilot_lift_throttle):
if mode == 'user':
return user_angle, user_throttle, user_lift_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle, user_lift_throttle
else:
return pilot_angle, (pilot_throttle * cfg.AI_THROTTLE_MULT), (
pilot_lift_throttle * cfg.AI_THROTTLE_MULT)
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'user/lift_throttle',
'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle'
],
outputs=['angle', 'throttle', 'lift_throttle'])
# Donkeycarへの最終的な指示が angle, throttle, lift_throttle 値となる
'''
AIランチャ
'''
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
'''
AIによる自動運転かどうかの判定
'''
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
'''
AI運転時の記録モード
'''
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
'''
モータ駆動
'''
#Drive train setup
if cfg.DONKEY_GYM:
pass
elif 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'])
V.mem['lift_throttle'] = 0
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'])
V.mem['lift_throttle'] = 0
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'])
V.mem['lift_motor_throttle'] = 0
# Forklift 駆動モータ操作
elif cfg.DRIVE_TRAIN_TYPE == "THREE_MOTORS_PIGPIO":
'''
フォークリフト３モータ駆動
'''
from parts import PIGPIO_OUT, PIGPIO_PWM, ForkliftMotorDriver
motor_driver = ForkliftMotorDriver(left_balance=cfg.LEFT_PWM_BALANCE,
right_balance=cfg.RIGHT_PWM_BALANCE,
debug=False)
V.add(motor_driver,
inputs=['throttle', 'angle', 'lift_throttle'],
outputs=[
'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
])
if use_debug:
class Prt:
def run(self, left_vref, left_in1, left_in2, right_vref,
right_in1, right_in2, lift_vref, lift_in1, lift_in2):
print('ForkliftMD left vref:{}, in1:{}, in2:{}'.format(
str(left_vref), str(left_in1), str(left_in2)))
print('ForkliftMD right vref:{}, in1:{}, in2:{}'.format(
str(right_vref), str(right_in1), str(right_in2)))
print('ForkliftMD lift vref:{}, in1:{}, in2:{}'.format(
str(lift_vref), str(lift_in1), str(lift_in2)))
V.add(Prt(),
inputs=[
'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
])
# TB6612#1 A系（右モータ、左折時駆動する）
V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio),
inputs=['left_motor_in1'])
V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio),
inputs=['left_motor_in2'])
V.add(PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO,
pgio=pgio,
freq=cfg.PWM_FREQ,
range=cfg.PWM_RANGE,
threshold=cfg.PWM_INPUT_THRESHOLD),
inputs=['left_motor_vref'])
# TB6612#1 B系（左モータ、右折時駆動する）
V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio),
inputs=['right_motor_in1'])
V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio),
inputs=['right_motor_in2'])
V.add(PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO,
pgio=pgio,
freq=cfg.PWM_FREQ,
range=cfg.PWM_RANGE,
threshold=cfg.PWM_INPUT_THRESHOLD),
inputs=['right_motor_vref'])
# TB6612#2 A系（リフトモータ）
V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN1_GPIO, pgio=pgio),
inputs=['lift_motor_in1'])
V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN2_GPIO, pgio=pgio),
inputs=['lift_motor_in2'])
V.add(PIGPIO_PWM(pin=cfg.LIFT_MOTOR_PWM_GPIO,
pgio=pgio,
freq=cfg.PWM_FREQ,
range=cfg.PWM_RANGE,
threshold=cfg.PWM_INPUT_THRESHOLD),
inputs=['lift_motor_vref'])
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"])
V.mem['lift_throttle'] = 0
'''
Tubデータ
'''
# ベースとなるTubデータ
if write_both_images:
from parts.datastore import FWD_CAMERA_KEY
inputs = ['cam/image_array', FWD_CAMERA_KEY]
types = ['image_array', 'image_array']
else:
inputs = ['cam/image_array']
types = ['image_array']
inputs += user_items
types += user_types
if cfg.TRAIN_BEHAVIORS:
'''
behaviorモデルを使用する場合の入力データを追加
'''
inputs += [
'behavior/state', 'behavior/label', 'behavior/one_hot_state_array'
]
types += ['int', 'str', 'vector']
'''
IMU(MPU6050)データ追加
'''
# Tubへ格納する加速度、角速度
tub_imu_inputs = [
'imu/acl_x',
'imu/acl_y',
'imu/acl_z',
'imu/gyr_x',
'imu/gyr_y',
'imu/gyr_z',
]
tub_imu_input_types = [
'float',
'float',
'float',
'float',
'float',
'float',
]
if not cfg.HAVE_IMU:
'''
MPU9250/MPU6050を持っていない場合
'''
if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
'''
Marvelmind が有効な場合
'''
if cfg.USE_HEDGE_IMU:
'''
Marvelmind IMUが使用可能な場合
'''
# 必要な Marvelmind IMUデータを移動させる
class MoveIMU:
def run(self, ax, ay, az, gx, gy, gz):
return ax, ay, az, gx, gy, gz
move = MoveIMU()
V.add(move,
inputs=[
'imu/ax',
'imu/ay',
'imu/az',
'imu/gx',
'imu/gy',
'imu/gz',
],
outputs=tub_imu_inputs)
inputs += tub_imu_inputs
types += tub_imu_input_types
else:
inputs += tub_imu_inputs
types += tub_imu_input_types
if cfg.RECORD_DURING_AI:
'''
自動運転中であっても記録しておくモードの場合
'''
# pilot/* を Tub データに加える
inputs += ['pilot/angle', 'pilot/throttle', 'pilot/lift_throttle']
types += ['float', 'float', 'float']
'''
Tubデータ書き込み
'''
th = TubHandler(path=cfg.DATA_PATH)
if write_both_images:
from parts.datastore import TubHandler as NewTubHandler
th = NewTubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
'''
カメライメージを通信して貰う場合(Donkeycar標準)
'''
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
'''
操縦系統別
'''
if type(ctr) is LocalWebController:
'''
LocalWebController を使用している場合の usage を表示
'''
print("You can now go to <your pi ip address>:8887 to drive your car.")
elif isinstance(ctr, JoystickController):
'''
ジョイスティックを使用している場合の usage を表示
'''
print("You can now move your joystick to drive your car.")
#tell the controller about the tub
ctr.set_tub(tub)
if cfg.BUTTON_PRESS_NEW_TUB:
'''
cfg.BUTTON_PRESS_NEW_TUB が True の場合、ボタンを押すごとに別のTubディレクトリに
データが格納されるようになる（デフォルト:False）。
'''
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
# ボタン割当
# F710
if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift':
ctr.set_button_down_trigger('A', new_tub_dir)
# JC-U3912T
elif cfg.CONTROLLER_TYPE == 'JC-U3912T' or cfg.CONTROLLER_TYPE == 'JC-U3912T_Forklift':
ctr.set_button_down_trigger('12', new_tub_dir)
# default
else:
ctr.set_button_down_trigger('cross', new_tub_dir)
# 現時点のボタン割当状態を表示
ctr.print_controls()
"""
AWS IoT Core
"""
if use_aws or cfg.USE_AWS_AS_DEFAULT:
print('Start aws configuration')
from parts.broker import AWSShadowClientFactory, PowerReporter
factory = AWSShadowClientFactory(cfg.AWS_CONFIG_PATH,
cfg.AWS_THING_NAME)
# Power ON 情報の送信
power = PowerReporter(factory, debug=use_debug)
power.on()
# Tubデータ(json)送信
from parts.broker.pub import Publisher
pub_tub = Publisher(factory, debug=use_debug)
V.add(pub_tub,
inputs=[
'user/angle', 'user/throttle', 'user/lift_throttle',
'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle',
'user/mode'
])
# Tubデータ(イメージ)送信
from parts.broker.pub import ImagePublisher
pub_img = ImagePublisher(factory, debug=use_debug)
V.add(pub_img, inputs=[
'cam/image_array',
])
# fwdイメージデータ送信
if write_both_images:
from parts.broker.pub import FwdImagePublisher
pub_fwd_img = FwdImagePublisher(factory, debug=use_debug)
from parts.datastore import FWD_CAMERA_KEY
V.add(pub_fwd_img, inputs=[
FWD_CAMERA_KEY,
])
'''
ジョイスティックデータの送信
'''
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
'''
ジョイスティックを使用している場合
'''
from parts.broker.pub import JoystickPublisher
pub_joy = JoystickPublisher(factory, debug=use_debug)
V.add(pub_joy, inputs=joystick_items)
'''
Marvelmind システムデータの送信
'''
if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
'''
Marvelmind システムを使用している場合
'''
if cfg.USE_HEDGE_USNAV:
'''
Marvelmind 位置情報を使用している場合
'''
from parts.broker.pub import USNavPublisher
pub_usn = USNavPublisher(factory, debug=use_debug)
V.add(pub_usn, inputs=usnav_items)
if cfg.USE_HEDGE_USNAV_RAW:
'''
Marvelmind 距離情報を使用している場合
'''
from parts.broker.pub import USNavRawPublisher
pub_raw = USNavRawPublisher(factory, debug=use_debug)
V.add(pub_raw, inputs=usnav_raw_items)
if cfg.USE_HEDGE_IMU:
'''
Marvelmind IMU情報を使用している場合
'''
from parts.broker.pub import IMUPublisher
pub_imu = IMUPublisher(factory, debug=use_debug)
V.add(pub_imu, inputs=imu_items)
'''
MPU9250/MPU6050 データの送信
'''
if cfg.HAVE_IMU:
'''
MPU9250/MPU6050 を使用している場合
'''
if cfg.IMU_TYPE == 'mpu6050':
'''
MPU6050を使用している場合
'''
from parts.broker.pub import Mpu6050Publisher
pub_mpu = Mpu6050Publisher(factory, debug=use_debug)
V.add(pub_mpu, inputs=mpu_items)
elif cfg.IMU_TYPE == 'mpu9250':
'''
MPU6050を使用している場合
'''
from parts.broker.pub import Mpu9250Publisher
pub_mpu = Mpu9250Publisher(factory, debug=use_debug)
V.add(pub_mpu, inputs=mpu_items)
'''
運転ループ
'''
try:
print('Start running')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
except KeyboardInterrupt:
# Ctrl+C押下時
pass
finally:
# デバッグ
if use_debug:
print('Stop running')
# pigpio 利用停止
pgio.stop()
if use_aws or cfg.USE_AWS_AS_DEFAULT:
if power is not None:
# Power Off 情報の送信
if use_debug:
print('Sending power off')
power.off()
# 送信するまで待機
time.sleep(1)
print('Stopped')
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single',
meta=[]):
'''
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 cfg.DONKEY_GYM:
# the simulator will use cuda and then we usually run out of resources
# if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
# Initialize car
V = dk.vehicle.Vehicle()
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif cfg.CAMERA_TYPE == "D435":
from donkeycar.parts.realsense435i import RealSense435i
cam = RealSense435i(enable_rgb=cfg.REALSENSE_D435_RGB,
enable_depth=cfg.REALSENSE_D435_DEPTH,
enable_imu=cfg.REALSENSE_D435_IMU,
device_id=cfg.REALSENSE_D435_ID)
V.add(cam,
inputs=[],
outputs=[
'cam/image_array', 'cam/depth_array', 'imu/acl_x',
'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
],
threaded=True)
else:
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
host=cfg.SIM_HOST,
env_name=cfg.DONKEY_GYM_ENV_NAME,
conf=cfg.GYM_CONF,
delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle']
elif 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,
framerate=cfg.CAMERA_FRAMERATE,
vflip=cfg.CAMERA_VFLIP,
hflip=cfg.CAMERA_HFLIP)
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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "IMAGE_LIST":
from donkeycar.parts.camera import ImageListCamera
cam = ImageListCamera(path_mask=cfg.PATH_MASK)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
V.add(ctr,
inputs=['cam/image_array', 'tub/num_records', 'showbutton'],
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording',
'wim', 'wtg', 'gon'
],
threaded=True)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT,
mode=cfg.WEB_INIT_MODE)
V.add(ctr,
inputs=['cam/image_array', 'tub/num_records', 'showbutton'],
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording',
'wim', 'wtg', 'gon'
],
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
# returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
# Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
# IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import IMU
imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG)
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
# Use the FPV preview, which will show the cropped image output, or the full frame.
if cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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 = [inf_input, "behavior/one_hot_state_array"]
# IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
# Run the pilot if the mode is not user.
inputs = [
inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = [inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
# When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
# when we have a .h5 extension
# load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
# when we have a .json extension
# load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
# this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
# these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
outputs = ['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
if cfg.STOP_SIGN_DETECTOR:
from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector
V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE,
cfg.STOP_SIGN_SHOW_BOUNDING_BOX),
inputs=['cam/image_array', 'pilot/throttle'],
outputs=['pilot/throttle', 'cam/image_array'])
# Choose what inputs should change the car.
# Choose what inputs should change the car.
'''下面代码能实现的重要条件是小车一开始就得处于原来的自动驾驶状态
'''
# wci=梁的where car is
# wim=蔡的where i am， wtg=蔡的where to go, gon=蔡接收到button的值后，另一个变量go or not
class Picmatch:
def __init__(self):
self.mylocation = '0'
self.color = 'green'
self.template = []
self.green_sign = 0
self.yellow_sign = 0
self.time1 = time.time()
self.img = np.ones(shape=(120, 160, 3))
print('start!!!')
self.template = cv2.imread(self.color + '_tem.jpg', 1)
def run_threaded(self, img):
self.img = img
return self.mylocation
def update(self):
while True:
self.mylocation = self.run(self.img)
def run(self, img):
try:
img = np.array(img)
img = np.array(img, dtype='uint8')
img = img[70:120]
except:
return '0'
# 相关系数匹配方法: cv2.TM_CCOEFF
res1 = cv2.matchTemplate(img, self.template, cv2.TM_CCOEFF_NORMED)
min_val1, max_val1, min_loc1, max_loc1 = cv2.minMaxLoc(res1)
if (max_val1 >= 0.62):
if (time.time() - self.time1 >= 2):
self.green_sign += 1
if (self.green_sign == 7):
self.green_sign = 1
self.time1 = time.time()
self.mylocation = str(self.green_sign)
#if (self.mylocation != '0'):
#print(self.mylocation)
#print(max_val1)
return self.mylocation
V.add(Picmatch(),
inputs=['cam/image_array'],
outputs=['wci'],
threaded=True)
class AfterPictureMatch:
def __init__(self):
self.extra = 0.
self.showbutton = 0. # '输入位置后点此等待小车接应！'
self.wci = '0'
self.wim = ''
self.wtg = ''
self.gon = 0
self.music_sign = 0
self.pre_music_sign = 0
def run_threaded(self, wci, wim, wtg, gon):
self.wci = wci
self.wim = wim
self.wtg = wtg
self.gon = gon
return self.extra, self.showbutton
def update(self):
while True:
self.extra, self.showbutton = self.run(self.wci, self.wim,
self.wtg, self.gon)
def run(self, wci, wim, wtg, gon):
# def run(self,wim,wtg,gon):
print('wci:', wci, 'wim:', wim, 'wtg:', wtg, 'gon:', gon)
if gon == 0:
# gon默认为0
if wim == wci:
print('可以准备出发')
self.showbutton = 2.
self.extra = 0
# '点此即可以出发！'
# 蔡小程序接收到button值，蔡默认为0的gon改为1
# play(2)#提示音我在这里哦，点击按钮即可出发
if (self.pre_music_sign != 2):
play(2)
self.pre_music_sign = 2
return self.extra, self.showbutton
elif wim != wci:
if (wim == ''):
self.extra = 0
self.showbutton = 0
self.pre_music_sign = 0
return self.extra, self.showbutton
else:
print('出发地不等于所在地')
self.showbutton = 1. # '请等待小车到达！'
self.extra = 1.
# play(1)#"提示音：小车正在自动驾驶中"
if (self.pre_music_sign != 1):
play(1)
self.pre_music_sign = 1
return self.extra, self.showbutton
else:
if wci == wtg:
print('到达目的地')
self.extra = 0
self.showbutton = 3. # '已经到达！'，蔡gon由1改为0
# play(3)#提示音：已经到达祝主人一路顺风
if (self.pre_music_sign != 3):
play(3)
self.pre_music_sign = 3
return self.extra, self.showbutton
elif wci != wtg:
print('在前进过程中')
self.extra = 1
self.showbutton = 1. # '请等待小车到达！'
# play(1)#"提示音：小车正在自动驾驶中"
if (self.pre_music_sign != 4):
play(1)
self.pre_music_sign = 4
return self.extra, self.showbutton
V.add(
AfterPictureMatch(),
# inputs=['wim','wtg','gon'],
inputs=['wci', 'wim', 'wtg', 'gon'],
outputs=['extra', 'showbutton'],
threaded=True)
class PlayMusic:
def __init__(self):
self.play_sign = 0
self.pre_play_sign = 0
def run_threaded(self, music):
self.play_sign = music
if (music != 0 and self.pre_play_sign == 0):
play(music)
self.pre_play_sign = music
if (music == 0):
self.pre_play_sign = 0
def update(self):
pass
#V.add(PlayMusic,inputs=['music'],outputs=[],threaded=True)
class Play_Music:
def __init__(self):
self.pre_play_sign = 0
def run(self, music=0):
self.play_sign = music
if (music != 0 and self.pre_play_sign == 0):
play(music)
self.pre_play_sign = music
if (music == 0):
self.pre_play_sign = 0
#V.add(Play_Music,inputs=['music'],outputs=[],)
class DriveMode:
def run(self, mode, extra, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'local_angle':
return pilot_angle if pilot_angle else 0.0, user_throttle
elif mode == 'user' or extra == 0:
#extra默认为0，由梁的类输出的一个切换参量
return user_angle, user_throttle
#已停止模式+进入沟里，控制驶出
else:
return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0
#已进入自动驾驶模式
#下面是extra参量的来源
V.add(DriveMode(),
inputs=[
'user/mode', 'extra', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'
],
outputs=['angle', 'throttle'])
# to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
# Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
# Drive train setup
if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK":
pass
elif 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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
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'], threaded=True)
V.add(motor, inputs=["throttle"])
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM":
from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM
steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN,
freq=cfg.STEERING_PWM_FREQ,
inverted=cfg.STEERING_PWM_INVERTED)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN,
freq=cfg.THROTTLE_PWM_FREQ,
inverted=cfg.THROTTLE_PWM_INVERTED)
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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
# OLED setup
if cfg.USE_SSD1306_128_32:
from donkeycar.parts.oled import OLEDPart
auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE
oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM,
auto_record_on_throttle=auto_record_on_throttle)
V.add(oled_part,
inputs=['recording', 'tub/num_records', 'user/mode'],
outputs=[],
threaded=True)
# 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.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH:
inputs += ['cam/depth_array']
types += ['gray16_array']
if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." %
cfg.WEB_CONTROL_PORT)
else:
print(
"You can now go to <your hostname.local>:%d to drive your car."
% cfg.WEB_CONTROL_PORT)
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
# run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def pickup(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.
"""
V = dk.vehicle.Vehicle()
# clock = Timestamp()
# V.add(clock, outputs=['timestamp'])
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
# for Emily's setup we need to flip the camera and change the shutter speed
cam.camera.hflip = True
cam.camera.vflip = True
cam.camera.shutter_speed = 5000
V.add(cam, inputs=[], outputs=['image'], threaded=True)
inputs = []
threaded = True
from donkeycar.parts.camera import PiCamera
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
V.add(cam, inputs=inputs, outputs=['image'], threaded=threaded)
#
# # get eyes
model = Eyes(cfg.MODEL_CFG, cfg.MODEL_WEIGHTS, cfg.IMAGE_W, cfg.IMAGE_H)
V.add(model, inputs=['image'], outputs=['bboxes'])
#
# get strategy
strategy = Brains(pic_to_reality_csv=cfg.MAP_PATH)
V.add(strategy,
inputs=['bboxes'],
outputs=[
'angle', 'throttle', 'runtime', 'waittime', 'emily', 'emil', 'x',
'y', 'emil3', 'emil3rest'
])
print('steering channel ', cfg.STEERING_CHANNEL, ' throttle channel ',
cfg.THROTTLE_CHANNEL)
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'], outputs=[], run_condition='emily')
V.add(throttle, inputs=['throttle'], outputs=[], run_condition='emily')
# # add the stopper
runner = Runner()
V.add(runner, inputs=['runtime'], outputs=['throttle2'])
#
# # add second throttle
throttle2 = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(throttle2, inputs=['throttle2'], outputs=[], run_condition='emily')
#
# # add the stopper
runner2 = Runner()
V.add(runner2, inputs=['waittime'], outputs=['throttle2'])
#
# # get Emil
# emil = Emil([0,1,2,3], cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM)
emil3_controller = PCA9685(3,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
emil3 = Emil(emil3_controller, 1.0)
V.add(emil3, inputs=['emil3'], outputs=[], run_condition='emil')
V.add(emil3, inputs=['emil3rest'], outputs=[], run_condition='emil')
#
if cfg.PUB_CAMERA_IMAGES:
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['image'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'], outputs=[])
#
# # add tub to save data
inputs = ['image', 'angle', 'throttle']
types = ['image_array', 'float', '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, outputs=["tub/num_records"])
#
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def build_recording(self):
#Ai Recording
if self.cfg.RECORD_DURING_AI and self.cfg.AUTO_RECORD_ON_THROTTLE:
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
self.vehicle.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode'
]
types = ['image_array', 'float', 'float', 'str']
if self.cfg.TRAIN_BEHAVIORS:
inputs += [
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
]
types += ['int', 'str', 'vector']
if self.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']
if self.cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
if self.cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
self.vehicle.add(ImgArrToJpg(),
inputs=['cam/image_array'],
outputs=['jpg/bin'])
self.vehicle.add(pub, inputs=['jpg/bin'])
if self.cfg.DONKEY_GYM:
if self.cfg.SIM_RECORD_LOCATION:
inputs += [
'pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed',
'pos/cte'
]
types += ['float', 'float', 'float', 'float', 'float']
if self.cfg.SIM_RECORD_GYROACCEL:
inputs += [
'gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z',
'accel/accel_x', 'accel/accel_y', 'accel/accel_z'
]
types += ['float', 'float', 'float', 'float', 'float', 'float']
if self.cfg.SIM_RECORD_VELOCITY:
inputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z']
types += ['float', 'float', 'float']
if self.cfg.SIM_RECORD_LIDAR:
inputs += ['lidar/dist_array']
types += ['nparray']
tub_path = TubNamer(path=self.cfg.DATA_PATH).create_tub_path()
tub_writer = TubWriter(tub_path,
inputs=inputs,
types=types,
metadata=self.meta)
self.vehicle.add(tub_writer,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if hasattr(self.controller, "set_tub"):
self.controller.set_tub(
tub_writer) # Only used to call delete_last_n_records
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single',
meta=[]):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
else:
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
elif 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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(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,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
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 get_js_controller
ctr = get_js_controller(cfg)
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import Mpu6050
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)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
#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 = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs = [
inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = [inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
outputs = ['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
#Choose what inputs should change the car.
class DriveMode:
def run(self, 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 * cfg.AI_THROTTLE_MULT
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM:
pass
elif 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 == "SKID_STEER":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, SBC_PiMotor
right_motor = SBC_PiMotor(1)
left_motor = SBC_PiMotor(2)
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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
print(
"You can now go to <your pis hostname.local>: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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
#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, model_path_1=None, use_joystick=False, model_type=None, camera_type='single', meta=[] ):
'''
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 cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"]="-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
else:
inputs = []
threaded = True
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
elif 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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(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, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
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 get_js_controller
ctr = get_js_controller(cfg)
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', 'switch_mod', 'taga', 'tagb', 'tagc', 'tagd'], #2020.10.24 by zmx
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg), inputs=['user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc"],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
V.add(rec_tracker_part, inputs=["tub/num_records"], outputs=['records/alert'])
class Detect():
def __init__(self, cfg):
self.cfg = cfg
def run(self,img):
#print('Adding part Detect')
#test=cv2.imread('/home/pi/mycar/picture/test1.jpg',1)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower_green = np.array([35,100,46])
upper_green = np.array([77,255,255])
mask = cv2.inRange(hsv, lower_green, upper_green)
masked = cv2.bitwise_and(img,img, mask= mask)
template=cv2.imread('/home/pi/mycar/picture/flag1.jpg',1)
temp_b = cv2.cvtColor(template, cv2.COLOR_BGR2GRAY)
#template=cv2.cvtColor(template,COLOR_BGR2GRAY)
res=cv2.matchTemplate(mask,temp_b,cv2.TM_CCOEFF_NORMED)
min_val,max_val,min_loc,max_loc=cv2.minMaxLoc(res)
#print(max_val)
if max_val > 0.60:
print("match",max_val)
return 1
else:
print("no",max_val)
return 0
V.add(Detect(cfg),inputs=['cam/image_array'],outputs=['mypilot_mod'],run_condition='run_pilot')
#在自动驾驶模式下加入detect部分
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import Mpu6050
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 Exception as e:
print(e)
print('ERR>> 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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)) )
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
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):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#by zmx 2020.11.30 试图添加第二个模型
if model_path_1:
#When we have a model, first create an appropriate Keras part
kl_1 = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb_1 = None
if '.h5' in model_path_1:
#when we have a .h5 extension
#load everything from the model file
load_model(kl_1, model_path_1)
def reload_model(filename):
load_model(kl_1, filename)
model_reload_cb_1 = reload_model
elif '.json' in model_path_1:
#when we have a .json extension
#load the model from there and look for a matching
#.wts file with just weights
load_model_json(kl_1, model_path_1)
weights_path_1 = model_path.replace('.json', '.weights')
load_weights(kl_1, weights_path_1)
def reload_weights(filename):
weights_path_1 = filename.replace('.json', '.weights')
load_weights(kl_1, weights_path_1)
model_reload_cb_1 = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running")
V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running")
outputs_0=['pilot/angle_0', 'pilot/throttle_0'] #by zmx 2020.11.30
outputs_1=['pilot/angle_1', 'pilot/throttle_1'] #by zmx 2020.11.30
if cfg.TRAIN_LOCALIZER:
outputs_0.append("pilot/loc")
outputs_1.append("pilot/loc")
V.add(kl, inputs=inputs,
outputs=outputs_0,
run_condition='run_pilot')
V.add(kl_1, inputs=inputs, #by zmx 2020.11.30 把模型的的输出更名为 _1 和 _0 在选择器(MyPilot)里选择
outputs=outputs_1,
run_condition='run_pilot')
class MySwitch:
def __init__(self):
self.pilot_state = 0 #by zmx 2020.10.24 到时候会改成int型
self.mode = 0
self.mode_switch = False
def run(self,mod_switch):
if (mod_switch):
#print("detected")
self.pilot_state = 1
else:
#print("not pushed")
self.pilot_state = 0
print("pushed : ",mod_switch,"\tmode : ",self.pilot_state)
return self.pilot_state
def update(self):
while(True):
self.pilot_state = self.run(self.mod_switch)
#V.add(MySwitch(), inputs=['switch_mod'], outputs=['web_switch'])
class MyButton:
def __init__(self):
self.button_in = [False,False,False,False]
self.state_out = [True,False,False,False] #by zmx 2020.12.7添加了4个状态量bool，其中默认0是默认状态
self.state_out1 = 1 #by zmx 2020.12.13 默认是n，接到后变化
def run(self,a,b,c,d):
if a:
self.state_out1 = 1
self.state_out = [True,False,False,False]
print("A triggered")
elif b:
self.state_out1 = 2
self.state_out = [False,True,False,False]
print("B triggered")
elif c:
self.state_out1 = 3
self.state_out = [False,False,True,False]
print("X triggered")
elif d:
self.state_out1 = 4
self.state_out = [False,False,False,True]
print("Y triggered")
#print(self.state_out1)
return self.state_out1
def update(self):
while(True):
a = self.button_in[0]
b = self.button_in[1]
c = self.button_in[2]
d = self.button_in[3]
self.state_out1 = self.run(a,b,c,d)
def run_threaded(self,taga,tagb,tagc,tagd):
self.button_in = [taga,tagb,tagc,tagd]
#print(self.state_out1,"---")
return self.state_out1
V.add(MyButton(),inputs=['taga','tagb','tagc','tagd'],outputs=['my_state'],run_condition='switch_mod')
class MyPilot:
def __init__(self):
self.angle = 0
self.throttle = 0
self.angle_0 = 0
self.angle_1 = 0
self.throttle_0 = 0
self.throttle_1 = 0
self.mode = 0
self.state = [False,False,False,False]
self.state1 = 0
def run_threaded(self,my_state,a0,t0,a1,t1):
self.angle_0 = a0
self.angle_1 = a1
self.throttle_0 = t0
self.throttle_1 = t1
#self.mode = mode
self.state1 = my_state
return self.angle, self.throttle
def update(self):
while True:
self.angle, self.throttle = self.run(self.state1,self.angle_0,self.angle_1,self.throttle_0,self.throttle_1)
#print(self.angle)
def run(self,state1,angle_0,throttle_0,angle_1,throttle_1):
print(state1,"---",angle_0,throttle_0,"------",angle_1,throttle_1)
if state1==1:
#print("mode:",pilot_mode," angle:",angle_0," throttle:",throttle_0)
return angle_0,throttle_0
elif state1==2:
#print("mode:",pilot_mode," angle:",angle_1," throttle:",throttle_1)
return angle_1,throttle_1
else:
return 0,0
V.add(MyPilot(), inputs=['my_state','pilot/angle_0', 'pilot/throttle_0','pilot/angle_1', 'pilot/throttle_1'], outputs=['mypilot/angle','mypilot/throttle'],run_condition='switch_mod')
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode,
user_angle, user_throttle,
mypilot_angle, mypilot_throttle):
#print("drivemode is running")
#print(taga,"!!!!!!!!",pilot_mode)
if mode == 'user':
#print(taga,"!!!!!!!!",pilot_mode)
return user_angle, user_throttle
elif mode == 'local_angle':
#print("local_angle")
return mypilot_angle, user_throttle
else:
#print("mode:",pilot_mode," angle:",mypilot_angle," throttle:",mypilot_throttle)
return(0.1,0.1)
#return mypilot_angle, mypilot_throttle * cfg.AI_THROTTLE_MULT
#return mypilot_angle, mypilot_throttle * cfg.AI_THROTTLE_MULT #by zmx 2020.11.30 临时的修改，把my的输出强加上去看看
V.add(DriveMode(),
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle_0', 'mypilot/throttle'],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher,
inputs=['user/mode', 'throttle'],
outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM:
pass
elif 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_GAMEPAD:
inputs += ['my_state']
types += ['int']
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']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
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)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
#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, model_type=None,
camera_type='single', meta=[]):
"""
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.
"""
logger.info(f'PID: {os.getpid()}')
if cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"]="-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
#Initialize logging before anything else to allow console logging
if cfg.HAVE_CONSOLE_LOGGING:
logger.setLevel(logging.getLevelName(cfg.LOGGING_LEVEL))
ch = logging.StreamHandler()
ch.setFormatter(logging.Formatter(cfg.LOGGING_FORMAT))
logger.addHandler(ch)
if cfg.HAVE_MQTT_TELEMETRY:
from donkeycar.parts.telemetry import MqttTelemetry
tel = MqttTelemetry(cfg)
if cfg.HAVE_ODOM:
if cfg.ENCODER_TYPE == "GPIO":
from donkeycar.parts.encoder import RotaryEncoder
enc = RotaryEncoder(mm_per_tick=0.306096, pin = cfg.ODOM_PIN, debug = cfg.ODOM_DEBUG)
V.add(enc, inputs=['throttle'], outputs=['enc/speed'], threaded=True)
elif cfg.ENCODER_TYPE == "arduino":
from donkeycar.parts.encoder import ArduinoEncoder
enc = ArduinoEncoder()
V.add(enc, outputs=['enc/speed'], threaded=True)
else:
print("No supported encoder found")
logger.info("cfg.CAMERA_TYPE %s"%cfg.CAMERA_TYPE)
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)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
elif cfg.CAMERA_TYPE == "D435":
from donkeycar.parts.realsense435i import RealSense435i
cam = RealSense435i(
enable_rgb=cfg.REALSENSE_D435_RGB,
enable_depth=cfg.REALSENSE_D435_DEPTH,
enable_imu=cfg.REALSENSE_D435_IMU,
device_id=cfg.REALSENSE_D435_ID)
V.add(cam, inputs=[],
outputs=['cam/image_array', 'cam/depth_array',
'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'],
threaded=True)
else:
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
outputs = ['cam/image_array']
threaded = True
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
#rbx
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, record_location=cfg.SIM_RECORD_LOCATION, record_gyroaccel=cfg.SIM_RECORD_GYROACCEL, record_velocity=cfg.SIM_RECORD_VELOCITY, delay=cfg.SIM_ARTIFICIAL_LATENCY)
threaded = True
inputs = ['angle', 'throttle']
elif 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, framerate=cfg.CAMERA_FRAMERATE, vflip=cfg.CAMERA_VFLIP, hflip=cfg.CAMERA_HFLIP)
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)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "IMAGE_LIST":
from donkeycar.parts.camera import ImageListCamera
cam = ImageListCamera(path_mask=cfg.PATH_MASK)
elif cfg.CAMERA_TYPE == "LEOPARD":
from donkeycar.parts.leopard_imaging import LICamera
cam = LICamera(width=cfg.IMAGE_W, height=cfg.IMAGE_H, fps=cfg.CAMERA_FRAMERATE)
else:
raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
# add lidar
if cfg.USE_LIDAR:
from donkeycar.parts.lidar import RPLidar
if cfg.LIDAR_TYPE == 'RP':
print("adding RP lidar part")
lidar = RPLidar(lower_limit = cfg.LIDAR_LOWER_LIMIT, upper_limit = cfg.LIDAR_UPPER_LIMIT)
V.add(lidar, inputs=[],outputs=['lidar/dist_array'], threaded=True)
if cfg.LIDAR_TYPE == 'YD':
print("YD Lidar not yet supported")
# Donkey gym part will output position information if it is configured
if cfg.DONKEY_GYM:
if cfg.SIM_RECORD_LOCATION:
outputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte']
if cfg.SIM_RECORD_GYROACCEL:
outputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z']
if cfg.SIM_RECORD_VELOCITY:
outputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z']
V.add(cam, inputs=inputs, outputs=outputs, threaded=threaded)
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT, mode=cfg.WEB_INIT_MODE)
V.add(ctr,
inputs=['cam/image_array', 'tub/num_records'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
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
if cfg.CONTROLLER_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATController
ctr = RoboHATController(cfg)
elif "custom" == cfg.CONTROLLER_TYPE:
#
# custom controller created with `donkey createjs` command
#
from my_joystick import MyJoystickController
ctr = MyJoystickController(
throttle_dir=cfg.JOYSTICK_THROTTLE_DIR,
throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
ctr.set_deadzone(cfg.JOYSTICK_DEADZONE)
else:
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
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
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
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.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 and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg), inputs=['user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc"],
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
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.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 self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
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():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import IMU
imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG)
V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True)
# Use the FPV preview, which will show the cropped image output, or the full frame.
if cfg.USE_FPV:
V.add(WebFpv(), inputs=['cam/image_array'], 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 cfg.USE_LIDAR:
inputs = ['cam/image_array', 'lidar/dist_array']
elif cfg.HAVE_ODOM:
inputs = ['cam/image_array', 'enc/speed']
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']
elif cfg.USE_LIDAR:
inputs = ['cam/image_array', 'lidar/dist_array']
else:
inputs=['cam/image_array']
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)) )
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 Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
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)) )
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there 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):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running")
V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running")
outputs=['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs,
outputs=outputs,
run_condition='run_pilot')
if cfg.STOP_SIGN_DETECTOR:
from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector
V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE, cfg.STOP_SIGN_SHOW_BOUNDING_BOX), inputs=['cam/image_array', 'pilot/throttle'], outputs=['pilot/throttle', 'cam/image_array'])
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle if pilot_angle else 0.0, user_throttle
else:
return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0
V.add(DriveMode(),
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher,
inputs=['user/mode', 'throttle'],
outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK":
pass
elif 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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
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 == "DC_TWO_WHEEL_L298N":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, L298N_HBridge_DC_Motor
left_motor = L298N_HBridge_DC_Motor(cfg.HBRIDGE_L298N_PIN_LEFT_FWD, cfg.HBRIDGE_L298N_PIN_LEFT_BWD, cfg.HBRIDGE_L298N_PIN_LEFT_EN)
right_motor = L298N_HBridge_DC_Motor(cfg.HBRIDGE_L298N_PIN_RIGHT_FWD, cfg.HBRIDGE_L298N_PIN_RIGHT_BWD, cfg.HBRIDGE_L298N_PIN_RIGHT_EN)
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'], threaded=True)
V.add(motor, inputs=["throttle"])
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM":
from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM
steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN, freq=cfg.STEERING_PWM_FREQ, inverted=cfg.STEERING_PWM_INVERTED)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN, freq=cfg.THROTTLE_PWM_FREQ, inverted=cfg.THROTTLE_PWM_INVERTED)
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'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
# OLED setup
if cfg.USE_SSD1306_128_32:
from donkeycar.parts.oled import OLEDPart
auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE
oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM, auto_record_on_throttle=auto_record_on_throttle)
V.add(oled_part, inputs=['recording', 'tub/num_records', 'user/mode'], outputs=[], threaded=True)
#add tub to save data
if cfg.USE_LIDAR:
inputs = ['cam/image_array', 'lidar/dist_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'nparray','float', 'float', 'str']
else:
inputs=['cam/image_array','user/angle', 'user/throttle', 'user/mode']
types=['image_array','float', 'float','str']
if cfg.USE_LIDAR:
inputs += ['lidar/dist_array']
types += ['nparray']
if cfg.HAVE_ODOM:
inputs += ['enc/speed']
types += ['float']
if cfg.TRAIN_BEHAVIORS:
inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]
types += ['int', 'str', 'vector']
if cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH:
inputs += ['cam/depth_array']
types += ['gray16_array']
if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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']
# rbx
if cfg.DONKEY_GYM:
if cfg.SIM_RECORD_LOCATION:
inputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte']
types += ['float', 'float', 'float', 'float', 'float']
if cfg.SIM_RECORD_GYROACCEL:
inputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z']
types += ['float', 'float', 'float', 'float', 'float', 'float']
if cfg.SIM_RECORD_VELOCITY:
inputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z']
types += ['float', 'float', 'float']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
if cfg.HAVE_PERFMON:
from donkeycar.parts.perfmon import PerfMonitor
mon = PerfMonitor(cfg)
perfmon_outputs = ['perf/cpu', 'perf/mem', 'perf/freq']
inputs += perfmon_outputs
types += ['float', 'float', 'float']
V.add(mon, inputs=[], outputs=perfmon_outputs, threaded=True)
# do we want to store new records into own dir or append to existing
tub_path = TubHandler(path=cfg.DATA_PATH).create_tub_path() if \
cfg.AUTO_CREATE_NEW_TUB else cfg.DATA_PATH
tub_writer = TubWriter(tub_path, inputs=inputs, types=types, metadata=meta)
V.add(tub_writer, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
# Telemetry (we add the same metrics added to the TubHandler
if cfg.HAVE_MQTT_TELEMETRY:
telem_inputs, _ = tel.add_step_inputs(inputs, types)
V.add(tel, inputs=telem_inputs, outputs=["tub/queue_size"], threaded=True)
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
if cfg.DONKEY_GYM:
print("You can now go to http://localhost:%d to drive your car." % cfg.WEB_CONTROL_PORT)
else:
print("You can now go to <your hostname.local>:%d to drive your car." % cfg.WEB_CONTROL_PORT)
elif isinstance(ctr, JoystickController):
print("You can now move your joystick to drive your car.")
ctr.set_tub(tub_writer.tub)
ctr.print_controls()
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def build_recording(self):
#Ai Recording
if self.cfg.RECORD_DURING_AI and self.cfg.AUTO_RECORD_ON_THROTTLE:
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
self.vehicle.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode'
]
types = ['image_array', 'float', 'float', 'str']
if self.cfg.TRAIN_BEHAVIORS:
inputs += [
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
]
types += ['int', 'str', 'vector']
if self.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']
if self.cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
if self.cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
self.vehicle.add(ImgArrToJpg(),
inputs=['cam/image_array'],
outputs=['jpg/bin'])
self.vehicle.add(pub, inputs=['jpg/bin'])
if self.cfg.DONKEY_GYM_INFO:
inputs += ['sim/info']
types += ['dict']
th = TubHandler(path=self.cfg.DATA_PATH,
name_format="{year}{month:02}{day:02}_{num}.{tub}",
short_year=False)
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=self.meta)
self.vehicle.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
self.controller.set_tub(tub)
if self.cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
self.vehicle.parts.pop(
) # TODO Fix this! Don't assume last part is tub writer.
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=self.meta)
self.vehicle.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
self.controller.set_tub(tub)
self.controller.set_button_down_trigger('cross', new_tub_dir)
