Beispiel #1
0
def test_motor(cfg):
    import pigpio
    pgio = pigpio.pi()

    V = dk.vehicle.Vehicle()
    from parts import PIGPIO_OUT, PIGPIO_PWM
    left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio)
    left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio)
    left_pwm = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO,
                          pgio=pgio,
                          freq=cfg.PWM_FREQ,
                          range=cfg.PWM_RANGE,
                          threshold=cfg.PWM_INPUT_THRESHOLD)

    right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio)
    right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio)
    right_pwm = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=cfg.PWM_FREQ,
                           range=cfg.PWM_RANGE,
                           threshold=cfg.PWM_INPUT_THRESHOLD)

    loop = True
    while loop:
        try:
            print('1, 0, 0.4')

            left_in1.run(1)
            left_in2.run(0)
            left_pwm.run(0.4)
            right_in1.run(1)
            right_in2.run(0)
            right_pwm.run(0.4)
            sleep(5)
            print('0, 1, 0.4')
            left_in1.run(0)
            left_in2.run(1)
            left_pwm.run(0.4)
            right_in1.run(0)
            right_in2.run(1)
            right_pwm.run(0.4)
            sleep(5)
        except KeyboardInterrupt:
            loop = False
    left_in1.run(0)
    left_in2.run(0)
    left_pwm.run(0)
    right_in1.run(0)
    right_in2.run(0)
    right_pwm.run(0)
    pgio.stop()
    print('stop')
Beispiel #2
0
def test_motor(use_debug=True):
    import pigpio
    pgio = pigpio.pi()

    import donkeycar as dk
    cfg = dk.load_config()
    V = dk.vehicle.Vehicle()

    class TestDriver:
        def __init__(self, debug=False):
            self.values = [
                [1, 0, 0.0], [1, 0, 0.1], [1, 0, 0.2], [1, 0, 0.3], [1, 0, 0.4], [1, 0, 0.5],
                [1, 0, 0.5], [1, 0, 0.4], [1, 0, 0.3], [1, 0, 0.2], [1, 0, 0.1], [1, 0, 0.0],
                [0, 1, 0.0], [0, 1, 0.1], [0, 1, 0.2], [0, 1, 0.3], [0, 1, 0.4], [0, 1, 0.5],
                [0, 1, 0.5], [0, 1, 0.4], [0, 1, 0.3], [0, 1, 0.2], [0, 1, 0.1], [0, 1, 0.0],
            ]
            self.debug = debug
            self.count = 0

        def run(self):
            vals = self.values[self.count]
            self.count = self.count + 1
            if self.count >= len(self.values):
                self.count = 0
            if self.debug:
                print('in1:{}, in2:{}, pwm:{}'.format(str(vals[0]), str(vals[1]), str(vals[2])))
            return vals[0], vals[1], vals[2], vals[0], vals[1], vals[2]
        
        def shutdown(self):
            self.count = 0
            self.debug = False

    V.add(TestDriver(debug=use_debug), outputs=[
        'left_motor_in1', 'left_motor_in2', 'left_motor_vref',
        'right_motor_in1', 'right_motor_in2', 'right_motor_vref'
    ])

    from parts import PIGPIO_OUT, PIGPIO_PWM

    # TB6612 STBY ピン初期化
    stby = PIGPIO_OUT(pin=cfg.TB6612_STBY_GPIO, pgio=pgio, debug=use_debug)
    stby.run(1)

    # 左モータ制御
    left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug)
    left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug)
    left_vref = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug)
    V.add(left_in1, inputs=['left_motor_in1'])
    V.add(left_in2, inputs=['left_motor_in2'])
    V.add(left_vref, inputs=['left_motor_vref'])  

    # 右モータ制御
    right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug)
    right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug)
    right_vref = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug)
    V.add(right_in1, inputs=['right_motor_in1'])
    V.add(right_in2, inputs=['right_motor_in2'])
    V.add(right_vref, inputs=['right_motor_vref'])

    try:
        print('Start driving')
        #run the vehicle for 20 seconds
        V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
    except KeyboardInterrupt:
        print('Halt driving')
    finally:
        if stby is not None:
            stby.run(0)
            if use_debug:
                print('Stop TB6612 STBY')
        if pgio is not None:
            pgio.stop()
            if use_debug:
                print('Stop pigpio controll')
        print('Stop driving')
Beispiel #3
0
def test_pad(cfg):
    from parts import get_js_controller
    controller = get_js_controller(cfg)
    V = dk.vehicle.Vehicle()

    V.add(controller,
          outputs=['angle', 'throttle', 'lift_throttle', 'mode', 'recording'],
          threaded=True)

    class Prt:
        def run(self, angle, throttle, lift_throttle, mode, recording):
            print('angle:{}, throttle:{}, lift:{}, mode:{}, rec:{}'.format(
                str(angle), str(throttle), str(lift_throttle), str(mode),
                str(recording)))

    V.add(Prt(),
          inputs=['angle', 'throttle', 'lift_throttle', 'mode', 'recording'])

    from parts import ForkliftMotorDriver
    motor_driver = ForkliftMotorDriver(debug=False)
    V.add(motor_driver,
          inputs=['throttle', 'angle', 'lift_throttle'],
          outputs=[
              'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
              'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
              'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
          ])

    class Prt2:
        def run(self, left_verf, left_in1, left_in2, right_verf, right_in1,
                right_in2, lift_verf, lift_in1, lift_in2):
            print('ForkliftMD left   verf:{}, in1:{}, in2:{}'.format(
                str(left_verf), str(left_in1), str(left_in2)))
            print('ForkliftMD right  verf:{}, in1:{}, in2:{}'.format(
                str(right_verf), str(right_in1), str(right_in2)))
            print('ForkliftMD lift   verf:{}, in1:{}, in2:{}'.format(
                str(lift_verf), str(lift_in1), str(lift_in2)))

    V.add(Prt2(),
          inputs=[
              'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
              'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
              'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
          ])

    import pigpio

    pgio = pigpio.pi()

    from parts import PIGPIO_OUT, PIGPIO_PWM
    V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio),
          inputs=['left_motor_in1'])
    V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio),
          inputs=['left_motor_in2'])
    V.add(PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO,
                     pgio=pgio,
                     freq=cfg.PWM_FREQ,
                     range=cfg.PWM_RANGE,
                     threshold=cfg.PWM_INPUT_THRESHOLD),
          inputs=['left_motor_vref'])

    V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio),
          inputs=['right_motor_in1'])
    V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio),
          inputs=['right_motor_in2'])
    V.add(PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO,
                     pgio=pgio,
                     freq=cfg.PWM_FREQ,
                     range=cfg.PWM_RANGE,
                     threshold=cfg.PWM_INPUT_THRESHOLD),
          inputs=['right_motor_vref'])

    V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN1_GPIO, pgio=pgio),
          inputs=['lift_motor_in1'])
    V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN2_GPIO, pgio=pgio),
          inputs=['lift_motor_in2'])
    V.add(PIGPIO_PWM(pin=cfg.LIFT_MOTOR_PWM_GPIO,
                     pgio=pgio,
                     freq=cfg.PWM_FREQ,
                     range=cfg.PWM_RANGE,
                     threshold=cfg.PWM_INPUT_THRESHOLD),
          inputs=['lift_motor_vref'])

    try:
        V.start(rate_hz=20, max_loop_count=10000)
    except KeyboardInterrupt:
        print('exit')
    finally:
        pgio.stop()
