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_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) cam = XRCamera(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() #kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) #steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) #throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def test_lambda_two_args(): l = Lambda(f2) b = l.run(1, 1) assert b == 3
def drive(cfg, model_path=None, model_type=None): V = dk.vehicle.Vehicle() assert (cfg.CAMERA_TYPE == 'PICAM') from donkeycar.parts.camera import PiCamera if (model_type == 'streamline'): cam = NickCamera(100, 20, True) else: cam = NickCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, grayscale=False) V.add(cam, outputs=['cam/image_array'], threaded=True) cont_class = MyJoystickController ctr = cont_class(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) def pilot_condition(mode): return not (mode == 'user') pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) if model_path: if (model_type == 'streamline'): kl = KerasStreamline() else: kl = dk.utils.get_model_by_type(model_type, cfg) assert ('.h5' in model_path) start = time.time() print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start))) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) assert (cfg.DRIVE_TRAIN_TYPE == 'SERVO_ESC') from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'float', 'float', 'str'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, outputs=['tub/num_records'], run_condition='recording') print('you can now move your joystick to drive your car') ctr.set_tub(tub) V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_tx=False): global myConfig global throttle global ctr global V ''' Start the drive loop Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' # Initialize car V = dk.vehicle.Vehicle() configCtrl = ConfigController(cfg.CONFIG_PATH) logger = logging.getLogger(myConfig['DEBUG']['PARTS']['MAIN']['NAME']) logger.setLevel(CONFIG2LEVEL[myConfig['DEBUG']['PARTS']['MAIN']['LEVEL']]) V.add(configCtrl, threaded=True) def get_tsc(): return int(round(time.time() * 1000)) logger.info("Init timestamper") get_tsc_part = Lambda(get_tsc) V.add(get_tsc_part, outputs=['ms']) logger.info("Init Cam part") if cfg.USE_WEB_CAMERA: cam = Webcam(resolution=cfg.CAMERA_RESOLUTION, fps=cfg.CAMERA_FPS, framerate=cfg.CAMERA_FRAMERATE) else: cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) logger.info("Init Controller part") if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: # modify max_throttle closer to 1.0 to have more power # modify steering_scale lower than 1.0 to have less responsive steering ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE, throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS, steering_axis=cfg.JOYSTICK_STEERING_AXIS, btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON, btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON, btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON, btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON, btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON, btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON, btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON, btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON, btn_toggle_const_throttle=cfg. JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON, verbose=cfg.JOYSTICK_VERBOSE) V.add( ctr, inputs=['cam/image_array', 'pilot/annoted_img'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) elif use_tx or cfg.USE_TX_AS_DEFAULT: #This is Tx controller (pilot Donkey from a RC Tx transmiter/receiver) ctr = TxController(verbose=cfg.TX_VERBOSE) V.add(ctr, inputs=[ 'user/mode', 'vehicle_armed', 'cam/image_array', 'pilot/annoted_img' ], outputs=[ 'user/angle', 'user/throttle', 'recording', 'lane', 'ch5', 'ch6', 'speedometer', 'sensor_left', 'sensor_right' ], threaded=True) actionctr = TxAuxCh() V.add(actionctr, inputs=['user/mode', 'vehicle_armed', 'ch5', 'ch6', 'recording'], outputs=['user/mode', 'vehicle_armed', 'flag', 'recording'], threaded=False) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add( ctr, inputs=['cam/image_array', 'pilot/annoted_img'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) if cfg.USE_THROTTLEINLINE: logger.info("Init throttleInLine part") throttleinline = ThrottleInLine(cfg.THROTTLEINLINE_ANGLE_MIN, cfg.THROTTLEINLINE_ANGLE_MAX) V.add(throttleinline, inputs=['cam/image_array'], outputs=['pilot/throttle_boost', 'pilot/annoted_img'], threaded=True) logger.info("Init emergency part") emergencyCtrl = EmergencyController() V.add(emergencyCtrl, inputs=['user/mode'], outputs=['user/mode'], threaded=True) perfMngt = dumpPerf() V.add(perfMngt, inputs=['user/mode'], threaded=False) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True logger.info("Init pilot part") pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) logger.info("Init Model part") # Run the pilot if the mode is not user and not Tx. if (myConfig['MODEL']['MODEL_IN_USE'] == 0): kl = KerasCategorical() if (myConfig['MODEL']['MODEL_IN_USE'] == 1): kl = KerasCategorical1() #kl = KerasLinear() if model_path: if (os.path.exists(model_path)): logger.info("IA : Load integrated model") kl.load(model_path) else: # Model reconstruction from JSON file logger.info("IA : Load Weights + Model Architecture model") kl.load2(model_path) if (myConfig['MODEL']['MODEL_IN_USE'] == 0): V.add(kl, inputs=['cam/image_array'], outputs=[ 'pilot/angle', 'pilot/throttle', 'pilot/fullspeed', 'pilot/lane', 'pilot/angle_bind' ], run_condition='run_pilot') if (myConfig['MODEL']['MODEL_IN_USE'] == 1): V.add(kl, inputs=['cam/image_array', 'speedometer'], outputs=[ 'pilot/angle', 'pilot/throttle', 'pilot/fullspeed', 'pilot/brake', 'pilot/angle_bind' ], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle, throttle_boost): if mode == 'user': return user_angle, user_throttle else: if cfg.USE_THROTTLEINLINE: if throttle_boost: pilot_throttle = pilot_throttle * cfg.THROTTLEINLINE_BOOST_FACTOR logger.debug("Apply Boost") if mode == 'local_angle': return pilot_angle, user_throttle else: logger.debug( 'drive_mode: Pilot return angle={:01.2f} throttle={:01.2f}' .format(pilot_angle, pilot_throttle)) if (pilot_angle > myConfig['POST_PILOT']['STEERING_TRIM_RIGHT_THRES']): pilot_angle = pilot_angle * myConfig['POST_PILOT'][ 'STEERING_TRIM_RIGHT_FACTOR'] if (pilot_angle < -myConfig['POST_PILOT']['STEERING_TRIM_LEFT_THRES']): pilot_angle = pilot_angle * myConfig['POST_PILOT'][ 'STEERING_TRIM_LEFT_FACTOR'] return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'pilot/throttle_boost' ], outputs=['angle', 'throttle']) if cfg.USE_PWM_ACTUATOR: logger.info("Init Actuator part") if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1: steering_controller = ctr else: steering_controller = PCA9685(channel=cfg.STEERING_CHANNEL, busnum=cfg.STEERING_I2C_BUS) steering = PWMSteering(controller=steering_controller) if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1: throttle_controller = ctr else: throttle_controller = PCA9685(channel=cfg.THROTTLE_CHANNEL, busnum=cfg.THROTTLE_I2C_BUS) throttle = PWMThrottle(controller=throttle_controller) V.add(steering, inputs=['angle']) V.add(throttle, inputs=[ 'throttle', 'user/mode', 'vehicle_armed', 'pilot/fullspeed', None, 'pilot/lane', 'sensor_left', 'sensor_right' ]) if cfg.BATTERY_USE_MONITOR: logger.info("Init Battery Monitor part") battery_controller = BatteryController(nbCells=cfg.BATTERY_NCELLS) V.add(battery_controller, outputs=['battery'], threaded=True) # add tub to save data inputs = [ 'cam/image_array', 'ms', 'user/angle', 'user/throttle', 'user/mode', 'pilot/angle', 'pilot/throttle', 'flag', 'speedometer', 'lane' ] types = [ 'image_array', 'int', 'float', 'float', 'str', 'numpy.float32', 'numpy.float32', 'str', 'float', 'int' ] logger.info("Init Tub Handler part") th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') if use_tx or cfg.USE_TX_AS_DEFAULT: fpv = FPVWebController() V.add(fpv, inputs=[ 'cam/image_array', 'pilot/annoted_img', 'user/angle', 'user/throttle', 'user/mode', 'pilot/angle', 'pilot/throttle', 'pilot/throttle_boost', 'pilot/fullspeed', 'pilot/angle_bind' ], threaded=True) logger.info("Start main loop") # run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') driver = ArduinoDriver() V.add(driver, inputs=['user/mode', 'pilot/angle', 'pilot/throttle'], outputs=['user/angle', 'user/throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ 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 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): ''' 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.")
print('Added PS3 Part') th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) def drive_mode(mode, switch, user_throttle, pilot_throttle): if mode == 'user': return switch, pilot_throttle elif mode == 'local_angle': return switch, user_throttle else: return switch, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'recording', 'user/throttle', 'PWM_Throttle'], outputs=['switch', 'throttle']) print('Added switch command part') filterImage = Cam.ImageConvandFilter() V.add(filterImage, inputs=["camera/image"], outputs=["x", "y", "Current_radius", "shape"], threaded=False) print('Added Filtering Part') Controller = FF.Controller() V.add(Controller, inputs=["x", "y", "Current_radius", "shape"],
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()
''' vals = obj[1:] x = round(obj[0] * 30.0) ret = [] i = 0 for val in vals: coord = (x, val * (h / 4.) + (h / 2.)) color = [0, 0, 0] color[i] = 1 i += 1 ret.append((coord, color)) #a solid white center line. coord = (x, h / 2.0) color = (1.0, 1.0, 1.0) ret.append((coord, color)) return ret l = Lambda(condition_values) V.add(ZMQValueSub(name="test", ip=ip), outputs=["obj"]) V.add(l, inputs=["obj"], outputs=["values"]) V.add(Graph(res=(h, w, d)), inputs=["values"], outputs=["graph/img"]) V.add(CvImageView(), inputs=["graph/img"]) V.start(rate_hz=10)
def _get_sample_lambda(): def f(): return 1 f.update = f return Lambda(f)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ たくさんのパーツから作業用のロボットVehicleを構築します。 各パーツはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded` に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出します。 すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレート (デフォルト:20MHz)で順次更新され、各パーツが適時に処理を終了すると仮定して ループを無限に繰り返します。 パーツにはラベル付きの `inputs` と `outputs` があります。 フレームワークは、ラベル付き `outputs` の値を、 同じ名前の `inputs` を要求する別のパーツに渡します。 引数: cfg config.py を読み込んだオブジェクト model_path 学習済みモデルファイルのパス use_joystick ジョイスティックを使用するかどうかの真偽値 use_chaos 操舵に一定のランダム操作を加えるかどうかの真偽値 """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # このWebコントローラでは、ステアリング、スロットル、モードなどを管理する # Webサーバを作成 ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # パイロットモジュールを走らせるべきかどうかを毎回判別させるためのパーツ # この関数の結果の真偽値で自動運転パーツ(ConvLSTM2DPilot)を実行するかを決定させる def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. # 独自のパイロットに置き換え #kl = KerasCategorical() kl = ConvLSTM2DPilot() if model_path: print("start loading trained model file") kl.load(model_path) print("finish loading trained model file") V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # 実車のインプットとしてどの値を使うかのモード選択 def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def 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) 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. if (hasattr(cfg, 'ENGINE') and cfg.ENGINE == "mxnet"): import donkeycar.parts.mxnetpart as mxp kl = mxp.MxnetLinear() else: kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) #add tub to save data inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types=['image_array', 'float', 'float', 'str'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False): ''' 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. ''' # Do imports. from donkeycar.parts.transform import Lambda from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle from donkeycar.parts.datastore import TubHandler from donkeycar.parts.controller import LocalWebController, JoystickController from donkeyturbo.pilot import DTKerasPilot # Initialize a car. V = dk.vehicle.Vehicle() # Camera. from donkeyturbo.camera import DTPiCamera cam = DTPiCamera() V.add(cam, outputs=['cam/image_array'], threaded=True) # Controller. 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: 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. # NOTE(r7vme): Always use DTKerasPilot kl = DTKerasPilot(config=cfg.DT_PILOT_CONFIG) 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. V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive_vis(cfg, model_path=None, use_chaos=False): V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) top_view_transform = TopViewTransform(cfg.CAMERA_RESOLUTION) V.add(Lambda(top_view_transform.wrap), inputs=['cam/image_array'], outputs=['cam/image_array_proj']) V.add(kl, inputs=['cam/image_array_proj'], outputs=['pilot/angle', 'pilot/throttle']) ctr = LocalWebControllerVis(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array_proj', 'pilot/angle', 'pilot/throttle'], outputs=['user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) driver = ArduinoDriver() V.add(driver, inputs=['user/mode', 'pilot/angle', 'pilot/throttle'], outputs=['user/angle', 'user/throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) #Part to save multiple image arrays from camera class ImageArrays: def __init__(self): tmp = np.zeros((120, 160, 3)) self.images = [tmp for i in range(3)] def run(self, image): self.images.pop(0) self.images.append(image) return np.array(self.images) image_arrays = ImageArrays() V.add(image_arrays, inputs=['cam/image_array'], outputs=['cam/image_arrays']) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_arrays'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = CSICamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=False) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering ctr = PS4JoystickController( throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE ) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Experiment with different models here ## Default Categorical #kl = Keras_Categorical() #V.add(kl, # inputs=['cam/image_array'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') ## Default Categorical on Simple Model (Faster Inference) #kl = Keras_Simple_Categorical() #V.add(kl, # inputs=['cam/image_array'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') ## Frame Stacking (GrayScale Frames) on Default Model # Frame stacking img_stack = ImgStack() V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack']) kl = Keras_StackedFrame_Categorical() V.add(kl, inputs=['cam/frame_stack'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # LSTM Model # Time Sequence Frames #ts_frames = TimeSequenceFrames() #V.add(ts_frames, inputs=['cam/image_array'], outputs=['cam/ts_frames']) #kl = Keras_LSTM_Categorical() #V.add(kl, # inputs=['cam/ts_frames'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') # DQN Q Network Model (Can be zero shot or finetuned) # Frame stacking #img_stack = ImgStack() #V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack']) #kl = Keras_Q_Categorical() #V.add(kl, # inputs=['cam/frame_stack'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': #return pilot_angle, user_throttle return pilot_angle, pilot_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' #Initialize car V = dk.vehicle.Vehicle() 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 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) 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 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)
def drive(cfg, model_path=None, use_joystick=False): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' #Initialize car V = dk.vehicle.Vehicle() #add led part #right_led = rgb_led_0(red_channel = 5, green_channel = 6, blue_channel = 7) center_led = rgb_led(red_channel = 9, green_channel = 10, blue_channel = 11) #left_led = rgb_led_0(red_channel = 13, green_channel = 14, blue_channel = 15) #turn_signals = turn_signal(left_led = left_led, right_led = right_led) status_led = status_indicator(status_led = center_led) #V.add(right_led, outputs=['none']) V.add(center_led, outputs=['none']) #V.add(left_led, outputs=['none']) #add pi_perfchecker loop_time = driveLoopTime() #loop_time.console=True #core_temp = coreTemp() V.add(loop_time,inputs = ['timein'], outputs = ['timein','displaytext']) #V.add(core_temp) #throtled_status = throttled() #V.add(throtled_status, outputs=['displaytext'],threaded=True) #pitft=display_text() #V.add(pitft, inputs=['displaytext'], outputs=['pitft/screen'], threaded=True) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) #boostBright=ImgBoostBright() #V.add(boostBright, inputs=['cam/image_array'], outputs=['cam/image_array']) #ncs_gn = googlenet(basedir=cfg.MODELS_PATH, debug=True) #V.add(ncs_gn, inputs=['cam/image_array'],outputs=['ncs/image_array', 'classificaiton'],threaded=True) #ncs_inception = inception(basedir=cfg.MODELS_PATH, probability_threshold=0.01, debug=True) #V.add(ncs_inception, inputs=['cam/image_array'],outputs=['ncs/image_array', 'classificaiton'],threaded=True) ncs_ty = tinyyolo(basedir = cfg.MODELS_PATH, draw_on_img = True, probability_threshold = 0.07,debug=False) V.add(ncs_ty, inputs=['cam/image_array'],outputs=['ncs/image_array','ncs/found_objs'],threaded=True) loop_time_display = ImgPutText() #V.add(loop_time_display,inputs=['ncs/image_array','displaytext'], outputs=['ncs/image_array']) #classify = ImgPutText() #V.add(classify,inputs=['cam/image_array','classificaiton'],outputs=['ncs/image_array']) # draw a line showing where training input is cropped #l1 = ImgDrawLine() #l1.start = (0,39) #l1.end = (160,39) #l1.color = (0,255,0) #l1.width=10 #V.add(l1, inputs=['ncs/image_array'],outputs=['ncs/image_array']) #driverInfo = ImgPutInfo() #throttleText.text='SPD:' #V.add(driverInfo, inputs=['cam/image_array','throttle', 'angle'],outputs=['cam/image_array']) #greyScale = ImgGreyscale() #imgCanny = ImgCanny() # moving croping to network input layer rather than image.. # Allows me to see and process the entire image but only conside # the bottom third for stearing input to the network.. #imgCrop = ImgCrop(top=40,bottom=0,left=0,right=0) #V.add(imgCrop, inputs=['cam/image_array'],outputs=['cam/image_array']) #V.add(greyScale, inputs=['cam/image_array'],outputs=['cam/image_array']) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering # auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,throttle_axis='rz',steering_axis='x') ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE, throttle_axis='y', steering_axis='x', panning_axis='z', tilting_axis='rz') ctr_webview = LocalWebController() V.add(ctr_webview, #inputs=['ncs/image_array'], inputs=['ncs/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() #ctr.auto_record_on_throttle = False V.add(ctr, inputs=['ncs/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording','user/pan','user/tilt'], threaded=True) # add the LED controller part #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) #Run the pilot if the mode is not user. #kl = KerasCroppedCategorical() # Reverting to non cropped.. adding pan\tilt to camerra kl = KerasCroppedCategorical() if model_path: kl.load(model_path) kResizeImg= ImgResize() V.add(kResizeImg, inputs=['cam/image_array'], outputs=['cam/image_array']) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle #return pilot_angle, 0.30 else: #overite throttle #return pilot_angle, pilot_throttle #return pilot_angle, 0.80 return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) panning_controller = PCA9685(cfg.PAN_CHANNEL) panning = PWMPanning(controller=panning_controller, left_pulse=cfg.PAN_LEFT_PWM, zero_pulse=cfg.PAN_CENTER_PWM, right_pulse=cfg.PAN_RIGHT_PWM) tilting_controller = PCA9685(cfg.TILT_CHANNEL) tilting = PWMThrottle(controller=tilting_controller, max_pulse=cfg.TILT_UP_PWM, zero_pulse=cfg.TILT_DRIVING_PWM, min_pulse=cfg.TILT_DOWN_PWM) wagging_controller = PCA9685(cfg.TAIL_CHANNEL) wagging = PWMThrottle(controller=wagging_controller, max_pulse=cfg.TAIL_UP_PWM, zero_pulse=cfg.TAIL_CENTER_PWM, min_pulse=cfg.TAIL_DOWN_PWM) #throttleText.text = throttle # add govenor part here. Governer has overridding power when drive mode is pilot #break_for_dog = break_for('dog', .3) #V.add(break_for_dog, inputs=['user/mode','angle', 'throttle','ncs/found_objs'], outputs=['angle','throttle']) #V.add(turn_signals, inputs=['angle']) #'user/angle', 'user/throttle', 'user/mode', 'recording' V.add(status_led, inputs=['user/mode', 'recording']) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) V.add(panning, inputs=['user/pan']) V.add(tilting, inputs=['user/tilt']) #V.add(wagging, inputs=['throttle']) #add tub to save data #inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] # need local_angle and local_pilot to save values to tub inputs=['cam/image_array', 'angle', 'throttle', 'user/mode'] types=['image_array', 'float', 'float', 'str'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False, use_fuzzy=False): from donkeycar.parts.camera import PiCamera from donkeycar.parts.ultrasonic import Ultrasonic, MockUltrasonic, CacheUltrasonicClient from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle from donkeycar.parts.obstacle import Obstacle ''' 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_front = PiCamera(resolution=cfg.CAMERA_RESOLUTION, name = 'front') V.add(cam_front, outputs=['cam/image_array'], threaded=True) us_front = CacheUltrasonicClient(gpio_trigger=cfg.ULTRASONIC_FRONT_TRIGGER, gpio_echo=cfg.ULTRASONIC_FRONT_ECHO, poll_delay = 0.05, name='front') V.add(us_front, outputs=['ultrasonic_front/distance'], threaded=True) us_front_left = CacheUltrasonicClient(gpio_trigger=cfg.ULTRASONIC_FRONT_LEFT_TRIGGER, gpio_echo=cfg.ULTRASONIC_FRONT_LEFT_ECHO, poll_delay = 0.05, name='front_left') V.add(us_front_left, outputs=['ultrasonic_front_left/distance'], threaded=True) us_front_right = CacheUltrasonicClient(gpio_trigger=cfg.ULTRASONIC_FRONT_RIGHT_TRIGGER, gpio_echo=cfg.ULTRASONIC_FRONT_RIGHT_ECHO, poll_delay = 0.05, name='front_right') V.add(us_front_right, outputs=['ultrasonic_front_right/distance'], 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', 'ultrasonic_front/distance', 'ultrasonic_front_left/distance', 'ultrasonic_front_right/distance', 'pilot/action', 'pilot/angle', 'pilot/throttle'], 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']) #Obstacle detection ob = Obstacle() V.add(ob, inputs=['cam/image_array', 'ultrasonic_front/distance', 'ultrasonic_front_left/distance', 'ultrasonic_front_right/distance'], outputs=['pilot/action']) #Run the pilot if the mode is not user. if use_fuzzy: kl = KerasFuzzyAndUltrasonicSensors() else: kl = KerasUltrasonicSensors() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array', 'ultrasonic_front/distance', 'ultrasonic_front_left/distance', 'ultrasonic_front_right/distance', 'pilot/action'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot', threaded = True) #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) 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', 'ultrasonic_front/distance', 'ultrasonic_front_left/distance', 'ultrasonic_front_right/distance', 'user/angle', 'user/throttle', 'user/mode'] types=['image_array', 'float', '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') #run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ (手動・自動)運転する。 多くの部品(part)から作業用のロボット車両を構築する。 各partはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded`に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出す。 すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレートで順次更新され、 各partが適時に処理を終了すると仮定する。 partには名前付きの出力と入力が存在する(どちらかがない場合や複数存在する場合もある)。 Vehicle フレームワークは、名前付き出力を同じ名前の入力を要求するpartに渡すことを処理する。 引数 cfg 個別車両設定オブジェクト、`config.py`がロードされたオブジェクト。 model_path 自動運転時のモデルファイルパスを指定する(デフォルトはNone)。 use_joystick ジョイスティックを使用するかどうかの真偽値(デフォルトはFalse)。 use_chaos 手動運転中に周期的なランダム操舵を加えるかどうかの真偽値(デフォルトはFalse)。 """ # Vehicle オブジェクトの生成 V = dk.vehicle.Vehicle() # Timestamp part の生成 clock = Timestamp() # Timestamp part をVehicleループへ追加 # 入力: # なし # 出力: # 'timestamp' 現在時刻 V.add(clock, outputs=['timestamp']) # PiCamera part の生成 cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) # 別スレッド実行される PiCamera part をVehicleループへ追加 # 入力: # なし # 出力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ V.add(cam, outputs=['cam/image_array'], threaded=True) # manage.py デフォルトのジョイスティックpart生成 if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE, # steering_scale=cfg.JOYSTICK_STEERING_SCALE, # throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS, # auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) # ジョイスティック part の生成 from elecom.part import JoystickController ctr = JoystickController(config_path='elecom/jc-u3912t.yml') else: # ステアリング、スロットル、モードなどを管理するWebサーバを作成する # Web Controller part の生成 ctr = LocalWebController(use_chaos=use_chaos) # 別スレッド実行される Web Controller part もしくはジョイスティック part をVehiecleループへ追加 # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'recording' tubデータとして保管するかどうかの真偽値 V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # オートパイロットモジュールを実行すべきかどうかを確認する関数を定義する。 def pilot_condition(mode): ''' オートパイロットモジュール実行判定関数。 引数で指定されたモードを判別して、オートパイロットモジュールを実行するべきかどうか 真偽値を返却する関数。 引数 mode Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) 戻り値 boolean オートパイロットモジュールを実行するかどうかの真偽値 ''' print('mode=' + mode) if mode == 'user': # 全手動時のみ実行しない return False else: return True # オートパイロットモジュール実行判定関数を part 化したオブジェクトを生成 pilot_condition_part = Lambda(pilot_condition) # オートパイロットモジュール実行判定 part を Vehiecle ループへ追加 # 入力: # 'user/mode' Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) # 出力: # 'run_pilot' オートパイロットモジュールを実行するかどうかの真偽値 V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Userモードでない場合、オートパイロットを実行する # CNNベースの線形回帰モデル(オートパイロット) part を生成する。 kl = KerasLinear() # 関数driveの引数 model_part 指定がある場合 if model_path: # 学習済みモデルファイルを読み込む kl.load(model_path) # run_condition が真の場合のみ実行されるオートパイロット part をVehicleループへ追加する # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # 車両にどの値を入力にするかを判別する def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): ''' 引数で指定された項目から、車両への入力とするステアリング値、スロットル値を確定する関数。 引数 mode Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) user_angle Web/Joystickにより手動指定した次に取るべきステアリング値 user_throttle Web/Joystickにより手動指定した次に取るべきスロットル値 pilot_angle オートパイロットが指定した次に取るべきステアリング値 pilot_throttle オートパイロットが指定した次に取るべきスロットル値 戻り値 angle 車両への入力とするステアリング値 throttle 車両への入力とするスロットル値 ''' if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle # 車両にどの値を入力にするかを判別する関数を part 化したオブジェクトを生成 drive_mode_part = Lambda(drive_mode) # 車両にどの値を入力にするかを判別する part を Vehicle ループへ追加 # 入力 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 # 戻り値 # 'angle' 車両への入力とするステアリング値 # 'throttle' 車両への入力とするスロットル値 V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) # 実車両のステアリングサーボを操作するオブジェクトを生成 steering_controller = PCA9685(cfg.STEERING_CHANNEL) # 実車両へステアリング値を指示する part を生成 steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) # 実車両のスロットルECSを操作するオブジェクトを生成 throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) # 実車両へスロットル値を指示する part を生成 throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) # 実車両へステアリング値を指示する part を Vehiecle ループへ追加 # 入力: # 'angle' 車両への入力とするステアリング値 # 出力: # なし(実車両の操舵へ) V.add(steering, inputs=['angle']) # 実車両へスロットル値を指示する part を Vehiecle ループへ追加 # 入力: # 'throttle' 車両への入力とするスロットル値 # 出力: # なし(実車両のスロットル操作へ) V.add(throttle, inputs=['throttle']) # 保存データを tub ディレクトリに追加 inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # 複数 tub ディレクトリの場合 # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # 単一 tub ディレクトリの場合 # tub ディレクトリへ書き込む part を生成 tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) # 'recording'が正であれば tub ディレクトリへ書き込む part を Vehiecle ループへ追加 # 入力 # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'timestamp' 現在時刻 V.add(tub, inputs=inputs, run_condition='recording') # テレメトリーデータの送信 #tele = PubTelemetry('iotf/emperor.ini', pub_count=20*5) #V.add(tele, inputs=['cam/image_array', 'user/mode', 'user/angle', 'user/throttle', # 'pilot/angle', 'pilot/throttle', 'angle', 'throttle']) # Vehicle ループを開始 V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def _get_sample_lambda(): def f(): return 1 return Lambda(f)
def drive(cfg, model_path=None, use_joystick=False, use_tx=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) 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: # 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, 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): V = dk.vehicle.Vehicle() V.mem.put(['square/angle', 'square/throttle'], (100, 100)) # display square box given by cooridantes. cam = SquareBoxCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, inputs=['square/angle', 'square/throttle'], outputs=['cam/image_array']) # display the image and read user values from a local web controller ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_contion only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # See if we should even run the pilot module. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'pilot_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) clock = Timestamp() V.add(clock, outputs=['timestamp']) # transform angle and throttle values to coordinate values def f(x): return int(x * 100 + 100) l = Lambda(f) V.add(l, inputs=['user/angle'], outputs=['square/angle']) V.add(l, inputs=['user/throttle'], outputs=['square/throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'square/angle', 'square/throttle', 'user/mode', 'timestamp' ] types = [ 'image_array', 'float', 'float', 'float', 'float', 'float', 'float', 'str', 'str' ] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle for 20 seconds V.start(rate_hz=50, max_loop_count=10000)
def test_lambda_one_arg(): l = Lambda(f) b = l.run(1) assert b == 2
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" #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, iCam = 0) camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, 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, iCam = 0) camB = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, 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: 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) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam cam = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H) else: raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering ctr = JoystickController(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE, controller_type=cfg.CONTROLLER_TYPE) 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, 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: if led_cond.last_num_rec_print != num_records: print("recorded", num_records, "records") led_cond.last_num_rec_print = num_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.last_num_rec_print = 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 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'] if model_path: kl = dk.utils.get_model_by_type(model_type, cfg) 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']) #Drive train setup if cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC": from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE": from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT, cfg.HBRIDGE_PIN_RIGHT) throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD) right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD) two_wheel_control = TwoWheelSteeringThrottle() V.add(two_wheel_control, inputs=['throttle', 'angle'], outputs=['left_motor_speed', 'right_motor_speed']) V.add(left_motor, inputs=['left_motor_speed']) V.add(right_motor, inputs=['right_motor_speed']) #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)