def drive(cfg): V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) # 略 V.mem['recording'] = True # JoyStickやWebControllerを改変し、以下の値をVehicleフレームワークの # メモリ上に格納する # .../value -1.0~1.0までの入力値→DCモータに加わる電圧になる # .../status 'move':前後動 'free':動力なし 'brake':制動停止 # 以下両モータ半速前進する入力値をダミーとして投入 V.mem['user/left/value'] = 0.5 V.mem['user/left/status'] = 'move' # 'free' 'brake' V.mem['user/right/value'] = 0.5 V.mem['user/right/status'] = 'move' # 'free' 'brake' # 対象Raspberry Pi上のpigpiodと通信するオブジェクト # 複数モータを使用する場合は、同じオブジェクトをそれぞれコンストラクタへ渡す # 但し、GPIOピンは重複させることはできない pi = pigpio.pi() # DCモータ1基ごとに1つのDCMotorインスタンスとして管理 from donkeypart_dc130ra import DCMotor left_motor = DCMotor(pi, cfg.LEFT_MOTOR_IN1_GPIO, cfg.LEFT_MOTOR_IN2_GPIO) right_motor = DCMotor(pi, cfg.RIGHT_MOTOR_IN1_GPIO, cfg.RIGHT_MOTOR_IN2_GPIO) # Vehicleフレームワークへ登録 V.add(left_motor, inputs=['user/left/value', 'user/left/status']) V.add(right_motor, inputs=['user/right/value', 'user/right/status']) # Tubデータ・フォーマットも変更しなくてはならない inputs = [ 'cam/image_array', 'user/left/value', 'user/left/status', 'user/right/status', 'user/right/status', 'timestamp' ] types = ['image_array', 'float', 'str', 'float', 'str', 'str'] # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None): vehicle = dk.vehicle.Vehicle() clock = Timestamp() vehicle.add(clock, outputs=['timestamp']) camera = PiCamera(resolution=cfg.CAMERA_RESOLUTION) vehicle.add(camera, outputs=['cam/image_array'], threaded=True) controller = PS3JoystickController( throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) vehicle.add( controller, outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) steering = PWMSteering(controller=PCA9685(cfg.STEERING_CHANNEL), left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) vehicle.add(steering, inputs=['user/angle']) throttle = PWMThrottle(controller=PCA9685(cfg.THROTTLE_CHANNEL), max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) vehicle.add(throttle, inputs=['user/throttle']) lidar = LidarScan() vehicle.add(lidar, outputs=['lidar/scan'], threaded=True) position = LidarPosition() vehicle.add(position, inputs=['lidar/scan'], outputs=['lidar/position', 'lidar/borders'], threaded=True) mqtt_client = MQTTClient(cfg) vehicle.add(mqtt_client, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'lidar/position', 'lidar/borders' ], outputs=['remote/mode'], threaded=True) vehicle.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) ctr = SerialController() V.add(ctr, inputs=[], outputs=['user/angle', 'user/throttle', 'user/mode'], threaded=True) V.add(LocalWebController(use_chaos=use_chaos), inputs=['cam/image_array'], outputs=['a', 'b', 'c', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None): V = dk.vehicle.Vehicle() V.mem.put(['square/angle', 'square/throttle'], (100, 100)) # display square box given by cooridantes. cam = SquareBoxCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, inputs=['square/angle', 'square/throttle'], outputs=['cam/image_array']) # display the image and read user values from a local web controller ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_contion only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # See if we should even run the pilot module. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'pilot_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) clock = Timestamp() V.add(clock, outputs=['timestamp']) # transform angle and throttle values to coordinate values def f(x): return int(x * 100 + 100) l = Lambda(f) V.add(l, inputs=['user/angle'], outputs=['square/angle']) V.add(l, inputs=['user/throttle'], outputs=['square/throttle']) # add tub to save data inputs=['cam/image_array', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'square/angle', 'square/throttle', 'user/mode', 'timestamp'] types=['image_array', 'float', 'float', 'float', 'float', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle for 20 seconds V.start(rate_hz=50, max_loop_count=10000)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ 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_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 add_timestamp(vehicle): from donkeycar.parts.clock import Timestamp clock = Timestamp() vehicle.add(clock, outputs=['timestamp'])
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ (手動・自動)運転する。 多くの部品(part)から作業用のロボット車両を構築する。 各partはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded`に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出す。 すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレートで順次更新され、 各partが適時に処理を終了すると仮定する。 partには名前付きの出力と入力が存在する(どちらかがない場合や複数存在する場合もある)。 Vehicle フレームワークは、名前付き出力を同じ名前の入力を要求するpartに渡すことを処理する。 引数 cfg 個別車両設定オブジェクト、`config.py`がロードされたオブジェクト。 model_path 自動運転時のモデルファイルパスを指定する(デフォルトはNone)。 use_joystick ジョイスティックを使用するかどうかの真偽値(デフォルトはFalse)。 use_chaos 手動運転中に周期的なランダム操舵を加えるかどうかの真偽値(デフォルトはFalse)。 """ # Vehicle オブジェクトの生成 V = dk.vehicle.Vehicle() # Timestamp part の生成 clock = Timestamp() # Timestamp part をVehicleループへ追加 # 入力: # なし # 出力: # 'timestamp' 現在時刻 V.add(clock, outputs=['timestamp']) # PiCamera part の生成 cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) # 別スレッド実行される PiCamera part をVehicleループへ追加 # 入力: # なし # 出力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ V.add(cam, outputs=['cam/image_array'], threaded=True) # manage.py デフォルトのジョイスティックpart生成 if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE, # steering_scale=cfg.JOYSTICK_STEERING_SCALE, # throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS, # auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) # ジョイスティック part の生成 from elecom.part import JoystickController ctr = JoystickController(config_path='elecom/jc-u3912t.yml') else: # ステアリング、スロットル、モードなどを管理するWebサーバを作成する # Web Controller part の生成 ctr = LocalWebController(use_chaos=use_chaos) # 別スレッド実行される Web Controller part もしくはジョイスティック part をVehiecleループへ追加 # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'recording' tubデータとして保管するかどうかの真偽値 V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # オートパイロットモジュールを実行すべきかどうかを確認する関数を定義する。 def pilot_condition(mode): ''' オートパイロットモジュール実行判定関数。 引数で指定されたモードを判別して、オートパイロットモジュールを実行するべきかどうか 真偽値を返却する関数。 引数 mode Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) 戻り値 boolean オートパイロットモジュールを実行するかどうかの真偽値 ''' print('mode=' + mode) if mode == 'user': # 全手動時のみ実行しない return False else: return True # オートパイロットモジュール実行判定関数を part 化したオブジェクトを生成 pilot_condition_part = Lambda(pilot_condition) # オートパイロットモジュール実行判定 part を Vehiecle ループへ追加 # 入力: # 'user/mode' Userモード('user':全手動、'local_angle':操舵のみ自動、'local':全自動) # 出力: # 'run_pilot' オートパイロットモジュールを実行するかどうかの真偽値 V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Userモードでない場合、オートパイロットを実行する # CNNベースの線形回帰モデル(オートパイロット) part を生成する。 kl = KerasLinear() # 関数driveの引数 model_part 指定がある場合 if model_path: # 学習済みモデルファイルを読み込む kl.load(model_path) # run_condition が真の場合のみ実行されるオートパイロット part をVehicleループへ追加する # 入力: # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 出力: # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # 車両にどの値を入力にするかを判別する def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): ''' 引数で指定された項目から、車両への入力とするステアリング値、スロットル値を確定する関数。 引数 mode Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) user_angle Web/Joystickにより手動指定した次に取るべきステアリング値 user_throttle Web/Joystickにより手動指定した次に取るべきスロットル値 pilot_angle オートパイロットが指定した次に取るべきステアリング値 pilot_throttle オートパイロットが指定した次に取るべきスロットル値 戻り値 angle 車両への入力とするステアリング値 throttle 車両への入力とするスロットル値 ''' if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle # 車両にどの値を入力にするかを判別する関数を part 化したオブジェクトを生成 drive_mode_part = Lambda(drive_mode) # 車両にどの値を入力にするかを判別する part を Vehicle ループへ追加 # 入力 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'pilot/angle' オートパイロットが指定した次に取るべきステアリング値 # 'pilot/throttle' オートパイロットが指定した次に取るべきスロットル値 # 戻り値 # 'angle' 車両への入力とするステアリング値 # 'throttle' 車両への入力とするスロットル値 V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) # 実車両のステアリングサーボを操作するオブジェクトを生成 steering_controller = PCA9685(cfg.STEERING_CHANNEL) # 実車両へステアリング値を指示する part を生成 steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) # 実車両のスロットルECSを操作するオブジェクトを生成 throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) # 実車両へスロットル値を指示する part を生成 throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) # 実車両へステアリング値を指示する part を Vehiecle ループへ追加 # 入力: # 'angle' 車両への入力とするステアリング値 # 出力: # なし(実車両の操舵へ) V.add(steering, inputs=['angle']) # 実車両へスロットル値を指示する part を Vehiecle ループへ追加 # 入力: # 'throttle' 車両への入力とするスロットル値 # 出力: # なし(実車両のスロットル操作へ) V.add(throttle, inputs=['throttle']) # 保存データを tub ディレクトリに追加 inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # 複数 tub ディレクトリの場合 # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # 単一 tub ディレクトリの場合 # tub ディレクトリへ書き込む part を生成 tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) # 'recording'が正であれば tub ディレクトリへ書き込む part を Vehiecle ループへ追加 # 入力 # 'cam/image_array' cfg.CAMERA_RESOLUTION 型式の画像データ # 'user/angle' Web/Joystickにより手動指定した次に取るべきステアリング値 # 'user/throttle' Web/Joystickにより手動指定した次に取るべきスロットル値 # 'user/mode' Web/Joystickにより手動指定した次に取るべきUserモード(入力なしの場合は前回値のまま) # 'timestamp' 現在時刻 V.add(tub, inputs=inputs, run_condition='recording') # テレメトリーデータの送信 #tele = PubTelemetry('iotf/emperor.ini', pub_count=20*5) #V.add(tele, inputs=['cam/image_array', 'user/mode', 'user/angle', 'user/throttle', # 'pilot/angle', 'pilot/throttle', 'angle', 'throttle']) # Vehicle ループを開始 V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #ctr = PS4JoystickController(steering_scale=cfg.JOYSTICK_STEERING_SCALE, # auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) ctr = SerialDevice() else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) ## Create the controller part V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) ## MUST: choose one of the below controllers actuator_type = cfg.ACTUATOR_MODE # normal, seesaw, serial if actuator_type == 'serial': pass #steering_controller = SerialDevice(cfg.STEERING_CHANNEL) #throttle_controller = SerialDevice(cfg.THROTTLE_CHANNEL) elif actuator_type == 'seesaw': steering_controller = RoboHATMM1(cfg.STEERING_CHANNEL) throttle_controller = RoboHATMM1(cfg.THROTTLE_CHANNEL) else: steering_controller = PCA9685(cfg.STEERING_CHANNEL) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) ## This Creates the magic PWM parts for the Controllers above. if actuator_type is not 'serial': steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) else: V.add(SerialDevice(), inputs['angle', 'throttle']) # add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) if cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam cam = Webcam() else: from donkeycar.parts.camera import PiCamera cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording', 'pan', 'tilt'], threaded=True) if cfg.USE_PAN: pan_controller = PCA9685(cfg.PAN_CHANNEL) pan = PWMPanTilt(controller=pan_controller, left_pulse=cfg.PAN_LEFT_PWM, right_pulse=cfg.PAN_RIGHT_PWM, dir=cfg.PAN_DIR) V.add(pan, inputs=['pan']) if cfg.USE_TILT: tilt_controller = PCA9685(cfg.TILT_CHANNEL) tilt = PWMPanTilt(controller=tilt_controller, left_pulse=cfg.TILT_DOWN_PWM, right_pulse=cfg.TILT_UP_PWM, dir=cfg.TILT_DIR) V.add(tilt, inputs=['tilt']) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: print(user_throttle) user_throttle = user_throttle * cfg.THROTTLE_PILOT_WEIGHT if(user_throttle < cfg.THROTTLE_PILOT_LIMIT): user_throttle = cfg.THROTTLE_PILOT_LIMIT return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle'], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM, dir=cfg.STEERING_DIR) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM, dir=cfg.THROTTLE_DIR) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp'] types = ['image_array', 'float', 'float', 'str', 'str'] if cfg.USE_SINGLE_TUB == False: #multiple tubs th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) else: # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ たくさんのパーツから作業用のロボットVehicleを構築します。 各パーツはVehicleループ内のジョブとして実行され、コンストラクタフラグ `threaded` に応じて `run` メソッドまたは `run_threaded` メソッドを呼び出します。 すべてのパーツは、 `cfg.DRIVE_LOOP_HZ` で指定されたフレームレート (デフォルト:20MHz)で順次更新され、各パーツが適時に処理を終了すると仮定して ループを無限に繰り返します。 パーツにはラベル付きの `inputs` と `outputs` があります。 フレームワークは、ラベル付き `outputs` の値を、 同じ名前の `inputs` を要求する別のパーツに渡します。 引数: cfg config.py を読み込んだオブジェクト model_path 学習済みモデルファイルのパス use_joystick ジョイスティックを使用するかどうかの真偽値 use_chaos 操舵に一定のランダム操作を加えるかどうかの真偽値 """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # このWebコントローラでは、ステアリング、スロットル、モードなどを管理する # Webサーバを作成 ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # パイロットモジュールを走らせるべきかどうかを毎回判別させるためのパーツ # この関数の結果の真偽値で自動運転パーツ(ConvLSTM2DPilot)を実行するかを決定させる def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. # 独自のパイロットに置き換え #kl = KerasCategorical() kl = ConvLSTM2DPilot() if model_path: print("start loading trained model file") kl.load(model_path) print("finish loading trained model file") V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # 実車のインプットとしてどの値を使うかのモード選択 def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() center_led = rgb_led(red_channel=9, green_channel=10, blue_channel=11) status_led = status_indicator(status_led=center_led) V.add(center_led, outputs=['none']) V.add(status_led, inputs=['user/mode', 'recording']) #add pi_perfchecker #loop_time = driveLoopTime() #loop_time.console=True #core_temp = coreTemp() #V.add(core_temp) #V.add(loop_time,inputs = ['timein'], outputs = ['timein','displaytext']) #throtled_status = throttled() #V.add(throtled_status, outputs=['displaytext'],threaded=True) loop_time = driveLoopTime() V.add(loop_time, inputs=['timein'], outputs=['timein', 'displaytext']) loop_time_display = ImgPutText(org=(100, 20)) clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) #cam = PiCamera(resolution=(960,1280)) V.add(cam, outputs=['cam/image_array'], threaded=True) #ncs_ty = tinyyolo(basedir = cfg.MODELS_PATH, draw_on_img = True, probability_threshold = 0.15,debug=False) #V.add(ncs_ty, inputs=['cam/image_array'],outputs=['cam/image_array','ncs/found_objs'],threaded=True) V.add(loop_time_display, inputs=['cam/image_array', 'displaytext'], outputs=['cam/image_array']) #img_resize = ImgResize() #V.add(img_resize,inputs=['cam/image_array'],outputs=['cam/image_array']) #ncs_gn = googlenet(basedir=cfg.MODELS_PATH, debug=True) #V.add(ncs_gn, inputs=['cam/image_array'],outputs=['ncs/image_array', 'classificaiton'],threaded=True) #ncs_inception = inception(basedir=cfg.MODELS_PATH, probability_threshold=0.01, debug=True) #V.add(ncs_inception, inputs=['cam/image_array'],outputs=['cam/image_array', 'classificaiton'],threaded=True) #face_detect = detect(basedir=cfg.MODELS_PATH, debug=True) # V.add(face_detect, inputs=['cam/image_array'],outputs=['face/image_array', 'face_center'],threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE, throttle_axis='rz', #throttle_axis='y', steering_axis='x', panning_axis='rx', tilting_axis='ry') """ when running from JS, let us display the nsc images on the web page """ """ ctr_webview = LocalWebController() V.add(ctr_webview, inputs=['cam/image_array'], #inputs=['cam/image_array'], outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'], #outputs=['outargs'], threaded=True) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) """ V.add( ctr, inputs=['cam/image_array'], outputs=[ 'user/angle', 'user/throttle', 'user/mode', 'recording', 'user/pan', 'user/tilt' ], #outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) ctr_webview = LocalWebController() V.add( ctr_webview, inputs=['cam/image_array'], #inputs=['cam/image_array'], outputs=['user/angleX', 'user/throttleX', 'user/modeX', 'recordingX'], #outputs=['outargs'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. #kl = KerasLinear() kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle, user_pan, user_tilt, face_pan, face_tilt): if mode == 'user': return user_angle, user_throttle, user_pan, user_tilt elif mode == 'face': return user_angle, user_throttle, face_pan, face_tilt elif mode == 'local_angle': return pilot_angle, user_throttle, face_pan, face_tilt else: return pilot_angle, pilot_throttle, face_pan, face_tilt drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'user/pan', 'user/tilt', 'face/pan', 'face/tilt' ], outputs=['angle', 'throttle', 'pan', 'tilt']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) panning_controller = PCA9685(cfg.PAN_CHANNEL) panning = PWMPanning(controller=panning_controller, left_pulse=cfg.PAN_LEFT_PWM, zero_pulse=cfg.PAN_CENTER_PWM, right_pulse=cfg.PAN_RIGHT_PWM) tilting_controller = PCA9685(cfg.TILT_CHANNEL) tilting = PWMTilting(controller=tilting_controller, max_pulse=cfg.TILT_UP_PWM, zero_pulse=cfg.TILT_DRIVING_PWM, min_pulse=cfg.TILT_DOWN_PWM) wagging_controller = PCA9685(cfg.TAIL_CHANNEL) wagging = PWMWagging(controller=wagging_controller, max_pulse=cfg.TAIL_UP_PWM, zero_pulse=cfg.TAIL_CENTER_PWM, min_pulse=cfg.TAIL_DOWN_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) #looping pan and tilt out to donkeyface, then back into this donkeypet donkeyPet_sender = send_to_donkey(send_to_address='10.11.44.20') V.add(donkeyPet_sender, inputs=['pan', 'tilt', 'user/mode', 'recording', 'timestamp'], outputs=['donkeyface_output']) donkeyPet_receiver = rcv_from_donkey(listen_to_port=10500, debug=False) V.add(donkeyPet_receiver, inputs=['rcv_in'], outputs=['face/pan', 'face/tilt', 'face/irq']) feels_on_target = target_status(debug=True) V.add(feels_on_target, inputs=['face/irq', 'tail', 'user/mode'], outputs=['tail']) V.add(panning, inputs=['face/pan']) V.add(tilting, inputs=['face/tilt']) V.add(wagging, inputs=["tail"]) # add tub to save data #pan and tilt setup tub_inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] tub_types = ['image_array', 'float', 'float', 'str', 'str'] #non pan and tilt test #tub_inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp','test_var'] #tub_types = ['image_array', 'float', 'float', 'str', 'str','str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=tub_inputs, types=tub_types) V.add(tub, inputs=tub_inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #setup AWS IoT if cfg.IOT_ENABLED: aws = AWSHandler(vehicle_id=cfg.VEHICLE_ID, endpoint=cfg.AWS_ENDPOINT, ca=cfg.CA_PATH, private_key=cfg.PRIVATE_KEY_PATH, certificate=cfg.CERTIFICATE_PATH) iot = aws.new_iot_publisher(intputs=inputs, types=types) V.add(iot, inputs=inputs, run_condition='recording') #run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive_vis(cfg, model_path=None, use_chaos=False): V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) # Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) top_view_transform = TopViewTransform(cfg.CAMERA_RESOLUTION) V.add(Lambda(top_view_transform.wrap), inputs=['cam/image_array'], outputs=['cam/image_array_proj']) V.add(kl, inputs=['cam/image_array_proj'], outputs=['pilot/angle', 'pilot/throttle']) ctr = LocalWebControllerVis(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array_proj', 'pilot/angle', 'pilot/throttle'], outputs=['user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) driver = ArduinoDriver() V.add(driver, inputs=['user/mode', 'pilot/angle', 'pilot/throttle'], outputs=['user/angle', 'user/throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] # multiple tubs # th = TubHandler(path=cfg.DATA_PATH) # tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False): """ Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. """ V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs='timestamp') cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: ctr = JoystickController( max_throttle=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) else: # This web controller will create a web server that is capable # of managing steering, throttle, and modes, and more. ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # See if we should even run the pilot module. # This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) class Timer: def __init__(self, loops_timed): self.start_time = time.time() self.loop_counter = 0 self.loops_timed = loops_timed def run(self): self.loop_counter += 1 if self.loop_counter == self.loops_timed: seconds = time.time() - self.start_time print("{} loops takes {} seconds".format( self.loops_timed, seconds)) self.loop_counter = 0 self.start_time = time.time() timer = Timer(50) V.add(timer) #Part to save multiple image arrays from camera class ImageArrays: def __init__(self, steps): tmp = np.zeros((120, 160, 3)) self.steps = steps self.storage = [tmp for i in range(self.steps * 2 + 1)] self.active_images = [tmp for i in range(3)] def run(self, image): self.storage.pop(0) self.storage.append(image) for i in range(3): self.active_images[i] = self.storage[i * self.steps] return np.array(self.active_images) image_arrays = ImageArrays(10) V.add(image_arrays, inputs=['cam/image_array'], outputs=['cam/image_arrays']) # Run the pilot if the mode is not user. kl = KerasCategorical() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_arrays'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') # Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) # add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #multiple tubs #th = TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) # single tub tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') # run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_chaos=False): #Initialized car V = dk.vehicle.Vehicle() clock = Timestamp() V.add(clock, outputs=['timestamp']) cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION) V.add(cam, outputs=['cam/image_array'], threaded=True) ctr = LocalWebController(use_chaos=use_chaos) V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) #See if we should even run the pilot module. #This is only needed because the part run_contion only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) #Run the pilot if the mode is not user. kl = KerasLinear() if model_path: kl.load(model_path) V.add(kl, inputs=['cam/image_array'], outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) controller_left = PCA9685(cfg.DD_STEERING_CHANNEL_LEFT) controller_right = PCA9685(cfg.DD_STEERING_CHANNEL_RIGHT) diffdrive = L298N(controller_left=controller_left, controller_right=controller_right) V.add(diffdrive, inputs=['throttle', 'angle']) #add tub to save data inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/mode', 'timestamp' ] types = ['image_array', 'float', 'float', 'str', 'str'] #th = dk.parts.TubHandler(path=cfg.DATA_PATH) #tub = th.new_tub_writer(inputs=inputs, types=types) #V.add(tub, inputs=inputs, run_condition='recording') tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types) V.add(tub, inputs=inputs, run_condition='recording') #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS) print("You can now go to <your pi ip address>:8887 to drive your car.")