Beispiel #4
0
def drive(cfg,
          model_path=None,
          use_joystick=False,
          use_hedge=False,
          use_aws=False,
          use_map=False,
          use_debug=False,
          model_type=None,
          camera_type='single',
          write_both_images=False,
          meta=[]):
    '''
    Construct a working robotic vehicle from many parts.
    Each part runs as a job in the Vehicle loop, calling either
    it's run or run_threaded method depending on the constructor flag `threaded`.
    All parts are updated one after another at the framerate given in
    cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
    Parts may have named outputs and inputs. The framework handles passing named outputs
    to parts requesting the same named input.
    '''

    if cfg.DONKEY_GYM:
        #the simulator will use cuda and then we usually run out of resources
        #if we also try to use cuda. so disable for donkey_gym.
        os.environ["CUDA_VISIBLE_DEVICES"] = "-1"

    if model_type is None:
        if cfg.TRAIN_LOCALIZER:
            model_type = "localizer"
        elif cfg.TRAIN_BEHAVIORS:
            model_type = "behavior"
        else:
            model_type = cfg.DEFAULT_MODEL_TYPE

    # pigpio 初期化
    try:
        import pigpio
    except:
        raise
    pgio = pigpio.pi()

    #Initialize car
    V = dk.vehicle.Vehicle()
    '''
    カメラ
    '''
    if camera_type == "stereo":

        if cfg.CAMERA_TYPE == "WEBCAM":
            from donkeycar.parts.camera import Webcam

            camA = Webcam(image_w=cfg.IMAGE_W,
                          image_h=cfg.IMAGE_H,
                          image_d=cfg.IMAGE_DEPTH,
                          iCam=0)
            camB = Webcam(image_w=cfg.IMAGE_W,
                          image_h=cfg.IMAGE_H,
                          image_d=cfg.IMAGE_DEPTH,
                          iCam=1)

        elif cfg.CAMERA_TYPE == "CVCAM":
            from donkeycar.parts.cv import CvCam

            camA = CvCam(image_w=cfg.IMAGE_W,
                         image_h=cfg.IMAGE_H,
                         image_d=cfg.IMAGE_DEPTH,
                         iCam=0)
            camB = CvCam(image_w=cfg.IMAGE_W,
                         image_h=cfg.IMAGE_H,
                         image_d=cfg.IMAGE_DEPTH,
                         iCam=1)
        else:
            raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE))

        V.add(camA, outputs=['cam/image_array_a'], threaded=True)
        V.add(camB, outputs=['cam/image_array_b'], threaded=True)

        from donkeycar.parts.image import StereoPair

        V.add(StereoPair(),
              inputs=['cam/image_array_a', 'cam/image_array_b'],
              outputs=['cam/image_array'])

    elif write_both_images or (cfg.CAMERA_TYPE != "MAP" and use_map == False):
        print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
        if cfg.DONKEY_GYM:
            from donkeycar.parts.dgym import DonkeyGymEnv

        inputs = []
        threaded = True
        #print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
        if cfg.DONKEY_GYM:
            from donkeycar.parts.dgym import DonkeyGymEnv
            cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
                               env_name=cfg.DONKEY_GYM_ENV_NAME)
            threaded = True
            inputs = ['angle', 'throttle']
        elif cfg.CAMERA_TYPE == "PICAM":
            from donkeycar.parts.camera import PiCamera
            cam = PiCamera(image_w=cfg.IMAGE_W,
                           image_h=cfg.IMAGE_H,
                           image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "WEBCAM":
            from donkeycar.parts.camera import Webcam
            cam = Webcam(image_w=cfg.IMAGE_W,
                         image_h=cfg.IMAGE_H,
                         image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "CVCAM":
            from donkeycar.parts.cv import CvCam
            cam = CvCam(image_w=cfg.IMAGE_W,
                        image_h=cfg.IMAGE_H,
                        image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "CSIC":
            from donkeycar.parts.camera import CSICamera
            cam = CSICamera(image_w=cfg.IMAGE_W,
                            image_h=cfg.IMAGE_H,
                            image_d=cfg.IMAGE_DEPTH,
                            framerate=cfg.CAMERA_FRAMERATE,
                            gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
        elif cfg.CAMERA_TYPE == "V4L":
            from donkeycar.parts.camera import V4LCamera
            cam = V4LCamera(image_w=cfg.IMAGE_W,
                            image_h=cfg.IMAGE_H,
                            image_d=cfg.IMAGE_DEPTH,
                            framerate=cfg.CAMERA_FRAMERATE)
        elif cfg.CAMERA_TYPE == "MOCK":
            from donkeycar.parts.camera import MockCamera
            cam = MockCamera(image_w=cfg.IMAGE_W,
                             image_h=cfg.IMAGE_H,
                             image_d=cfg.IMAGE_DEPTH)
        else:
            raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))

        # 前方画像を保管するV.memキー名
        if write_both_images:
            from parts.datastore import FWD_CAMERA_KEY
            outputs = [FWD_CAMERA_KEY]
        else:
            outputs = ['cam/image_array']

        V.add(cam, inputs=inputs, outputs=outputs, threaded=threaded)
    '''
    Marvelmind 位置情報システム
    '''
    # Marvelmind USNav データ
    hedge_items = []
    hedge_types = []
    usnav_items = [
        'usnav/id',
        'usnav/x',
        'usnav/y',
        'usnav/z',
        'usnav/angle',
        'usnav/timestamp',
    ]
    usnav_types = [
        'str',
        'float',
        'float',
        'float',
        'float',
        'float',
    ]
    hedge_items += usnav_items
    hedge_types += usnav_types
    # Marvelmind USNav Raw データ
    usnav_raw_items = [
        'dist/id', 'dist/b1', 'dist/b1d', 'dist/b2', 'dist/b2d', 'dist/b3',
        'dist/b3d', 'dist/b4', 'dist/b4d', 'dist/timestamp'
    ]
    usnav_raw_types = [
        'str',
        'str',
        'float',
        'str',
        'float',
        'str',
        'float',
        'str',
        'float',
        'float',
    ]
    hedge_items += usnav_raw_items
    hedge_types += usnav_raw_types
    # Marvelmind IMU データ
    imu_items = [
        'imu/x',
        'imu/y',
        'imu/z',
        'imu/qw',
        'imu/qx',
        'imu/qy',
        'imu/qz',
        'imu/vx',
        'imu/vy',
        'imu/vz',
        'imu/ax',
        'imu/ay',
        'imu/az',
        'imu/gx',
        'imu/gy',
        'imu/gz',
        'imu/mx',
        'imu/my',
        'imu/mz',
        'imu/timestamp',
    ]
    imu_types = [
        'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float',
        'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float',
        'float', 'float', 'float', 'float'
    ]
    # Marvelmind 全データ
    hedge_items += imu_items
    hedge_types += imu_types
    # Veichle上の Marvelmind 全データ初期化
    for i in range(len(hedge_items)):
        _item = hedge_items[i]
        _type = hedge_types[i]
        if _type == 'str':
            V.mem[_item] = '0'
        elif _type == 'float':
            V.mem[_item] = 0.0
        else:
            raise ValueError('unknown type:{}'.format(_type))

    if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
        '''
        Marvelmind システムを使用する場合
        '''
        print('Using Marvelmind Mobile Beacon')
        # Marvelmind モバイルビーコンパーツ追加
        from parts import HedgehogController
        hedge = HedgehogController(tty=cfg.HEDGE_SERIAL_TTY, adr=cfg.HEDGE_ID)
        V.add(hedge, outputs=hedge_items)
    '''
    IMU(MPU9250/MPU6050)
    '''
    mpu6050_items = [
        'imu/acl_x',
        'imu/acl_y',
        'imu/acl_z',
        'imu/gyr_x',
        'imu/gyr_y',
        'imu/gyr_z',
        'imu/recent',
        'imu/mpu_timestamp',
    ]
    mpu6050_types = [
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'str',
        'float',
    ]
    mpu9250_items = [
        'imu/acl_x',
        'imu/acl_y',
        'imu/acl_z',
        'imu/gyr_x',
        'imu/gyr_y',
        'imu/gyr_z',
        'imu/mgt_x',
        'imu/mgt_y',
        'imu/mgt_z',
        'imu/temp',
        'imu/recent',
        'imu/mpu_timestamp',
    ]
    mpu9250_types = [
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
        'str',
        'float',
    ]
    if cfg.HAVE_IMU:
        mpu_items = []
        mpu_types = []
        if cfg.IMU_TYPE == 'mpu6050':
            '''
            MPU6050を使用する場合
            '''
            mpu_items += mpu6050_items
            mpu_types += mpu6050_types
            from parts.sensors.imu import Mpu6050
            imu = Mpu6050(pgio=pgio,
                          bus=cfg.MPU6050_I2C_BUS,
                          address=cfg.MPU6050_I2C_ADDRESS,
                          depth=cfg.MPU6050_DEPTH,
                          debug=use_debug)

        elif cfg.IMU_TYPE == 'mpu9250':
            '''
            MPU9250を使用する場合
            '''
            mpu_items += mpu9250_items
            mpu_types += mpu9250_types
            from parts.sensors.imu import Mpu9250
            imu = Mpu9250(pgio=pgio,
                          bus=cfg.MPU9250_I2C_BUS,
                          mpu9250_address=cfg.MPU9250_I2C_ADDRESS,
                          ak8963_address=cfg.AK8963_I2C_ADDRESS,
                          depth=cfg.MPU9250_DEPTH,
                          debug=use_debug)
        else:
            raise ValueError('unknown IMU_TYPE = {}'.format(str(cfg.IMU_TYPE)))

        # Veihcle上のIMUデータを初期化
        for i in range(len(mpu_items)):
            _item = mpu_items[i]
            _type = mpu_types[i]
            if _type == 'str':
                V.mem[_item] = '{}'
            elif _type == 'float':
                V.mem[_item] = 0.0
            else:
                raise ValueError('unknown type:{}'.format(_type))

        # IMUパーツの追加
        V.add(imu, outputs=mpu_items)
    '''
    2D マップ画像 (usnav/x, usnav/y, imu/recent を使用する)
    '''
    map_items = [
        'usnav/x',
        'usnav/y',
        'imu/recent',
    ]
    map_types = [
        'float',
        'float',
        'str',
    ]
    if use_map or cfg.CAMERA_TYPE == "MAP":
        '''
        2D マップ画像でカメラの代替とする場合
        '''
        if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
            '''
            Marvelmindが有効である場合
            '''
            if (cfg.HAVE_IMU and cfg.IMU_TYPE == 'mpu9250'):
                '''
                MPU9250を使用する場合
                '''
                # 2D マップ生成パーツを追加
                from parts import MapImageCreator
                creator = MapImageCreator(
                    base_image_path=cfg.MAP_BASE_IMAGE_PATH, debug=use_debug)
                V.add(creator, inputs=map_items, outputs=['cam/image_array'])
            else:
                raise ValueError('2D map needs mpu9250 data')
        else:
            raise ValueError('2D map needs Marvelmind USNav data')
    '''
    ジョイスティック
    '''
    user_items = [
        'user/angle',
        'user/throttle',
        'user/lift_throttle',
        'user/mode',
    ]
    user_types = [
        'float',
        'float',
        'float',
        'str',
    ]
    joystick_items = []
    joystick_types = []
    joystick_items += user_items
    joystick_types += user_types
    joystick_items += ['recording']
    joystick_types += ['boolean']
    # ジョイスティックパーツのアウトプット値なのでここでは初期化せず

    if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
        '''
        ジョイスティックを使用する場合
        '''
        # フォークリフト用ジョイスティックパーツ取得ファクトリ関数を呼び出す
        from parts import get_js_controller
        ctr = get_js_controller(cfg)

        if cfg.USE_NETWORKED_JS:
            '''
            ネットワーク経由で操作する場合(Donkeycar標準)
            '''
            from donkeycar.parts.controller import JoyStickSub
            netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
            V.add(netwkJs, threaded=True)
            ctr.js = netwkJs

        # リフト値の追加
        V.add(ctr,
              inputs=['cam/image_array'],
              outputs=joystick_items,
              threaded=True)

    else:
        '''
        Webコントローラを使用する
        '''
        # user/lift_throttle 値が常に0となるWebコントローラパーツを生成
        from parts import LocalWebForkliftController
        ctr = LocalWebForkliftController()

        V.add(ctr,
              inputs=['cam/image_array'],
              outputs=joystick_items,
              threaded=True)

    # 変数ctrはこの後でも使用する
    '''
    スロットルフィルタ(ワンタップESC後進)
    '''
    # 入力値はジョイスティックパーツがアウトプット
    #this throttle filter will allow one tap back for esc reverse
    th_filter = ThrottleFilter()
    V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
    '''
    手動運転・自動運転判定フラグ設定
    '''

    #See if we should even run the pilot module.
    #This is only needed because the part run_condition only accepts boolean
    class PilotCondition:
        def run(self, mode):
            if mode == 'user':
                return False
            else:
                return True

    V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
    # run_pilot 値はboolean(偽:手動運転、真:一部もしくは全て自動運転)
    '''
    3色LED表示
    '''

    class LedConditionLogic:
        def __init__(self, cfg):
            self.cfg = cfg

        def run(self, mode, recording, recording_alert, behavior_state,
                model_file_changed, track_loc):
            #returns a blink rate. 0 for off. -1 for on. positive for rate.

            if track_loc is not None:
                led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
                return -1

            if model_file_changed:
                led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
                            self.cfg.MODEL_RELOADED_LED_G,
                            self.cfg.MODEL_RELOADED_LED_B)
                return 0.1
            else:
                led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)

            if recording_alert:
                led.set_rgb(*recording_alert)
                return self.cfg.REC_COUNT_ALERT_BLINK_RATE
            else:
                led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)

            if behavior_state is not None and model_type == 'behavior':
                r, g, b = self.cfg.BEHAVIOR_LED_COLORS[behavior_state]
                led.set_rgb(r, g, b)
                return -1  #solid on

            if recording:
                return -1  #solid on
            elif mode == 'user':
                return 1
            elif mode == 'local_angle':
                return 0.5
            elif mode == 'local':
                return 0.1
            return 0

    if cfg.HAVE_RGB_LED and not cfg.DONKEY_GYM:
        #from donkeycar.parts.led_status import RGB_LED
        #led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT)
        from parts import RGB_LED
        led = RGB_LED(pgio, cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
                      cfg.LED_INVERT)
        led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)

        V.add(LedConditionLogic(cfg),
              inputs=[
                  'user/mode', 'recording', "records/alert", 'behavior/state',
                  'modelfile/modified', "pilot/loc"
              ],
              outputs=['led/blink_rate'])

        V.add(led, inputs=['led/blink_rate'])

    def get_record_alert_color(num_records):
        """
        Tubデータ件数を色PWMタプル(r, g, b):各0-100に変換する。
        件数範囲、PWMタプルはconfig.py上のRECORD_ALERT_COLOR_ARR
        を参照している。
        引数:
            num_records     Tubデータ件数
        戻り値
            色PWMタプル     (r, g, b)形式(0-100)
        """
        col = (0, 0, 0)
        for count, color in cfg.RECORD_ALERT_COLOR_ARR:
            if num_records >= count:
                col = color
        return col

    '''
    レコードトラッカ
    '''

    class RecordTracker:
        def __init__(self):
            self.last_num_rec_print = 0
            self.dur_alert = 0
            self.force_alert = 0

        def run(self, num_records):
            if num_records is None:
                return 0

            if self.last_num_rec_print != num_records or self.force_alert:
                self.last_num_rec_print = num_records

                if num_records % 10 == 0:
                    print("recorded", num_records, "records")

                if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert:
                    self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
                    self.force_alert = 0

            if self.dur_alert > 0:
                self.dur_alert -= 1

            if self.dur_alert != 0:
                return get_record_alert_color(num_records)

            return 0

    rec_tracker_part = RecordTracker()
    V.add(rec_tracker_part,
          inputs=["tub/num_records"],
          outputs=['records/alert'])
    '''
    自動記録
    '''

    if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
        """
        自動記録指定されておりかつジョイスティックを使用する場合
        """

        #then we are not using the circle button. hijack that to force a record count indication
        def show_record_acount_status():
            rec_tracker_part.last_num_rec_print = 0
            rec_tracker_part.force_alert = 1

        # ジョイスティックのボタンにレコード件数表示用のボタン割当

        # F710
        if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift':
            ctr.set_button_down_trigger('back', show_record_acount_status)
        # JC-U3912T
        elif cfg.CONTROLLER_TYPE == 'JC-U3912T':
            ctr.set_button_down_trigger('7', show_record_acount_status)
        # default
        else:
            ctr.set_button_down_trigger('circle', show_record_acount_status)
    '''
    Sombero HAT
    '''
    #Sombrero
    if cfg.HAVE_SOMBRERO:
        from donkeycar.parts.sombrero import Sombrero
        _ = Sombrero()

    # この段階でカメラ or 2Dマップ画像が cam/image_array に格納されている
    '''
    画像前処理
    '''

    class ImgPreProcess():
        '''
        preprocess camera image for inference.
        normalize and crop if needed.
        '''
        def __init__(self, cfg):
            self.cfg = cfg

        def run(self, img_arr):
            return normalize_and_crop(img_arr, self.cfg)

    if "coral" in model_type:
        '''
        coral を使用する場合は画像をそのまま使用
        '''
        inf_input = 'cam/image_array'
    else:
        '''
        自動運転時のみ正規化・CROP処理する
        '''
        inf_input = 'cam/normalized/cropped'
        V.add(ImgPreProcess(cfg),
              inputs=['cam/image_array'],
              outputs=[inf_input],
              run_condition='run_pilot')

    #Behavioral state
    if cfg.TRAIN_BEHAVIORS:
        '''
        behaviorモデルを使用する場合
        '''

        bh = BehaviorPart(cfg.BEHAVIOR_LIST)
        V.add(bh,
              outputs=[
                  'behavior/state', 'behavior/label',
                  "behavior/one_hot_state_array"
              ])
        try:
            # L1ボタンにbehavior状態更新を割当
            ctr.set_button_down_trigger('L1', bh.increment_state)
        except:
            pass

        inputs = [inf_input, "behavior/one_hot_state_array"]

    #IMU
    elif model_type == "imu":
        '''
        IMU(MPU9250/MPU6050)モデル
        '''

        #assert(cfg.HAVE_IMU)
        #Run the pilot if the mode is not user.
        if cfg.HAVE_IMU:
            # 機械学習モデルのインプットにMPU9250/6050の加速度、角速度を追加
            inputs = [
                'cam/image_array',
                'imu/acl_x',
                'imu/acl_y',
                'imu/acl_z',
                'imu/gyr_x',
                'imu/gyr_y',
                'imu/gyr_z',
            ]
        elif cfg.HAVE_HEDGE and (
                use_hedge or cfg.USE_HEDGE_AS_DEFAULT) and cfg.USE_HEDGE_IMU:
            # 機械学習モデルのインプットにMarvelmindの加速度、角速度を追加
            inputs = [
                'cam/image_array', 'imu/ax', 'imu/ay', 'imu/az', 'imu/vx',
                'imu/vy', 'imu/vz'
            ]
        else:
            raise ValueError('can not use imu model without imu data')
    else:
        # 機械学習モデルのインプットは画像のみ
        inputs = [inf_input]
    '''
    機械学習モデル
    '''

    def load_model(kl, model_path):
        """
        機械学習モデルに学習済みパラメータをロードする。
        引数:
            kl          機械学習モデルオブジェクト
            model_path  学習済みパラーメータファイルのパス
        戻り値:
            なし
        """
        start = time.time()
        print('loading model', model_path)
        kl.load(model_path)
        print('finished loading in %s sec.' % (str(time.time() - start)))

    def load_weights(kl, weights_path):
        """
        機械学習モデルに学習済みパラメータをロードする。
        引数:
            kl          機械学習モデルオブジェクト
            model_path  学習済みパラーメータファイルのパス
        戻り値:
            なし
        """
        start = time.time()
        try:
            print('loading model weights', weights_path)
            kl.model.load_weights(weights_path)
            print('finished loading in %s sec.' % (str(time.time() - start)))
        except Exception as e:
            print(e)
            print('ERR>> problems loading weights', weights_path)

    def load_model_json(kl, json_fnm):
        start = time.time()
        print('loading model json', json_fnm)
        try:
            from tensorflow.python import keras
        except:
            raise
        try:
            with open(json_fnm, 'r') as handle:
                contents = handle.read()
                kl.model = keras.models.model_from_json(contents)
            print('finished loading json in %s sec.' %
                  (str(time.time() - start)))
        except Exception as e:
            print(e)
            print("ERR>> problems loading model json", json_fnm)

    if model_path:
        #When we have a model, first create an appropriate Keras part
        kl = dk.utils.get_model_by_type(model_type, cfg)

        model_reload_cb = None

        if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
            #when we have a .h5 extension
            #load everything from the model file
            load_model(kl, model_path)

            def reload_model(filename):
                load_model(kl, filename)

            model_reload_cb = reload_model

        elif '.json' in model_path:
            #when we have a .json extension
            #load the model from there and look for a matching
            #.wts file with just weights
            load_model_json(kl, model_path)
            weights_path = model_path.replace('.json', '.weights')
            load_weights(kl, weights_path)

            def reload_weights(filename):
                weights_path = filename.replace('.json', '.weights')
                load_weights(kl, weights_path)

            model_reload_cb = reload_weights

        else:
            print("ERR>> Unknown extension type on model file!!")
            return
        '''
        モデルファイル更新監視
        '''

        #this part will signal visual LED, if connected
        V.add(FileWatcher(model_path, verbose=True),
              outputs=['modelfile/modified'])

        #these parts will reload the model file, but only when ai is running so we don't interrupt user driving
        V.add(FileWatcher(model_path),
              outputs=['modelfile/dirty'],
              run_condition="ai_running")
        V.add(DelayedTrigger(100),
              inputs=['modelfile/dirty'],
              outputs=['modelfile/reload'],
              run_condition="ai_running")
        V.add(TriggeredCallback(model_path, model_reload_cb),
              inputs=["modelfile/reload"],
              run_condition="ai_running")

        outputs = ['pilot/angle', 'pilot/throttle']
        '''
        ローカライザモデルの場合の出力編集
        '''

        if cfg.TRAIN_LOCALIZER:
            outputs.append("pilot/loc")

        V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
    '''
    モデルがない場合の自動運転結果初期値設定
    '''

    V.mem['pilot/throttle'] = 0.0
    V.mem['pilot/angle'] = 0.0
    V.mem['pilot/lift_throttle'] = 0.0
    '''
    運転モードから、モータ入力値決定
    '''

    #Choose what inputs should change the car.
    class DriveMode:
        def run(self, mode, user_angle, user_throttle, user_lift_throttle,
                pilot_angle, pilot_throttle, pilot_lift_throttle):
            if mode == 'user':
                return user_angle, user_throttle, user_lift_throttle

            elif mode == 'local_angle':
                return pilot_angle, user_throttle, user_lift_throttle

            else:
                return pilot_angle, (pilot_throttle * cfg.AI_THROTTLE_MULT), (
                    pilot_lift_throttle * cfg.AI_THROTTLE_MULT)

    V.add(DriveMode(),
          inputs=[
              'user/mode', 'user/angle', 'user/throttle', 'user/lift_throttle',
              'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle'
          ],
          outputs=['angle', 'throttle', 'lift_throttle'])

    # Donkeycarへの最終的な指示が angle, throttle, lift_throttle 値となる
    '''
    AIランチャ
    '''

    #to give the car a boost when starting ai mode in a race.
    aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
                          cfg.AI_LAUNCH_KEEP_ENABLED)

    V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])

    if isinstance(ctr, JoystickController):
        ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
                                    aiLauncher.enable_ai_launch)
    '''
    AIによる自動運転かどうかの判定
    '''

    class AiRunCondition:
        '''
        A bool part to let us know when ai is running.
        '''
        def run(self, mode):
            if mode == "user":
                return False
            return True

    V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
    '''
    AI運転時の記録モード
    '''

    #Ai Recording
    class AiRecordingCondition:
        '''
        return True when ai mode, otherwize respect user mode recording flag
        '''
        def run(self, mode, recording):
            if mode == 'user':
                return recording
            return True

    if cfg.RECORD_DURING_AI:
        V.add(AiRecordingCondition(),
              inputs=['user/mode', 'recording'],
              outputs=['recording'])
    '''
    モータ駆動
    '''

    #Drive train setup
    if cfg.DONKEY_GYM:
        pass

    elif cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC":
        from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle

        steering_controller = PCA9685(cfg.STEERING_CHANNEL,
                                      cfg.PCA9685_I2C_ADDR,
                                      busnum=cfg.PCA9685_I2C_BUSNUM)
        steering = PWMSteering(controller=steering_controller,
                               left_pulse=cfg.STEERING_LEFT_PWM,
                               right_pulse=cfg.STEERING_RIGHT_PWM)

        throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
                                      cfg.PCA9685_I2C_ADDR,
                                      busnum=cfg.PCA9685_I2C_BUSNUM)
        throttle = PWMThrottle(controller=throttle_controller,
                               max_pulse=cfg.THROTTLE_FORWARD_PWM,
                               zero_pulse=cfg.THROTTLE_STOPPED_PWM,
                               min_pulse=cfg.THROTTLE_REVERSE_PWM)

        V.add(steering, inputs=['angle'])
        V.add(throttle, inputs=['throttle'])
        V.mem['lift_throttle'] = 0

    elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE":
        from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM

        steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT,
                                             cfg.HBRIDGE_PIN_RIGHT)
        throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
                                             cfg.HBRIDGE_PIN_BWD)

        V.add(steering, inputs=['angle'])
        V.add(throttle, inputs=['throttle'])
        V.mem['lift_throttle'] = 0

    elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
        from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM

        left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD,
                                               cfg.HBRIDGE_PIN_LEFT_BWD)
        right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD,
                                                cfg.HBRIDGE_PIN_RIGHT_BWD)
        two_wheel_control = TwoWheelSteeringThrottle()

        V.add(two_wheel_control,
              inputs=['throttle', 'angle'],
              outputs=['left_motor_speed', 'right_motor_speed'])

        V.add(left_motor, inputs=['left_motor_speed'])
        V.add(right_motor, inputs=['right_motor_speed'])
        V.mem['lift_motor_throttle'] = 0

    # Forklift 駆動モータ操作
    elif cfg.DRIVE_TRAIN_TYPE == "THREE_MOTORS_PIGPIO":
        '''
        フォークリフト3モータ駆動
        '''

        from parts import PIGPIO_OUT, PIGPIO_PWM, ForkliftMotorDriver

        motor_driver = ForkliftMotorDriver(left_balance=cfg.LEFT_PWM_BALANCE,
                                           right_balance=cfg.RIGHT_PWM_BALANCE,
                                           debug=False)
        V.add(motor_driver,
              inputs=['throttle', 'angle', 'lift_throttle'],
              outputs=[
                  'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
                  'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
                  'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
              ])

        if use_debug:

            class Prt:
                def run(self, left_vref, left_in1, left_in2, right_vref,
                        right_in1, right_in2, lift_vref, lift_in1, lift_in2):
                    print('ForkliftMD left   vref:{}, in1:{}, in2:{}'.format(
                        str(left_vref), str(left_in1), str(left_in2)))
                    print('ForkliftMD right  vref:{}, in1:{}, in2:{}'.format(
                        str(right_vref), str(right_in1), str(right_in2)))
                    print('ForkliftMD lift   vref:{}, in1:{}, in2:{}'.format(
                        str(lift_vref), str(lift_in1), str(lift_in2)))

            V.add(Prt(),
                  inputs=[
                      'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
                      'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
                      'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
                  ])

        # TB6612#1 A系(右モータ、左折時駆動する)
        V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio),
              inputs=['left_motor_in1'])
        V.add(PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio),
              inputs=['left_motor_in2'])
        V.add(PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO,
                         pgio=pgio,
                         freq=cfg.PWM_FREQ,
                         range=cfg.PWM_RANGE,
                         threshold=cfg.PWM_INPUT_THRESHOLD),
              inputs=['left_motor_vref'])

        # TB6612#1 B系(左モータ、右折時駆動する)
        V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio),
              inputs=['right_motor_in1'])
        V.add(PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio),
              inputs=['right_motor_in2'])
        V.add(PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO,
                         pgio=pgio,
                         freq=cfg.PWM_FREQ,
                         range=cfg.PWM_RANGE,
                         threshold=cfg.PWM_INPUT_THRESHOLD),
              inputs=['right_motor_vref'])

        # TB6612#2 A系(リフトモータ)
        V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN1_GPIO, pgio=pgio),
              inputs=['lift_motor_in1'])
        V.add(PIGPIO_OUT(pin=cfg.LIFT_MOTOR_IN2_GPIO, pgio=pgio),
              inputs=['lift_motor_in2'])
        V.add(PIGPIO_PWM(pin=cfg.LIFT_MOTOR_PWM_GPIO,
                         pgio=pgio,
                         freq=cfg.PWM_FREQ,
                         range=cfg.PWM_RANGE,
                         threshold=cfg.PWM_INPUT_THRESHOLD),
              inputs=['lift_motor_vref'])

    elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM":
        from donkeycar.parts.actuator import ServoBlaster, PWMSteering
        steering_controller = ServoBlaster(cfg.STEERING_CHANNEL)  #really pin
        #PWM pulse values should be in the range of 100 to 200
        assert (cfg.STEERING_LEFT_PWM <= 200)
        assert (cfg.STEERING_RIGHT_PWM <= 200)
        steering = PWMSteering(controller=steering_controller,
                               left_pulse=cfg.STEERING_LEFT_PWM,
                               right_pulse=cfg.STEERING_RIGHT_PWM)

        from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
        motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
                                          cfg.HBRIDGE_PIN_BWD)

        V.add(steering, inputs=['angle'])
        V.add(motor, inputs=["throttle"])
        V.mem['lift_throttle'] = 0
    '''
    Tubデータ
    '''
    # ベースとなるTubデータ
    if write_both_images:
        from parts.datastore import FWD_CAMERA_KEY
        inputs = ['cam/image_array', FWD_CAMERA_KEY]
        types = ['image_array', 'image_array']
    else:
        inputs = ['cam/image_array']
        types = ['image_array']
    inputs += user_items
    types += user_types

    if cfg.TRAIN_BEHAVIORS:
        '''
        behaviorモデルを使用する場合の入力データを追加
        '''
        inputs += [
            'behavior/state', 'behavior/label', 'behavior/one_hot_state_array'
        ]
        types += ['int', 'str', 'vector']
    '''
    IMU(MPU6050)データ追加
    '''

    # Tubへ格納する加速度、角速度
    tub_imu_inputs = [
        'imu/acl_x',
        'imu/acl_y',
        'imu/acl_z',
        'imu/gyr_x',
        'imu/gyr_y',
        'imu/gyr_z',
    ]
    tub_imu_input_types = [
        'float',
        'float',
        'float',
        'float',
        'float',
        'float',
    ]
    if not cfg.HAVE_IMU:
        '''
            MPU9250/MPU6050を持っていない場合
        '''
        if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
            '''
            Marvelmind が有効な場合
            '''
            if cfg.USE_HEDGE_IMU:
                '''
                Marvelmind IMUが使用可能な場合
                '''

                # 必要な Marvelmind IMUデータを移動させる
                class MoveIMU:
                    def run(self, ax, ay, az, gx, gy, gz):
                        return ax, ay, az, gx, gy, gz

                move = MoveIMU()
                V.add(move,
                      inputs=[
                          'imu/ax',
                          'imu/ay',
                          'imu/az',
                          'imu/gx',
                          'imu/gy',
                          'imu/gz',
                      ],
                      outputs=tub_imu_inputs)

            inputs += tub_imu_inputs
            types += tub_imu_input_types
    else:
        inputs += tub_imu_inputs
        types += tub_imu_input_types

    if cfg.RECORD_DURING_AI:
        '''
        自動運転中であっても記録しておくモードの場合
        '''
        # pilot/* を Tub データに加える
        inputs += ['pilot/angle', 'pilot/throttle', 'pilot/lift_throttle']
        types += ['float', 'float', 'float']
    '''
    Tubデータ書き込み
    '''
    th = TubHandler(path=cfg.DATA_PATH)
    if write_both_images:
        from parts.datastore import TubHandler as NewTubHandler
        th = NewTubHandler(path=cfg.DATA_PATH)
    tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
    V.add(tub,
          inputs=inputs,
          outputs=["tub/num_records"],
          run_condition='recording')

    if cfg.PUB_CAMERA_IMAGES:
        '''
        カメライメージを通信して貰う場合(Donkeycar標準)
        '''
        from donkeycar.parts.network import TCPServeValue
        from donkeycar.parts.image import ImgArrToJpg
        pub = TCPServeValue("camera")
        V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
        V.add(pub, inputs=['jpg/bin'])
    '''
    操縦系統別
    '''

    if type(ctr) is LocalWebController:
        '''
        LocalWebController を使用している場合の usage を表示
        '''
        print("You can now go to <your pi ip address>:8887 to drive your car.")
    elif isinstance(ctr, JoystickController):
        '''
        ジョイスティックを使用している場合の usage を表示
        '''
        print("You can now move your joystick to drive your car.")
        #tell the controller about the tub
        ctr.set_tub(tub)

        if cfg.BUTTON_PRESS_NEW_TUB:
            '''
            cfg.BUTTON_PRESS_NEW_TUB が True の場合、ボタンを押すごとに別のTubディレクトリに
            データが格納されるようになる(デフォルト:False)。
            '''
            def new_tub_dir():
                V.parts.pop()
                tub = th.new_tub_writer(inputs=inputs,
                                        types=types,
                                        user_meta=meta)
                V.add(tub,
                      inputs=inputs,
                      outputs=["tub/num_records"],
                      run_condition='recording')
                ctr.set_tub(tub)

            # ボタン割当
            # F710
            if cfg.CONTROLLER_TYPE == 'F710' or cfg.CONTROLLER_TYPE == 'F710_Forklift':
                ctr.set_button_down_trigger('A', new_tub_dir)
            # JC-U3912T
            elif cfg.CONTROLLER_TYPE == 'JC-U3912T' or cfg.CONTROLLER_TYPE == 'JC-U3912T_Forklift':
                ctr.set_button_down_trigger('12', new_tub_dir)
            # default
            else:
                ctr.set_button_down_trigger('cross', new_tub_dir)

        # 現時点のボタン割当状態を表示
        ctr.print_controls()
    """
    AWS IoT Core
    """
    if use_aws or cfg.USE_AWS_AS_DEFAULT:
        print('Start aws configuration')
        from parts.broker import AWSShadowClientFactory, PowerReporter
        factory = AWSShadowClientFactory(cfg.AWS_CONFIG_PATH,
                                         cfg.AWS_THING_NAME)
        # Power ON 情報の送信
        power = PowerReporter(factory, debug=use_debug)
        power.on()

        # Tubデータ(json)送信
        from parts.broker.pub import Publisher
        pub_tub = Publisher(factory, debug=use_debug)
        V.add(pub_tub,
              inputs=[
                  'user/angle', 'user/throttle', 'user/lift_throttle',
                  'pilot/angle', 'pilot/throttle', 'pilot/lift_throttle',
                  'user/mode'
              ])

        # Tubデータ(イメージ)送信
        from parts.broker.pub import ImagePublisher
        pub_img = ImagePublisher(factory, debug=use_debug)
        V.add(pub_img, inputs=[
            'cam/image_array',
        ])

        # fwdイメージデータ送信
        if write_both_images:
            from parts.broker.pub import FwdImagePublisher
            pub_fwd_img = FwdImagePublisher(factory, debug=use_debug)
            from parts.datastore import FWD_CAMERA_KEY
            V.add(pub_fwd_img, inputs=[
                FWD_CAMERA_KEY,
            ])
        '''
        ジョイスティックデータの送信
        '''
        if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
            '''
            ジョイスティックを使用している場合
            '''
            from parts.broker.pub import JoystickPublisher
            pub_joy = JoystickPublisher(factory, debug=use_debug)
            V.add(pub_joy, inputs=joystick_items)
        '''
        Marvelmind システムデータの送信
        '''
        if cfg.HAVE_HEDGE and (use_hedge or cfg.USE_HEDGE_AS_DEFAULT):
            '''
            Marvelmind システムを使用している場合
            '''
            if cfg.USE_HEDGE_USNAV:
                '''
                Marvelmind 位置情報を使用している場合
                '''
                from parts.broker.pub import USNavPublisher
                pub_usn = USNavPublisher(factory, debug=use_debug)
                V.add(pub_usn, inputs=usnav_items)

            if cfg.USE_HEDGE_USNAV_RAW:
                '''
                Marvelmind 距離情報を使用している場合
                '''
                from parts.broker.pub import USNavRawPublisher
                pub_raw = USNavRawPublisher(factory, debug=use_debug)
                V.add(pub_raw, inputs=usnav_raw_items)

            if cfg.USE_HEDGE_IMU:
                '''
                Marvelmind IMU情報を使用している場合
                '''
                from parts.broker.pub import IMUPublisher
                pub_imu = IMUPublisher(factory, debug=use_debug)
                V.add(pub_imu, inputs=imu_items)
        '''
        MPU9250/MPU6050 データの送信
        '''
        if cfg.HAVE_IMU:
            '''
            MPU9250/MPU6050 を使用している場合
            '''
            if cfg.IMU_TYPE == 'mpu6050':
                '''
                MPU6050を使用している場合
                '''
                from parts.broker.pub import Mpu6050Publisher
                pub_mpu = Mpu6050Publisher(factory, debug=use_debug)
                V.add(pub_mpu, inputs=mpu_items)

            elif cfg.IMU_TYPE == 'mpu9250':
                '''
                MPU6050を使用している場合
                '''
                from parts.broker.pub import Mpu9250Publisher
                pub_mpu = Mpu9250Publisher(factory, debug=use_debug)
                V.add(pub_mpu, inputs=mpu_items)
    '''
    運転ループ
    '''

    try:
        print('Start running')
        #run the vehicle for 20 seconds
        V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
    except KeyboardInterrupt:
        # Ctrl+C押下時
        pass
    finally:
        # デバッグ
        if use_debug:
            print('Stop running')
        # pigpio 利用停止
        pgio.stop()
        if use_aws or cfg.USE_AWS_AS_DEFAULT:
            if power is not None:
                # Power Off 情報の送信
                if use_debug:
                    print('Sending power off')
                power.off()
                # 送信するまで待機
                time.sleep(1)
        print('Stopped')
