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