def __init__(self, dev_conf_path=None, app_conf_path=None): from donkeypart_tub_loader import TubLoader, TubPrinter tubitems = [ 'cam/image_array', 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'angle', 'throttle' ] self.vehicle = dk.vehicle.Vehicle() self.vehicle.add(TubLoader('tub'), outputs=tubitems) self.vehicle.add(PubTelemetry(dev_conf_path=dev_conf_path, pub_count=0), inputs=tubitems) self.vehicle.add(TubPrinter(), inputs=tubitems) def wait(): delay = 3 #print('wait {} seconds'.format(str(delay))) sleep(delay) self.vehicle.add(Lambda(wait)) self.vehicle.add(SubTelemetry(app_conf_path=app_conf_path, dev_conf_path=dev_conf_path), outputs=tubitems) self.vehicle.add(TubPrinter(), inputs=tubitems)
def add_steering_model(vehicle, steering_path, lidar_clip, camera_input, lidar_input, steering_model_output): # Process image image_transformation = ImageTransformation([ image_transformer.normalize, image_transformer.generate_crop_fn(0, 40, 160, 80), image_transformer.edges ]) vehicle.add(image_transformation, inputs=[camera_input], outputs=['ai/_image']) # Process lidar clip_max = lidar_clip def lidar_preprocess(lidar): lidar = tf.clip_by_value(lidar, clip_value_min=0, clip_value_max=clip_max) lidar = 1 - (lidar / clip_max) return lidar lidar_preprocess = Lambda(lidar_preprocess) vehicle.add(lidar_preprocess, inputs=[lidar_input], outputs=['lidar/processed']) # Predict on transformed image steering_model = OneOutputModel() steering_model.load(steering_path) vehicle.add(steering_model, inputs=['ai/_image', 'lidar/processed'], outputs=[steering_model_output])
def vehicle(): v = dk.Vehicle() def f(): return 1 l = Lambda(f, outputs=['test_out']) v.register(l) return v
def test_vehicle_register_part(vehicle): def f_mock(input): pass vehicle.register(Lambda(f_mock, inputs=["input1"], outputs=["output1"])) logging.info(str(vehicle.parts)) assert len(vehicle.parts) == 2 assert vehicle.parts[1]['inputs'] == ['input1'] assert vehicle.parts[1]['outputs'] == ['output1']
def vehicle(): v = dk.Vehicle() def f(): return 1 l = Lambda(f) v.add(l, outputs=['test_out']) return v
def test_add_part(): v = dk.Vehicle() def f(): return 1 l = Lambda(f) v.add(l, outputs=['test_out']) assert len(v.parts) == 1
def _configure(self, cfg): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ self._configure_camera(cfg) self.register(ComponentRoadPart2()) # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. self.register(LocalWebController()) self._configure_arduino(cfg) self._configure_angle_part(cfg) self._configure_throttle_controller(cfg) # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == DRIVE_MODE_USER: return user_angle, user_throttle elif mode == DRIVE_MODE_LOCAL_ANGLE: return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode, inputs=[USER_MODE, USER_ANGLE, USER_THROTTLE, PILOT_ANGLE, PILOT_THROTTLE], outputs=[ANGLE, THROTTLE]) self.register(drive_mode_part) self._configure_car_hardware(cfg) self._configure_indicators(cfg) logger.info("You can now go to <your pi ip address>:8887 to drive your car.")
import donkeycar as dk from donkeycar.parts.actuator import ServoBlaster, PWMSteering from donkeycar.parts.transform import Lambda import math cfg = dk.load_config() V = dk.Vehicle() V.timer = time.time() def steering_control(): t = time.time() - V.timer val = math.cos(t) return val controls = Lambda(steering_control) V.add(controls, outputs=["angle"]) 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) V.add(steering, inputs=['angle']) V.start(rate_hz=cfg.DRIVE_LOOP_HZ)
def drive(cfg, model_path=None, model_wrapper=None, debug=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() if not debug: from donkeycar.parts.camera import PiCamera cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) else: print("Debug : ignoring camera.") def fake_cam(): from PIL import Image import numpy as np dir_path = os.path.dirname(os.path.realpath(__file__)) img = Image.open(os.path.join(dir_path, 'img.jpg')) a = np.array(img) return a fake_cam_part = Lambda(fake_cam) V.add(fake_cam_part, outputs=['cam/image_array']) ctr = LocalWebController(use_chaos=False) 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 model_path: model = load_model(model_path) model_wrapper_class = getattr(model_wrappers, model_wrapper) model_instance = model_wrapper_class(model=model) V.add(model_instance, inputs=['cam/image_array'], outputs=['pilot/angle1', 'pilot/throttle1'], run_condition='run_pilot') def bound_throttle(pilot_angle1, pilot_throttle1): if pilot_angle1 is None: pilot_angle1 = 0 if pilot_throttle1 is None: pilot_throttle1 = 0 pilot_throttle = min(1, pilot_throttle1) pilot_throttle = max(0, pilot_throttle) return pilot_angle1, pilot_throttle bound_throttle_part = Lambda(bound_throttle) V.add(bound_throttle_part, inputs=['pilot/angle1', 'pilot/throttle1'], outputs=['pilot/angle', 'pilot/throttle']) # add tub to save data set_path = os.path.join(ROOT_PATH, "set_{}".format(datetime.now().strftime("%Y%m%d%H%M%S"))) os.makedirs(set_path) inputs = ['cam/image_array', 'user/angle', 'user/throttle'] # 'user/mode' rec = Recorder(path=set_path) V.add(rec, inputs=inputs, run_condition='recording') # 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 not debug: 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']) else: print("Debug : ignoring controller part.") # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def test_lambda_one_arg(): l = Lambda(f) b = l.run(1) assert b == 2
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 _get_sample_lambda(): def f(): return 1 f.update = f return Lambda(f)
def drive(cfg, model_path=None): V = dk.vehicle.Vehicle() V.mem.put(['square/angle', 'square/throttle'], (100, 100)) # display square box given by cooridantes. cam = SquareBoxCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, inputs=['square/angle', 'square/throttle'], outputs=['cam/image_array']) # display the image and read user values from a local web controller ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_contion only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # See if we should even run the pilot module. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'pilot_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) clock = Timestamp() V.add(clock, outputs=['timestamp']) # transform angle and throttle values to coordinate values def f(x): return int(x * 100 + 100) l = Lambda(f) V.add(l, inputs=['user/angle'], outputs=['square/angle']) V.add(l, inputs=['user/throttle'], outputs=['square/throttle']) # add tub to save data inputs=['cam/image_array', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'square/angle', 'square/throttle', 'user/mode', 'timestamp'] types=['image_array', 'float', 'float', 'float', 'float', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle for 20 seconds V.start(rate_hz=50, max_loop_count=10000)
def drive(cfg, model_path=None, use_joystick=False, use_reader=False): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' #Initialize car V = dk.vehicle.Vehicle() cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) elif use_reader or cfg.USE_READER_AS_DEFAULT: pi = pigpio.pi() ctr = reader(pi, 12, 18, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) #Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) #add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'float', 'float', 'str'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def 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, 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_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ memory = RedisMemory() V = dk.vehicle.Vehicle(mem=memory) clock = Timestamp() V.add(clock, outputs='timestamp') # ***** CAMERA ***** print("Starting camera") cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) # ***** Web Controller ***** print("Starting web controller") ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # ***** SPEKTRUM/MOVE32 REMOTE ***** print("Starting Spektrum/Move32") #rc = SpektrumRemoteReceiver(cfg.SPEKTRUM_OFFSET, cfg.SPEKTRUM_SCALE, cfg.SPEKTRUM_DEFAULT, cfg.SPEKTRUM_SERIALPORT) rc = Move32Receiver(cfg.MOVE32_OFFSET, cfg.MOVE32_SCALE, cfg.MOVE32_DEFAULT, cfg.MOVE32_SERIALPORT, cfg.MOVE32_RXTYPE, cfg.MOVE32_RXAUTO, cfg.MOVE32_TIMEOUT) V.add(rc, threaded=True, outputs=['rc0', 'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7']) def rc_convert_func(*args): angle = args[0] throttle = args[1] mode = 'manual' if args[ 2] > 0.3 else 'auto' if args[2] < -0.3 else 'auto_angle' recording = args[3] <= 0.3 return angle, throttle, mode, recording V.add(Lambda(rc_convert_func), inputs=['rc0', 'rc2', 'rc5', 'rc4'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording']) # ***** user/mode -> run_pilot ***** V.add(Lambda(lambda mode: mode.lower() != 'manual'), inputs=['user/mode'], outputs=['run_pilot']) # ***** cam/image_array -> pilot/angle,pilot_throttle ***** # Run the pilot if the mode is not user. print("Starting KerasCategorical") kl = KerasCategorical() if model_path: print("Loading model...") kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # ***** user/*, pilot/* -> angle, throttle ***** # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'manual': return user_angle, user_throttle elif mode == 'auto_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) # ***** throttle, angle -> motor_left, motor_right ***** ackermann_to_diff_converter = AckermannToDifferentialDriveConverter( cfg.ACKERMANN_LENGTH, cfg.ACKERMANN_WIDTH) V.add(ackermann_to_diff_converter, inputs=['angle', 'throttle'], outputs=['motor_left', 'motor_right']) # ***** motor_left, motor_right -> DRIVE ***** motors_part = DifferentialDriveActuator_MotorHat( cfg.MOTORHAT_ADDR, cfg.MOTORHAT_LEFT_FRONT_ID, cfg.MOTORHAT_LEFT_REAR_ID, cfg.MOTORHAT_RIGHT_FRONT_ID, cfg.MOTORHAT_RIGHT_REAR_ID) V.add(motors_part, inputs=['motor_left', 'motor_right']) # ***** output debug data ***** debug_keys = [ 'user/mode', 'recording', 'run_pilot', "angle", "throttle", "motor_left", "motor_right", ] #'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7', 'rc8'] def debug_func(*args): print(args[0], " ", args[1], " ", args[2], " ".join("{:5.2f}".format(e) for e in args[3:])) V.add(Lambda(debug_func), inputs=debug_keys) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = CSICamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=False) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering ctr = PS4JoystickController( throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE ) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Experiment with different models here ## Default Categorical #kl = Keras_Categorical() #V.add(kl, # inputs=['cam/image_array'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') ## Default Categorical on Simple Model (Faster Inference) #kl = Keras_Simple_Categorical() #V.add(kl, # inputs=['cam/image_array'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') ## Frame Stacking (GrayScale Frames) on Default Model # Frame stacking img_stack = ImgStack() V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack']) kl = Keras_StackedFrame_Categorical() V.add(kl, inputs=['cam/frame_stack'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # LSTM Model # Time Sequence Frames #ts_frames = TimeSequenceFrames() #V.add(ts_frames, inputs=['cam/image_array'], outputs=['cam/ts_frames']) #kl = Keras_LSTM_Categorical() #V.add(kl, # inputs=['cam/ts_frames'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') # DQN Q Network Model (Can be zero shot or finetuned) # Frame stacking #img_stack = ImgStack() #V.add(img_stack, inputs=['cam/image_array'], outputs=['cam/frame_stack']) #kl = Keras_Q_Categorical() #V.add(kl, # inputs=['cam/frame_stack'], # outputs=['pilot/angle', 'pilot/throttle'], # run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': #return pilot_angle, user_throttle return pilot_angle, pilot_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False): ''' 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_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_tx=False, use_pirf=False): ''' Start the drive loop Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' # Initialize car V = dk.vehicle.Vehicle() if cfg.USE_WEB_CAMERA: cam = Webcam(resolution=cfg.CAMERA_RESOLUTION) else: cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: # modify max_throttle closer to 1.0 to have more power # modify steering_scale lower than 1.0 to have less responsive steering ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE, throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS, steering_axis=cfg.JOYSTICK_STEERING_AXIS, btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON, btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON, btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON, btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON, btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON, btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON, btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON, btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON, btn_toggle_const_throttle=cfg. JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON, verbose=cfg.JOYSTICK_VERBOSE) elif use_tx or cfg.USE_TX_AS_DEFAULT: ctr = TxController(throttle_tx_min=cfg.TX_THROTTLE_MIN, throttle_tx_max=cfg.TX_THROTTLE_MAX, steering_tx_min=cfg.TX_STEERING_MIN, steering_tx_max=cfg.TX_STEERING_MAX, throttle_tx_thresh=cfg.TX_THROTTLE_TRESH, verbose=cfg.TX_VERBOSE) fpv = FPVWebController() V.add(fpv, inputs=['cam/image_array'], threaded=True) elif use_pirf or cfg.USE_PI_RF_AS_DEFAULT: ctr = PiRfController(throttle_tx_min=cfg.PI_RF_THROTTLE_MIN, throttle_tx_max=cfg.PI_RF_THROTTLE_MAX, steering_tx_min=cfg.PI_RF_STEERING_MIN, steering_tx_max=cfg.PI_RF_STEERING_MAX, throttle_tx_thresh=cfg.PI_RF_THROTTLE_TRESH, steering_pin=cfg.PI_RF_STEERING_PIN, throttle_pin=cfg.PI_RF_THROTTLE_PIN, verbose=cfg.PI_RF_VERBOSE) fpv = FPVWebController() V.add(fpv, inputs=['cam/image_array'], threaded=True) else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) emergencyCtrl = EmergencyController() V.add(emergencyCtrl, inputs=['user/mode'], outputs=['user/mode'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) if not use_tx and not use_pirf: # Run the pilot if the mode is not user and not Tx. kl = KerasCategorical() #kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) if cfg.USE_PWM_ACTUATOR: steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle', 'user/mode']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'pilot/angle', 'pilot/throttle' ] types = [ 'image_array', 'float', 'float', 'str', 'numpy.float32', 'numpy.float32' ] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")
def _get_sample_lambda(): def f(): return 1 return Lambda(f)
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. ''' #Initialisation the CAR V = dk.vehicle.Vehicle() # Initialisation of the CAMERA if cfg.OS == cfg.LINUX_OS: if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK: print("Camera {} can't be used.".format(cfg.CAMERA_MACBOOK)) sys.exit() elif cfg.CAMERA_ID == cfg.CAMERA_MOCK: cam = MockCamera() else: cam = PiCamera(camera=cfg.CAMERA_ID, resolution=cfg.CAMERA_RESOLUTION) elif cfg.OS == cfg.MAC_OS: if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK: cam = MacWebcam() else: cam = VideoStream(camera=cfg.CAMERA_ID, framerate=cfg.DRIVE_LOOP_HZ) 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, modes, and more. ctr = LocalWebController() if (cfg.LANE_DETECTOR): ctr_inputs = ['cam/image_overlaid_lanes', 'ld/runtime'] #Lane Detector #Detects the lane the car is in and outputs a best estimate for the 2 lines lane_detector = LanesDetector(cfg.CAMERA_ID, cfg.LD_PARAMETERS) V.add(lane_detector, inputs=['cam/image_array', 'algorithm/reset'], outputs=['cam/image_overlaid_lanes', 'ld/runtime']) else: ctr_inputs = ['cam/image_array'] V.add( ctr, ctr_inputs, outputs=[ 'user/angle', 'user/throttle', 'user/mode', 'recording' #'algorithm/reset' ], threaded=True) #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) #Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) if cfg.OS == cfg.LINUX_OS: steering_controller = PCA9685(cfg.STEERING_CHANNEL) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) elif cfg.OS == cfg.MAC_OS: steering_controller = MockController() throttle_controller = MockController() steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, center_pulse=cfg.STEERING_CENTER_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle = PWMThrottle( controller=throttle_controller, cal_max_pulse=cfg.THROTTLE_FORWARD_CAL_PWM, max_pulse=cfg.THROTTLE_FORWARD_PWM, cal_min_pulse=cfg.THROTTLE_REVERSE_CAL_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM, zero_pulse=cfg.THROTTLE_STOPPED_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') print("You can now go to <your pi ip address>:8887 to drive your car.") #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, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #setup AWS IoT if cfg.IOT_ENABLED: aws = AWSHandler(vehicle_id=cfg.VEHICLE_ID, endpoint=cfg.AWS_ENDPOINT, ca=cfg.CA_PATH, private_key=cfg.PRIVATE_KEY_PATH, certificate=cfg.CERTIFICATE_PATH) iot = aws.new_iot_publisher(intputs=inputs, types=types) V.add(iot, inputs=inputs, run_condition='recording') #run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, 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_BEHAVIORS: model_type = "behavior" else: model_type = "categorical" if model_type == "streamline": camera_type = "streamline" #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']) #elif camera_type == "streamline": # print("using grayscale pi cam") # assert(cfg.CAMERA_TYPE == "PICAM") # from donkeycar.parts.camera import PiCamera # cam = PiCamera(image_w=cfg.STREAMLINE_IMAGE_W, image_h=cfg.STREAMLINE_IMAGE_H, image_d=1) 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, 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, 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 from donkeycar.parts.controller import PS3JoystickController, PS4JoystickController #cont_class = PS3JoystickController cont_class = MyJoystickController 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): #returns a blink rate. 0 for off. -1 for on. positive for rate. 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: 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' ], 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: print('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: print('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 #if model_type == "streamline": #kl = KerasStreamline() #else: 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) 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']) 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) #run the vehicle for 20 seconds 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, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) ctr = SerialController() V.add(ctr, inputs=[], outputs=['user/angle', 'user/throttle', 'user/mode'], threaded=True) V.add(LocalWebController(use_chaos=use_chaos), inputs=['cam/image_array'], outputs=['a', 'b', 'c', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ (手動・自動)運転する。 多くの部品(part)から作業用のロボット車両を構築する。 各partはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded`に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出す。 すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレートで順次更新され、 各partが適時に処理を終了すると仮定する。 partには名前付きの出力と入力が存在する(どちらかがない場合や複数存在する場合もある)。 Vehicle フレームワークは、名前付き出力を同じ名前の入力を要求するpartに渡すことを処理する。 引数 cfg 個別車両設定オブジェクト、`config.py`がロードされたオブジェクト。 model_path 自動運転時のモデルファイルパスを指定する(デフォルトはNone)。 use_joystick ジョイスティックを使用するかどうかの真偽値(デフォルトはFalse)。 use_chaos 手動運転中に周期的なランダム操舵を加えるかどうかの真偽値(デフォルトはFalse)。 """ # Vehicle オブジェクトの生成 V = dk.vehicle.Vehicle() # Timestamp part の生成 clock = Timestamp() # Timestamp part をVehicleループへ追加 # 入力: # なし # 出力: # 'timestamp' 現在時刻 V.add(clock, outputs=['timestamp']) # PiCamera part の生成 cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) # 別スレッド実行される PiCamera part をVehicleループへ追加 # 入力: # なし # 出力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ V.add(cam, outputs=['cam/image_array'], threaded=True) # manage.py デフォルトのジョイスティックpart生成 if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE, # steering_scale=cfg.JOYSTICK_STEERING_SCALE, # throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS, # auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) # ジョイスティック part の生成 from elecom.part import JoystickController ctr = JoystickController(config_path='elecom/jc-u3912t.yml') else: # ステアリング、スロットル、モードなどを管理するWebサーバを作成する # Web Controller part の生成 ctr = LocalWebController(use_chaos=use_chaos) # 別スレッド実行される Web Controller part もしくはジョイスティック part をVehiecleループへ追加 # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'recording' tubデータとして保管するかどうかの真偽値 V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # オートパイロットモジュールを実行すべきかどうかを確認する関数を定義する。 def pilot_condition(mode): ''' オートパイロットモジュール実行判定関数。 引数で指定されたモードを判別して、オートパイロットモジュールを実行するべきかどうか 真偽値を返却する関数。 引数 mode Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) 戻り値 boolean オートパイロットモジュールを実行するかどうかの真偽値 ''' print('mode=' + mode) if mode == 'user': # 全手動時のみ実行しない return False else: return True # オートパイロットモジュール実行判定関数を part 化したオブジェクトを生成 pilot_condition_part = Lambda(pilot_condition) # オートパイロットモジュール実行判定 part を Vehiecle ループへ追加 # 入力: # 'user/mode' Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) # 出力: # 'run_pilot' オートパイロットモジュールを実行するかどうかの真偽値 V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Userモードでない場合、オートパイロットを実行する # CNNベースの線形回帰モデル(オートパイロット) part を生成する。 kl = KerasLinear() # 関数driveの引数 model_part 指定がある場合 if model_path: # 学習済みモデルファイルを読み込む kl.load(model_path) # run_condition が真の場合のみ実行されるオートパイロット part をVehicleループへ追加する # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # 車両にどの値を入力にするかを判別する def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): ''' 引数で指定された項目から、車両への入力とするステアリング値、スロットル値を確定する関数。 引数 mode Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) user_angle Web/Joystickにより手動指定した次に取るべきステアリング値 user_throttle Web/Joystickにより手動指定した次に取るべきスロットル値 pilot_angle オートパイロットが指定した次に取るべきステアリング値 pilot_throttle オートパイロットが指定した次に取るべきスロットル値 戻り値 angle 車両への入力とするステアリング値 throttle 車両への入力とするスロットル値 ''' if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle # 車両にどの値を入力にするかを判別する関数を part 化したオブジェクトを生成 drive_mode_part = Lambda(drive_mode) # 車両にどの値を入力にするかを判別する part を Vehicle ループへ追加 # 入力 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 # 戻り値 # 'angle' 車両への入力とするステアリング値 # 'throttle' 車両への入力とするスロットル値 V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) # 実車両のステアリングサーボを操作するオブジェクトを生成 steering_controller = PCA9685(cfg.STEERING_CHANNEL) # 実車両へステアリング値を指示する part を生成 steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) # 実車両のスロットルECSを操作するオブジェクトを生成 throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) # 実車両へスロットル値を指示する part を生成 throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) # 実車両へステアリング値を指示する part を Vehiecle ループへ追加 # 入力: # 'angle' 車両への入力とするステアリング値 # 出力: # なし(実車両の操舵へ) V.add(steering, inputs=['angle']) # 実車両へスロットル値を指示する part を Vehiecle ループへ追加 # 入力: # 'throttle' 車両への入力とするスロットル値 # 出力: # なし(実車両のスロットル操作へ) V.add(throttle, inputs=['throttle']) # 保存データを tub ディレクトリに追加 inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # 複数 tub ディレクトリの場合 # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # 単一 tub ディレクトリの場合 # tub ディレクトリへ書き込む part を生成 tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) # 'recording'が正であれば tub ディレクトリへ書き込む part を Vehiecle ループへ追加 # 入力 # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'timestamp' 現在時刻 V.add(tub, inputs=inputs, run_condition='recording') # テレメトリーデータの送信 #tele = PubTelemetry('iotf/emperor.ini', pub_count=20*5) #V.add(tele, inputs=['cam/image_array', 'user/mode', 'user/angle', 'user/throttle', # 'pilot/angle', 'pilot/throttle', 'angle', 'throttle']) # Vehicle ループを開始 V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) class Timer: def __init__(self, loops_timed): self.start_time = time.time() self.loop_counter = 0 self.loops_timed = loops_timed def run(self): self.loop_counter += 1 if self.loop_counter == self.loops_timed: seconds = time.time() - self.start_time print("{} loops takes {} seconds".format( self.loops_timed, seconds)) self.loop_counter = 0 self.start_time = time.time() timer = Timer(50) V.add(timer) #Part to save multiple image arrays from camera class ImageArrays: def __init__(self, steps): tmp = np.zeros((120, 160, 3)) self.steps = steps self.storage = [tmp for i in range(self.steps * 2 + 1)] self.active_images = [tmp for i in range(3)] def run(self, image): self.storage.pop(0) self.storage.append(image) for i in range(3): self.active_images[i] = self.storage[i * self.steps] return np.array(self.active_images) image_arrays = ImageArrays(10) V.add(image_arrays, inputs=['cam/image_array'], outputs=['cam/image_arrays']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_arrays'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, 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)