Beispiel #5
0
def drive_straight(cfg, pgio, left_balance=1.0, right_balance=1.0):

    from parts import PIGPIO_OUT, PIGPIO_PWM
    left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio)
    left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio)
    left_pwm = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO,
                          pgio=pgio,
                          freq=cfg.PWM_FREQ,
                          range=cfg.PWM_RANGE,
                          threshold=cfg.PWM_INPUT_THRESHOLD)

    right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio)
    right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio)
    right_pwm = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=cfg.PWM_FREQ,
                           range=cfg.PWM_RANGE,
                           threshold=cfg.PWM_INPUT_THRESHOLD)

    from parts import ForkliftMotorDriver
    driver = ForkliftMotorDriver(left_balance=left_balance,
                                 right_balance=right_balance)
    left_pwm_v, left_in1_v, left_in2_v, right_pwm_v, right_in1_v, right_in2_v, _, _, _ = driver.run(
        0.5, 0.0, 0.0)

    print('drive start')
    left_in1.run(left_in1_v)
    left_in2.run(left_in2_v)
    right_in1.run(right_in1_v)
    right_in2.run(right_in2_v)
    left_pwm.run(left_pwm_v)
    right_pwm.run(right_pwm_v)

    sleep(3)
    left_pwm.run(0)
    right_pwm.run(0)
    print('drive end')
Beispiel #6
0
def test_motor():
    import pigpio
    pgio = pigpio.pi()
    from parts import PIGPIO_OUT, PIGPIO_PWM
    right_in1 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN1_GPIO, pgio=pgio)
    right_in2 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN2_GPIO, pgio=pgio)
    right_vref = PIGPIO_PWM(pin=RIGHT_MOTOR_PWM_GPIO,
                            pgio=pgio,
                            freq=PWM_FREQ,
                            range=PWM_RANGE)

    right_in1.run(1.0)
    right_in2.run(0.0)
    right_vref.run(0.5)
    print('right fwd')
    sleep(1)
    right_in1.run(0.0)
    right_in2.run(0.0)
    right_vref.run(0.0)
    print('right stop')
    sleep(3)

    left_in1 = PIGPIO_OUT(pin=LEFT_MOTOR_IN1_GPIO, pgio=pgio)
    left_in2 = PIGPIO_OUT(pin=LEFT_MOTOR_IN2_GPIO, pgio=pgio)
    left_vref = PIGPIO_PWM(pin=LEFT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=PWM_FREQ,
                           range=PWM_RANGE)

    left_in1.run(1.0)
    left_in2.run(0.0)
    left_vref.run(1.0)
    print('left fwd')
    sleep(1)
    left_in1.run(0.0)
    left_in2.run(0.0)
    left_vref.run(0.0)
    print('left stop')
    sleep(3)

    lift_in1 = PIGPIO_OUT(pin=LIFT_MOTOR_IN1_GPIO, pgio=pgio)
    lift_in2 = PIGPIO_OUT(pin=LIFT_MOTOR_IN2_GPIO, pgio=pgio)
    lift_vref = PIGPIO_PWM(pin=LIFT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=PWM_FREQ,
                           range=PWM_RANGE)

    lift_in1.run(1.0)
    lift_in2.run(0.0)
    lift_vref.run(1.0)
    print('lift fwd')
    sleep(2)
    lift_in1.run(0.0)
    lift_in2.run(0.0)
    lift_vref.run(0.0)
    print('lift stop')
    #sleep(3)

    pgio.stop()
Beispiel #7
0
def test_lift():
    import donkeycar as dk
    V = dk.vehicle.Vehicle()

    import pigpio
    pgio = pigpio.pi()

    from parts import PIGPIO_OUT, PIGPIO_PWM, ForkliftMotorDriver  #, CalibrateAngle

    left_in1 = PIGPIO_OUT(pin=LEFT_MOTOR_IN1_GPIO, pgio=pgio)
    left_in2 = PIGPIO_OUT(pin=LEFT_MOTOR_IN2_GPIO, pgio=pgio)
    left_vref = PIGPIO_PWM(pin=LEFT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=PWM_FREQ,
                           range=PWM_RANGE)

    right_in1 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN1_GPIO, pgio=pgio)
    right_in2 = PIGPIO_OUT(pin=RIGHT_MOTOR_IN2_GPIO, pgio=pgio)
    right_vref = PIGPIO_PWM(pin=RIGHT_MOTOR_PWM_GPIO,
                            pgio=pgio,
                            freq=PWM_FREQ,
                            range=PWM_RANGE)

    lift_in1 = PIGPIO_OUT(pin=LIFT_MOTOR_IN1_GPIO, pgio=pgio)
    lift_in2 = PIGPIO_OUT(pin=LIFT_MOTOR_IN2_GPIO, pgio=pgio)
    lift_vref = PIGPIO_PWM(pin=LIFT_MOTOR_PWM_GPIO,
                           pgio=pgio,
                           freq=PWM_FREQ,
                           range=PWM_RANGE)

    V.add(ThrottleTest(), outputs=['throttle', 'steering', 'lift_throttle'])

    #V.add(CalibrateAngle(diff=0.00, debug=False), inputs=['steering'], outputs=['steering'])
    #V.mem['dummy_gpio'] = 0

    driver = ForkliftMotorDriver(debug=True)
    V.add(driver,
          inputs=['throttle', 'steering', 'lift_throttle'],
          outputs=[
              'left_motor_vref', 'left_motor_in1', 'left_motor_in2',
              'right_motor_vref', 'right_motor_in1', 'right_motor_in2',
              'lift_motor_vref', 'lift_motor_in1', 'lift_motor_in2'
          ])

    V.add(lift_in1, inputs=['lift_motor_in1'])
    V.add(lift_in2, inputs=['lift_motor_in2'])
    V.add(lift_vref, inputs=['lift_motor_vref'])

    V.add(left_in1, inputs=['left_motor_in1'])
    V.add(left_in2, inputs=['left_motor_in2'])
    V.add(left_vref, inputs=['left_motor_vref'])

    V.add(right_in1, inputs=['right_motor_in1'])
    V.add(right_in2, inputs=['right_motor_in2'])
    V.add(right_vref, inputs=['right_motor_vref'])

    try:
        #V.mem['lift_throttle'] = 0.0
        V.start(rate_hz=20, max_loop_count=2000)

    finally:
        V.mem['throttle'] = 0.0
        V.mem['lift_throttle'] = 0.0
        sleep(1)
        pgio.stop()
        print('test stop')
Beispiel #8
0
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', use_debug=False, meta=[] ):
    '''
    Construct a working robotic vehicle from many parts.
    Each part runs as a job in the Vehicle loop, calling either
    it's run or run_threaded method depending on the constructor flag `threaded`.
    All parts are updated one after another at the framerate given in
    cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
    Parts may have named outputs and inputs. The framework handles passing named outputs
    to parts requesting the same named input.
    '''

    if cfg.DONKEY_GYM:
        #the simulator will use cuda and then we usually run out of resources
        #if we also try to use cuda. so disable for donkey_gym.
        os.environ["CUDA_VISIBLE_DEVICES"]="-1" 

    if model_type is None:
        if cfg.TRAIN_LOCALIZER:
            model_type = "localizer"
        elif cfg.TRAIN_BEHAVIORS:
            model_type = "behavior"
        else:
            model_type = cfg.DEFAULT_MODEL_TYPE
    
    #Initialize car
    V = dk.vehicle.Vehicle()

    if camera_type == "stereo":

        if cfg.CAMERA_TYPE == "WEBCAM":
            from donkeycar.parts.camera import Webcam            

            camA = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 0)
            camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 1)

        elif cfg.CAMERA_TYPE == "CVCAM":
            from donkeycar.parts.cv import CvCam

            camA = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 0)
            camB = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam = 1)
        else:
            raise(Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE))

        V.add(camA, outputs=['cam/image_array_a'], threaded=True)
        V.add(camB, outputs=['cam/image_array_b'], threaded=True)

        from donkeycar.parts.image import StereoPair

        V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'], 
            outputs=['cam/image_array'])

    else:
        print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
        if cfg.DONKEY_GYM:
            from donkeycar.parts.dgym import DonkeyGymEnv 
        
        inputs = []
        threaded = True
        print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
        if cfg.DONKEY_GYM:
            from donkeycar.parts.dgym import DonkeyGymEnv 
            cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, env_name=cfg.DONKEY_GYM_ENV_NAME)
            threaded = True
            inputs = ['angle', 'throttle']
        elif cfg.CAMERA_TYPE == "PICAM":
            from donkeycar.parts.camera import PiCamera
            cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "WEBCAM":
            from donkeycar.parts.camera import Webcam
            cam = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "CVCAM":
            from donkeycar.parts.cv import CvCam
            cam = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
        elif cfg.CAMERA_TYPE == "CSIC":
            from donkeycar.parts.camera import CSICamera
            cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE, gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
        elif cfg.CAMERA_TYPE == "V4L":
            from donkeycar.parts.camera import V4LCamera
            cam = V4LCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
        elif cfg.CAMERA_TYPE == "MOCK":
            from donkeycar.parts.camera import MockCamera
            cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
        else:
            raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
            
        V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
        
    if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
        #modify max_throttle closer to 1.0 to have more power
        #modify steering_scale lower than 1.0 to have less responsive steering
        # 独自のファクトリ関数に変更
        #from donkeycar.parts.controller import get_js_controller
        from parts import get_js_controller
        
        ctr = get_js_controller(cfg)
        
        if cfg.USE_NETWORKED_JS:
            from donkeycar.parts.controller import JoyStickSub
            netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
            V.add(netwkJs, threaded=True)
            ctr.js = netwkJs

    else:        
        #This web controller will create a web server that is capable
        #of managing steering, throttle, and modes, and more.
        ctr = LocalWebController()

    
    V.add(ctr, 
          inputs=['cam/image_array'],
          outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
          threaded=True)

    #this throttle filter will allow one tap back for esc reverse
    th_filter = ThrottleFilter()
    V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
    
    #See if we should even run the pilot module. 
    #This is only needed because the part run_condition only accepts boolean
    class PilotCondition:
        def run(self, mode):
            if mode == 'user':
                return False
            else:
                return True       

    V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
    
    class LedConditionLogic:
        def __init__(self, cfg):
            self.cfg = cfg

        def run(self, mode, recording, recording_alert, behavior_state, model_file_changed, track_loc):
            #returns a blink rate. 0 for off. -1 for on. positive for rate.
            
            if track_loc is not None:
                led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
                return -1

            if model_file_changed:
                led.set_rgb(self.cfg.MODEL_RELOADED_LED_R, self.cfg.MODEL_RELOADED_LED_G, self.cfg.MODEL_RELOADED_LED_B)
                return 0.1
            else:
                led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)

            if recording_alert:
                led.set_rgb(*recording_alert)
                return self.cfg.REC_COUNT_ALERT_BLINK_RATE
            else:
                led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
        
            if behavior_state is not None and model_type == 'behavior':
                r, g, b = self.cfg.BEHAVIOR_LED_COLORS[behavior_state]
                led.set_rgb(r, g, b)
                return -1 #solid on

            if recording:
                return -1 #solid on
            elif mode == 'user':
                return 1
            elif mode == 'local_angle':
                return 0.5
            elif mode == 'local':
                return 0.1
            return 0

    if cfg.HAVE_RGB_LED and not cfg.DONKEY_GYM:
        from donkeycar.parts.led_status import RGB_LED
        led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT)
        led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)        
        
        V.add(LedConditionLogic(cfg), inputs=['user/mode', 'recording', "records/alert", 'behavior/state', 'modelfile/modified', "pilot/loc"],
              outputs=['led/blink_rate'])

        V.add(led, inputs=['led/blink_rate'])
        

    def get_record_alert_color(num_records):
        col = (0, 0, 0)
        for count, color in cfg.RECORD_ALERT_COLOR_ARR:
            if num_records >= count:
                col = color
        return col    

    class RecordTracker:
        def __init__(self):
            self.last_num_rec_print = 0
            self.dur_alert = 0
            self.force_alert = 0

        def run(self, num_records):
            if num_records is None:
                return 0
            
            if self.last_num_rec_print != num_records or self.force_alert:
                self.last_num_rec_print = num_records

                if num_records % 10 == 0:
                    print("recorded", num_records, "records")
                        
                if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert:
                    self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
                    self.force_alert = 0
                    
            if self.dur_alert > 0:
                self.dur_alert -= 1

            if self.dur_alert != 0:
                return get_record_alert_color(num_records)

            return 0

    rec_tracker_part = RecordTracker()
    V.add(rec_tracker_part, inputs=["tub/num_records"], outputs=['records/alert'])

    if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
        #then we are not using the circle button. hijack that to force a record count indication
        def show_record_acount_status():
            rec_tracker_part.last_num_rec_print = 0
            rec_tracker_part.force_alert = 1
        ctr.set_button_down_trigger('circle', show_record_acount_status)

    #Sombrero
    if cfg.HAVE_SOMBRERO:
        from donkeycar.parts.sombrero import Sombrero
        s = Sombrero()

    #IMU
    if cfg.HAVE_IMU:
        from donkeycar.parts.imu import Mpu6050
        imu = Mpu6050()
        V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
            'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True)

    class ImgPreProcess():
        '''
        preprocess camera image for inference.
        normalize and crop if needed.
        '''
        def __init__(self, cfg):
            self.cfg = cfg

        def run(self, img_arr):
            return normalize_and_crop(img_arr, self.cfg)

    if "coral" in model_type:
        inf_input = 'cam/image_array'
    else:
        inf_input = 'cam/normalized/cropped'
        V.add(ImgPreProcess(cfg),
            inputs=['cam/image_array'],
            outputs=[inf_input],
            run_condition='run_pilot')

    #Behavioral state
    if cfg.TRAIN_BEHAVIORS:
        bh = BehaviorPart(cfg.BEHAVIOR_LIST)
        V.add(bh, outputs=['behavior/state', 'behavior/label', "behavior/one_hot_state_array"])
        try:
            ctr.set_button_down_trigger('L1', bh.increment_state)
        except:
            pass

        inputs = [inf_input, "behavior/one_hot_state_array"]  
    #IMU
    elif model_type == "imu":
        assert(cfg.HAVE_IMU)
        #Run the pilot if the mode is not user.
        inputs=[inf_input,
            'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
            'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z']
    else:
        inputs=[inf_input]

    def load_model(kl, model_path):
        start = time.time()
        print('loading model', model_path)
        kl.load(model_path)
        print('finished loading in %s sec.' % (str(time.time() - start)) )

    def load_weights(kl, weights_path):
        start = time.time()
        try:
            print('loading model weights', weights_path)
            kl.model.load_weights(weights_path)
            print('finished loading in %s sec.' % (str(time.time() - start)) )
        except Exception as e:
            print(e)
            print('ERR>> problems loading weights', weights_path)

    def load_model_json(kl, json_fnm):
        start = time.time()
        print('loading model json', json_fnm)
        try:
            from tensorflow.python import keras
        except:
            raise
        try:
            with open(json_fnm, 'r') as handle:
                contents = handle.read()
                kl.model = keras.models.model_from_json(contents)
            print('finished loading json in %s sec.' % (str(time.time() - start)) )
        except Exception as e:
            print(e)
            print("ERR>> problems loading model json", json_fnm)

    if model_path:
        #When we have a model, first create an appropriate Keras part
        kl = dk.utils.get_model_by_type(model_type, cfg)

        model_reload_cb = None

        if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
            #when we have a .h5 extension
            #load everything from the model file
            load_model(kl, model_path)

            def reload_model(filename):
                load_model(kl, filename)

            model_reload_cb = reload_model

        elif '.json' in model_path:
            #when we have a .json extension
            #load the model from there and look for a matching
            #.wts file with just weights
            load_model_json(kl, model_path)
            weights_path = model_path.replace('.json', '.weights')
            load_weights(kl, weights_path)

            def reload_weights(filename):
                weights_path = filename.replace('.json', '.weights')
                load_weights(kl, weights_path)
            
            model_reload_cb = reload_weights

        else:
            print("ERR>> Unknown extension type on model file!!")
            return

        #this part will signal visual LED, if connected
        V.add(FileWatcher(model_path, verbose=True), outputs=['modelfile/modified'])

        #these parts will reload the model file, but only when ai is running so we don't interrupt user driving
        V.add(FileWatcher(model_path), outputs=['modelfile/dirty'], run_condition="ai_running")
        V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running")
        V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running")

        outputs=['pilot/angle', 'pilot/throttle']

        if cfg.TRAIN_LOCALIZER:
            outputs.append("pilot/loc")
    
        V.add(kl, inputs=inputs, 
            outputs=outputs,
            run_condition='run_pilot')            
    
    #Choose what inputs should change the car.
    class DriveMode:
        def run(self, mode, 
                    user_angle, user_throttle,
                    pilot_angle, pilot_throttle):
            if mode == 'user': 
                return user_angle, user_throttle
            
            elif mode == 'local_angle':
                return pilot_angle, user_throttle
            
            else: 
                return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
        
    V.add(DriveMode(), 
          inputs=['user/mode', 'user/angle', 'user/throttle',
                  'pilot/angle', 'pilot/throttle'], 
          outputs=['angle', 'throttle'])

    
    #to give the car a boost when starting ai mode in a race.
    aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE, cfg.AI_LAUNCH_KEEP_ENABLED)
    
    V.add(aiLauncher,
        inputs=['user/mode', 'throttle'],
        outputs=['throttle'])

    if isinstance(ctr, JoystickController):
        ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON, aiLauncher.enable_ai_launch)


    class AiRunCondition:
        '''
        A bool part to let us know when ai is running.
        '''
        def run(self, mode):
            if mode == "user":
                return False
            return True

    V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])

    #Ai Recording
    class AiRecordingCondition:
        '''
        return True when ai mode, otherwize respect user mode recording flag
        '''
        def run(self, mode, recording):
            if mode == 'user':
                return recording
            return True

    if cfg.RECORD_DURING_AI:
        V.add(AiRecordingCondition(), inputs=['user/mode', 'recording'], outputs=['recording'])
    
    #Drive train setup
    if cfg.DONKEY_GYM:
        pass

    elif cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC":
        from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle

        steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM)
        steering = PWMSteering(controller=steering_controller,
                                        left_pulse=cfg.STEERING_LEFT_PWM, 
                                        right_pulse=cfg.STEERING_RIGHT_PWM)
        
        throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM)
        throttle = PWMThrottle(controller=throttle_controller,
                                        max_pulse=cfg.THROTTLE_FORWARD_PWM,
                                        zero_pulse=cfg.THROTTLE_STOPPED_PWM, 
                                        min_pulse=cfg.THROTTLE_REVERSE_PWM)

        V.add(steering, inputs=['angle'])
        V.add(throttle, inputs=['throttle'])
    

    elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE":
        from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
        
        steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT, cfg.HBRIDGE_PIN_RIGHT)
        throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD)

        V.add(steering, inputs=['angle'])
        V.add(throttle, inputs=['throttle'])
    

    elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
        from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM

        left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD)
        right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD)
        two_wheel_control = TwoWheelSteeringThrottle()

        V.add(two_wheel_control, 
                inputs=['throttle', 'angle'],
                outputs=['left_motor_speed', 'right_motor_speed'])

        V.add(left_motor, inputs=['left_motor_speed'])
        V.add(right_motor, inputs=['right_motor_speed'])

    elif cfg.DRIVE_TRAIN_TYPE == "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"])
    # DC2モータ pigpio制御、TB6612 ジャンパ設定無し
    elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL_PIGPIO":
        try:
            import pigpio
        except:
            raise
        # pigpio 制御開始
        pgio = pigpio.pi()

        from parts import PIGPIO_OUT, PIGPIO_PWM, CaterpillerMotorDriver

        # TB6612 STBY ピン初期化
        stby = PIGPIO_OUT(pin=cfg.TB6612_STBY_GPIO, pgio=pgio, debug=use_debug)
        stby.run(1)

        # ジョイスティック出力値をDCモータ入力値に変換
        driver = CaterpillerMotorDriver(
            left_balance=cfg.LEFT_PWM_BALANCE, 
            right_balance=cfg.RIGHT_PWM_BALANCE,
            debug=use_debug)
        V.add(driver, 
                inputs=['throttle', 'angle'],
                outputs=['left_motor_vref', 'left_motor_in1', 'left_motor_in2',
                'right_motor_vref', 'right_motor_in1', 'right_motor_in2'])

        # 左モータ制御
        left_in1 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug)
        left_in2 = PIGPIO_OUT(pin=cfg.LEFT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug)
        left_vref = PIGPIO_PWM(pin=cfg.LEFT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug)
        V.add(left_in1, inputs=['left_motor_in1'])
        V.add(left_in2, inputs=['left_motor_in2'])
        V.add(left_vref, inputs=['left_motor_vref'])  

        # 右モータ制御
        right_in1 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN1_GPIO, pgio=pgio, debug=use_debug)
        right_in2 = PIGPIO_OUT(pin=cfg.RIGHT_MOTOR_IN2_GPIO, pgio=pgio, debug=use_debug)
        right_vref = PIGPIO_PWM(pin=cfg.RIGHT_MOTOR_PWM_GPIO, pgio=pgio, freq=cfg.PWM_FREQ, range=cfg.PWM_RANGE, debug=use_debug)
        V.add(right_in1, inputs=['right_motor_in1'])
        V.add(right_in2, inputs=['right_motor_in2'])
        V.add(right_vref, inputs=['right_motor_vref'])

    
    #add tub to save data

    inputs=['cam/image_array',
            'user/angle', 'user/throttle', 
            'user/mode']

    types=['image_array',
           'float', 'float',
           'str']

    if cfg.TRAIN_BEHAVIORS:
        inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]
        types += ['int', 'str', 'vector']
    
    if cfg.HAVE_IMU:
        inputs += ['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
            'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z']

        types +=['float', 'float', 'float',
           'float', 'float', 'float']

    if cfg.RECORD_DURING_AI:
        inputs += ['pilot/angle', 'pilot/throttle']
        types += ['float', 'float']
    
    th = TubHandler(path=cfg.DATA_PATH)
    tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
    V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')

    if cfg.PUB_CAMERA_IMAGES:
        from donkeycar.parts.network import TCPServeValue
        from donkeycar.parts.image import ImgArrToJpg
        pub = TCPServeValue("camera")
        V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
        V.add(pub, inputs=['jpg/bin'])

    if type(ctr) is LocalWebController:
        print("You can now go to <your pis hostname.local>:8887 to drive your car.")
    elif isinstance(ctr, JoystickController):
        print("You can now move your joystick to drive your car.")
        #tell the controller about the tub        
        ctr.set_tub(tub)
        
        if cfg.BUTTON_PRESS_NEW_TUB:
    
            def new_tub_dir():
                V.parts.pop()
                tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
                V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording')
                ctr.set_tub(tub)
    
            ctr.set_button_down_trigger('cross', new_tub_dir)
        ctr.print_controls()

    try:
        print('Start driving')
        #run the vehicle for 20 seconds
        V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
    except KeyboardInterrupt:
        print('Halt driving')
    finally:
        if stby is not None:
            stby.run(0)
            if use_debug:
                print('Stop TB6612 STBY')
        if pgio is not None:
            pgio.stop()
            if use_debug:
                print('Stop pigpio controll')
        print('Stop driving')