def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', 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 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) from tensorflow.python import keras 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 == "SKID_STEER": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, SBC_PiMotor right_motor = SBC_PiMotor(1) left_motor = SBC_PiMotor(2) two_wheel_control = TwoWheelSteeringThrottle() V.add(two_wheel_control, inputs=['throttle', 'angle'], outputs=['left_motor_speed', 'right_motor_speed']) V.add(left_motor, inputs=['left_motor_speed']) V.add(right_motor, inputs=['right_motor_speed']) elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM": from donkeycar.parts.actuator import ServoBlaster, PWMSteering steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin #PWM pulse values should be in the range of 100 to 200 assert (cfg.STEERING_LEFT_PWM <= 200) assert (cfg.STEERING_RIGHT_PWM <= 200) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(motor, inputs=["throttle"]) #add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'float', 'float', 'str'] if cfg.TRAIN_BEHAVIORS: inputs += [ 'behavior/state', 'behavior/label', "behavior/one_hot_state_array" ] types += ['int', 'str', 'vector'] if cfg.HAVE_IMU: inputs += [ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z' ] types += ['float', 'float', 'float', 'float', 'float', 'float'] 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() #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', 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. """ logger.info(f'PID: {os.getpid()}') 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() #Initialize logging before anything else to allow console logging if cfg.HAVE_CONSOLE_LOGGING: logger.setLevel(logging.getLevelName(cfg.LOGGING_LEVEL)) ch = logging.StreamHandler() ch.setFormatter(logging.Formatter(cfg.LOGGING_FORMAT)) logger.addHandler(ch) if cfg.HAVE_MQTT_TELEMETRY: from donkeycar.parts.telemetry import MqttTelemetry tel = MqttTelemetry(cfg) if cfg.HAVE_ODOM: if cfg.ENCODER_TYPE == "GPIO": from donkeycar.parts.encoder import RotaryEncoder enc = RotaryEncoder(mm_per_tick=0.306096, pin = cfg.ODOM_PIN, debug = cfg.ODOM_DEBUG) V.add(enc, inputs=['throttle'], outputs=['enc/speed'], threaded=True) elif cfg.ENCODER_TYPE == "arduino": from donkeycar.parts.encoder import ArduinoEncoder enc = ArduinoEncoder() V.add(enc, outputs=['enc/speed'], threaded=True) else: print("No supported encoder found") logger.info("cfg.CAMERA_TYPE %s"%cfg.CAMERA_TYPE) 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 cfg.CAMERA_TYPE == "D435": from donkeycar.parts.realsense435i import RealSense435i cam = RealSense435i( enable_rgb=cfg.REALSENSE_D435_RGB, enable_depth=cfg.REALSENSE_D435_DEPTH, enable_imu=cfg.REALSENSE_D435_IMU, device_id=cfg.REALSENSE_D435_ID) V.add(cam, inputs=[], outputs=['cam/image_array', 'cam/depth_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True) else: if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv inputs = [] outputs = ['cam/image_array'] threaded = True if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv #rbx cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, record_location=cfg.SIM_RECORD_LOCATION, record_gyroaccel=cfg.SIM_RECORD_GYROACCEL, record_velocity=cfg.SIM_RECORD_VELOCITY, delay=cfg.SIM_ARTIFICIAL_LATENCY) 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, framerate=cfg.CAMERA_FRAMERATE, vflip=cfg.CAMERA_VFLIP, hflip=cfg.CAMERA_HFLIP) 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) elif cfg.CAMERA_TYPE == "IMAGE_LIST": from donkeycar.parts.camera import ImageListCamera cam = ImageListCamera(path_mask=cfg.PATH_MASK) elif cfg.CAMERA_TYPE == "LEOPARD": from donkeycar.parts.leopard_imaging import LICamera cam = LICamera(width=cfg.IMAGE_W, height=cfg.IMAGE_H, fps=cfg.CAMERA_FRAMERATE) else: raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) # add lidar if cfg.USE_LIDAR: from donkeycar.parts.lidar import RPLidar if cfg.LIDAR_TYPE == 'RP': print("adding RP lidar part") lidar = RPLidar(lower_limit = cfg.LIDAR_LOWER_LIMIT, upper_limit = cfg.LIDAR_UPPER_LIMIT) V.add(lidar, inputs=[],outputs=['lidar/dist_array'], threaded=True) if cfg.LIDAR_TYPE == 'YD': print("YD Lidar not yet supported") # Donkey gym part will output position information if it is configured if cfg.DONKEY_GYM: if cfg.SIM_RECORD_LOCATION: outputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte'] if cfg.SIM_RECORD_GYROACCEL: outputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z'] if cfg.SIM_RECORD_VELOCITY: outputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z'] V.add(cam, inputs=inputs, outputs=outputs, threaded=threaded) #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT, mode=cfg.WEB_INIT_MODE) V.add(ctr, inputs=['cam/image_array', 'tub/num_records'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering if cfg.CONTROLLER_TYPE == "MM1": from donkeycar.parts.robohat import RoboHATController ctr = RoboHATController(cfg) elif "custom" == cfg.CONTROLLER_TYPE: # # custom controller created with `donkey createjs` command # from my_joystick import MyJoystickController ctr = MyJoystickController( throttle_dir=cfg.JOYSTICK_THROTTLE_DIR, throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) ctr.set_deadzone(cfg.JOYSTICK_DEADZONE) else: from donkeycar.parts.controller 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 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 IMU imu = IMU(sensor=cfg.IMU_SENSOR, dlp_setting=cfg.IMU_DLP_CONFIG) V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True) # Use the FPV preview, which will show the cropped image output, or the full frame. if cfg.USE_FPV: V.add(WebFpv(), inputs=['cam/image_array'], threaded=True) #Behavioral state if cfg.TRAIN_BEHAVIORS: bh = BehaviorPart(cfg.BEHAVIOR_LIST) V.add(bh, outputs=['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]) try: ctr.set_button_down_trigger('L1', bh.increment_state) except: pass inputs = ['cam/image_array', "behavior/one_hot_state_array"] #IMU elif cfg.USE_LIDAR: inputs = ['cam/image_array', 'lidar/dist_array'] elif cfg.HAVE_ODOM: inputs = ['cam/image_array', 'enc/speed'] elif model_type == "imu": assert(cfg.HAVE_IMU) #Run the pilot if the mode is not user. inputs=['cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'] elif cfg.USE_LIDAR: inputs = ['cam/image_array', 'lidar/dist_array'] else: inputs=['cam/image_array'] 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) from tensorflow.python import keras 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') if cfg.STOP_SIGN_DETECTOR: from donkeycar.parts.object_detector.stop_sign_detector import StopSignDetector V.add(StopSignDetector(cfg.STOP_SIGN_MIN_SCORE, cfg.STOP_SIGN_SHOW_BOUNDING_BOX), inputs=['cam/image_array', 'pilot/throttle'], outputs=['pilot/throttle', 'cam/image_array']) #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 if pilot_angle else 0.0, user_throttle else: return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0 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 or cfg.DRIVE_TRAIN_TYPE == "MOCK": 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'], threaded=True) V.add(throttle, inputs=['throttle'], threaded=True) 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 == "DC_TWO_WHEEL_L298N": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, L298N_HBridge_DC_Motor left_motor = L298N_HBridge_DC_Motor(cfg.HBRIDGE_L298N_PIN_LEFT_FWD, cfg.HBRIDGE_L298N_PIN_LEFT_BWD, cfg.HBRIDGE_L298N_PIN_LEFT_EN) right_motor = L298N_HBridge_DC_Motor(cfg.HBRIDGE_L298N_PIN_RIGHT_FWD, cfg.HBRIDGE_L298N_PIN_RIGHT_BWD, cfg.HBRIDGE_L298N_PIN_RIGHT_EN) 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'], threaded=True) V.add(motor, inputs=["throttle"]) elif cfg.DRIVE_TRAIN_TYPE == "MM1": from donkeycar.parts.robohat import RoboHATDriver V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle']) elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM": from donkeycar.parts.actuator import PWMSteering, PWMThrottle, PiGPIO_PWM steering_controller = PiGPIO_PWM(cfg.STEERING_PWM_PIN, freq=cfg.STEERING_PWM_FREQ, inverted=cfg.STEERING_PWM_INVERTED) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PiGPIO_PWM(cfg.THROTTLE_PWM_PIN, freq=cfg.THROTTLE_PWM_FREQ, inverted=cfg.THROTTLE_PWM_INVERTED) 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'], threaded=True) V.add(throttle, inputs=['throttle'], threaded=True) # OLED setup if cfg.USE_SSD1306_128_32: from donkeycar.parts.oled import OLEDPart auto_record_on_throttle = cfg.USE_JOYSTICK_AS_DEFAULT and cfg.AUTO_RECORD_ON_THROTTLE oled_part = OLEDPart(cfg.SSD1306_128_32_I2C_BUSNUM, auto_record_on_throttle=auto_record_on_throttle) V.add(oled_part, inputs=['recording', 'tub/num_records', 'user/mode'], outputs=[], threaded=True) #add tub to save data if cfg.USE_LIDAR: inputs = ['cam/image_array', 'lidar/dist_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'nparray','float', 'float', 'str'] else: inputs=['cam/image_array','user/angle', 'user/throttle', 'user/mode'] types=['image_array','float', 'float','str'] if cfg.USE_LIDAR: inputs += ['lidar/dist_array'] types += ['nparray'] if cfg.HAVE_ODOM: inputs += ['enc/speed'] types += ['float'] if cfg.TRAIN_BEHAVIORS: inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"] types += ['int', 'str', 'vector'] if cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_DEPTH: inputs += ['cam/depth_array'] types += ['gray16_array'] if cfg.HAVE_IMU or (cfg.CAMERA_TYPE == "D435" and cfg.REALSENSE_D435_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'] # rbx if cfg.DONKEY_GYM: if cfg.SIM_RECORD_LOCATION: inputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte'] types += ['float', 'float', 'float', 'float', 'float'] if cfg.SIM_RECORD_GYROACCEL: inputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z'] types += ['float', 'float', 'float', 'float', 'float', 'float'] if cfg.SIM_RECORD_VELOCITY: inputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z'] types += ['float', 'float', 'float'] if cfg.RECORD_DURING_AI: inputs += ['pilot/angle', 'pilot/throttle'] types += ['float', 'float'] if cfg.HAVE_PERFMON: from donkeycar.parts.perfmon import PerfMonitor mon = PerfMonitor(cfg) perfmon_outputs = ['perf/cpu', 'perf/mem', 'perf/freq'] inputs += perfmon_outputs types += ['float', 'float', 'float'] V.add(mon, inputs=[], outputs=perfmon_outputs, threaded=True) # do we want to store new records into own dir or append to existing tub_path = TubHandler(path=cfg.DATA_PATH).create_tub_path() if \ cfg.AUTO_CREATE_NEW_TUB else cfg.DATA_PATH tub_writer = TubWriter(tub_path, inputs=inputs, types=types, metadata=meta) V.add(tub_writer, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') # Telemetry (we add the same metrics added to the TubHandler if cfg.HAVE_MQTT_TELEMETRY: telem_inputs, _ = tel.add_step_inputs(inputs, types) V.add(tel, inputs=telem_inputs, outputs=["tub/queue_size"], threaded=True) 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: if cfg.DONKEY_GYM: print("You can now go to http://localhost:%d to drive your car." % cfg.WEB_CONTROL_PORT) else: print("You can now go to <your hostname.local>:%d to drive your car." % cfg.WEB_CONTROL_PORT) elif isinstance(ctr, JoystickController): print("You can now move your joystick to drive your car.") ctr.set_tub(tub_writer.tub) ctr.print_controls() #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single'): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' if model_type is None: if cfg.TRAIN_BEHAVIORS: model_type = "behavior" else: model_type = "categorical" if model_type == "streamline": camera_type = "streamline" #Initialize car V = dk.vehicle.Vehicle() if camera_type == "stereo": if cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam camA = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=0) camB = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=1) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam camA = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=0) camB = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, iCam=1) else: raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE)) V.add(camA, outputs=['cam/image_array_a'], threaded=True) V.add(camB, outputs=['cam/image_array_b'], threaded=True) def stereo_pair(image_a, image_b): ''' This will take the two images and combine them into a single image One in red, the other in green, and diff in blue channel. ''' if image_a is not None and image_b is not None: width, height, _ = image_a.shape grey_a = dk.utils.rgb2gray(image_a) grey_b = dk.utils.rgb2gray(image_b) grey_c = grey_a - grey_b stereo_image = np.zeros([width, height, 3], dtype=np.dtype('B')) stereo_image[..., 0] = np.reshape(grey_a, (width, height)) stereo_image[..., 1] = np.reshape(grey_b, (width, height)) stereo_image[..., 2] = np.reshape(grey_c, (width, height)) else: stereo_image = [] return np.array(stereo_image) image_sterero_pair_part = Lambda(stereo_pair) V.add(image_sterero_pair_part, inputs=['cam/image_array_a', 'cam/image_array_b'], outputs=['cam/image_array']) #elif camera_type == "streamline": # print("using grayscale pi cam") # assert(cfg.CAMERA_TYPE == "PICAM") # from donkeycar.parts.camera import PiCamera # cam = PiCamera(image_w=cfg.STREAMLINE_IMAGE_W, image_h=cfg.STREAMLINE_IMAGE_H, image_d=1) else: print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.CAMERA_TYPE == "PICAM": from donkeycar.parts.camera import PiCamera cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "WEBCAM": from donkeycar.parts.camera import Webcam cam = Webcam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) elif cfg.CAMERA_TYPE == "CVCAM": from donkeycar.parts.cv import CvCam cam = CvCam(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) else: raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) V.add(cam, outputs=['cam/image_array'], threaded=True) if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: #modify max_throttle closer to 1.0 to have more power #modify steering_scale lower than 1.0 to have less responsive steering from donkeycar.parts.controller import PS3JoystickController, PS4JoystickController #cont_class = PS3JoystickController cont_class = MyJoystickController if cfg.CONTROLLER_TYPE == "ps4": cont_class = PS4JoystickController ctr = cont_class(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, steering_scale=cfg.JOYSTICK_STEERING_SCALE, auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE) if cfg.USE_NETWORKED_JS: from donkeycar.parts.controller import JoyStickSub netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP) V.add(netwkJs, threaded=True) ctr.js = netwkJs else: #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() V.add(ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) #this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) #See if we should even run the pilot module. #This is only needed because the part run_condition only accepts boolean def pilot_condition(mode): if mode == 'user': return False else: return True pilot_condition_part = Lambda(pilot_condition) V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot']) def led_cond(mode, recording, recording_alert, behavior_state, reloaded_model): #returns a blink rate. 0 for off. -1 for on. positive for rate. if reloaded_model: led.set_rgb(cfg.MODEL_RELOADED_LED_R, cfg.MODEL_RELOADED_LED_G, cfg.MODEL_RELOADED_LED_B) return 0.1 else: led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B) if recording_alert: led.set_rgb(*recording_alert) return cfg.REC_COUNT_ALERT_BLINK_RATE else: led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B) if behavior_state is not None and model_type == 'behavior': r, g, b = cfg.BEHAVIOR_LED_COLORS[behavior_state] led.set_rgb(r, g, b) return -1 #solid on if recording: return -1 #solid on elif mode == 'user': return 1 elif mode == 'local_angle': return 0.5 elif mode == 'local': return 0.1 return 0 if cfg.HAVE_RGB_LED: from donkeycar.parts.led_status import RGB_LED led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B, cfg.LED_INVERT) led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B) led_cond_part = Lambda(led_cond) V.add(led_cond_part, inputs=[ 'user/mode', 'recording', "records/alert", 'behavior/state', 'reloaded/model' ], outputs=['led/blink_rate']) V.add(led, inputs=['led/blink_rate']) def get_record_alert_color(num_records): col = (0, 0, 0) for count, color in cfg.RECORD_ALERT_COLOR_ARR: if num_records >= count: col = color return col def record_tracker(num_records): if num_records is None: return 0 if record_tracker.last_num_rec_print != num_records or record_tracker.force_alert: record_tracker.last_num_rec_print = num_records if num_records % 10 == 0: print("recorded", num_records, "records") if num_records % cfg.REC_COUNT_ALERT == 0 or record_tracker.force_alert: record_tracker.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC record_tracker.force_alert = 0 if record_tracker.dur_alert > 0: record_tracker.dur_alert -= 1 if record_tracker.dur_alert != 0: return get_record_alert_color(num_records) return 0 record_tracker.last_num_rec_print = 0 record_tracker.dur_alert = 0 record_tracker.force_alert = 0 rec_tracker_part = Lambda(record_tracker) V.add(rec_tracker_part, inputs=["tub/num_records"], outputs=['records/alert']) if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController): #then we are not using the circle button. hijack that to force a record count indication def show_record_acount_status(): record_tracker.last_num_rec_print = 0 record_tracker.force_alert = 1 ctr.set_button_down_trigger('circle', show_record_acount_status) #IMU if cfg.HAVE_IMU: imu = Mpu6050() V.add(imu, outputs=[ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z' ], threaded=True) #Behavioral state if cfg.TRAIN_BEHAVIORS: bh = BehaviorPart(cfg.BEHAVIOR_LIST) V.add(bh, outputs=[ 'behavior/state', 'behavior/label', "behavior/one_hot_state_array" ]) try: ctr.set_button_down_trigger('L1', bh.increment_state) except: pass inputs = ['cam/image_array', "behavior/one_hot_state_array"] #IMU elif model_type == "imu": assert (cfg.HAVE_IMU) #Run the pilot if the mode is not user. inputs = [ 'cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z' ] else: inputs = ['cam/image_array'] def load_model(kl, model_path): start = time.time() try: print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start))) except: print('problems loading model', model_path) def load_weights(kl, weights_path): start = time.time() try: print('loading model weights', weights_path) kl.model.load_weights(weights_path) print('finished loading in %s sec.' % (str(time.time() - start))) except: print('problems loading weights', weights_path) def load_model_json(kl, json_fnm): start = time.time() print('loading model json', json_fnm) import keras with open(json_fnm, 'r') as handle: contents = handle.read() kl.model = keras.models.model_from_json(contents) print('finished loading json in %s sec.' % (str(time.time() - start))) if model_path: #When we have a model, first create an appropriate Keras part #if model_type == "streamline": #kl = KerasStreamline() #else: kl = dk.utils.get_model_by_type(model_type, cfg) if '.h5' in model_path: #when we have a .h5 extension #load everything from the model file load_model(kl, model_path) def reload_model(filename): print(filename, "was changed!") load_model(kl, filename) fw_part = FileWatcher(model_path, reload_model, wait_for_write_stop=10.0) V.add(fw_part, outputs=['reloaded/model']) elif '.json' in model_path: #when we have a .json extension #load the model from their and look for a matching #.wts file with just weights load_model_json(kl, model_path) weights_path = model_path.replace('.json', '.weights') load_weights(kl, weights_path) def reload_weights(filename): print(filename, "was changed!") weights_path = filename.replace('.json', '.weights') load_weights(kl, weights_path) fw_part = FileWatcher(model_path, reload_weights, wait_for_write_stop=1.0) V.add(fw_part, outputs=['reloaded/model']) else: #previous default behavior load_model(kl, model_path) V.add(kl, inputs=inputs, outputs=['pilot/angle', 'pilot/throttle'], run_condition='run_pilot') #Choose what inputs should change the car. def drive_mode(mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle drive_mode_part = Lambda(drive_mode) V.add(drive_mode_part, inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) #Drive train setup if cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC": from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) throttle = PWMThrottle(controller=throttle_controller, max_pulse=cfg.THROTTLE_FORWARD_PWM, zero_pulse=cfg.THROTTLE_STOPPED_PWM, min_pulse=cfg.THROTTLE_REVERSE_PWM) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE": from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT, cfg.HBRIDGE_PIN_RIGHT) throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(throttle, inputs=['throttle']) elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL": from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD) right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD) two_wheel_control = TwoWheelSteeringThrottle() V.add(two_wheel_control, inputs=['throttle', 'angle'], outputs=['left_motor_speed', 'right_motor_speed']) V.add(left_motor, inputs=['left_motor_speed']) V.add(right_motor, inputs=['right_motor_speed']) elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM": from donkeycar.parts.actuator import ServoBlaster, PWMSteering steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin #PWM pulse values should be in the range of 100 to 200 assert (cfg.STEERING_LEFT_PWM <= 200) assert (cfg.STEERING_RIGHT_PWM <= 200) steering = PWMSteering(controller=steering_controller, left_pulse=cfg.STEERING_LEFT_PWM, right_pulse=cfg.STEERING_RIGHT_PWM) from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD, cfg.HBRIDGE_PIN_BWD) V.add(steering, inputs=['angle']) V.add(motor, inputs=["throttle"]) #add tub to save data inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'float', 'float', 'str'] if cfg.TRAIN_BEHAVIORS: inputs += [ 'behavior/state', 'behavior/label', "behavior/one_hot_state_array" ] types += ['int', 'str', 'vector'] if cfg.HAVE_IMU: inputs += [ 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z' ] types += ['float', 'float', 'float', 'float', 'float', 'float'] th = TubHandler(path=cfg.DATA_PATH) tub = th.new_tub_writer(inputs=inputs, types=types) V.add(tub, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') if type(ctr) is LocalWebController: print("You can now go to <your pi ip address>:8887 to drive your car.") elif isinstance(ctr, JoystickController): print("You can now move your joystick to drive your car.") #tell the controller about the tub ctr.set_tub(tub) #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', 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 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() # setup camera 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) # setup joystick/web controller if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: # modify max_throttle closer to 1.0 to have more power # modify steering_scale lower than 1.0 to have less responsive steering logging.warning('Using joystick controller') ctr = get_js_controller(cfg) if cfg.USE_NETWORKED_JS: logging.warning('Using networked joystick controller') 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. logging.warning('Using web controller') 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']) V.add(ctr, inputs=[], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # NOTE: user mode is given by controller, which can change DriveMode # this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) # autopilot 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) from tensorflow.python import keras 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 outputs = ['pilot/angle', 'pilot/throttle'] if cfg.TRAIN_LOCALIZER: outputs.append("pilot/loc") V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot') 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']) # add LIDAR stuff # NOTE: using part from donkeyacc lidar_processor = LidarProcessor(non_block=True, debug=False) # use non-block mode for more fps V.add(lidar_processor, outputs=['lidar/distance'], threaded=False) # add cruise controller # NOTE: using part from donkeyacc cruise_controller = CruiseController(kp=1, kd=0.8, default_distance=1.0, throttle_scale=cfg.JOYSTICK_MAX_THROTTLE, max_throttle=0.75, use_timer=True, debug=False) V.add(cruise_controller, inputs=['lidar/distance', 'user/throttle'], outputs=['acc/throttle'], threaded=False) # TODO: YOLO detection yolo_processor = YoloProcessor(non_block=True, warning=True, debug=True) # TODO: disable debug mode V.add(yolo_processor, inputs=['cam/image_array'], outputs=['yolo/detection'], threaded=False) class DriveMode: """ Change different drive modes: manual, adaptive CC, autopilot, etc... """ def run(self, mode, user_angle, user_throttle, pilot_angle, pilot_throttle): # NOTE: pilot_throttle is now handled by ACC if mode == 'user': # using user angle and throttle return user_angle, user_throttle elif mode == 'local_angle': # using user angle and ACC throttle return user_angle, pilot_throttle else: # using autopilot angle and ACC throttle return pilot_angle, pilot_throttle V.add(DriveMode(), inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'acc/throttle'], outputs=['angle', 'throttle']) # setup drive train 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']) # run the vehicle with drive loop frequency and max loops defined in config.py V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type='single', 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. ''' #zd d 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() #zd d #1 ********************************************* camera ******************************************** inputs = [] threaded = True print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE) if cfg.CAMERA_TYPE == "PICAM": from donkeycar.parts.camera import PiCamera cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) #zd d else: raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded) #2 ********************************************* controller ******************************************** #This web controller will create a web server that is capable #of managing steering, throttle, and modes, and more. ctr = LocalWebController() 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) V.add( ctr, inputs=['cam/image_array'], #outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], outputs=[ 'user/angle', 'user/throttle', 'user/mode', 'recording', 'change_model', 'user/arrival' ], #outputs=['user/angle', 'user/throttle', 'user/mode', 'recording', 'change_model'], #zd threaded=True) # ********************************************* communication ******************************************** class communication_class: def __init__(self): self.recv = '' self.arrived = 0 # ser = serial.Serial("/dev/ttyUSB0", 9600) self.serial = serial.Serial('/dev/ttyUSB0', 9600, timeout=3600) # serial = serial.Serial('COM9', 9600, timeout=3600) if self.serial.isOpen(): print("open success") else: print("open failed") data_judge = '0' while data_judge != b'\r\nOK\r\n': send_data = 'AT' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nOK\r\n': send_data = 'AT+CMEE=1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nQuectel\r\n\r\nOK\r\n': send_data = 'AT+CGMI' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nBC95HB-02-STD_900\r\n\r\nOK\r\n': send_data = 'AT+CGMM' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nSECURITY,V100R100C10B657SP3\r\n\r\nPROTOCOL,V100R100C10B657SP3\r\n\r\nAPPLICATION,V100R100C10B657SP3\r\n\r\nSEC_UPDATER,V100R100C10B657SP3\r\n\r\nAPP_UPDATER,V100R100C10B657SP3\r\n\r\nRADIO,BC95HB-02-STD_900\r\n\r\nOK\r\n': send_data = 'AT+CGMR' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+NBAND:8\r\n\r\nOK\r\n': send_data = 'AT+NBAND?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+NCONFIG:AUTOCONNECT,TRUE\r\n+NCONFIG:CR_0354_0338_SCRAMBLING,TRUE\r\n+NCONFIG:CR_0859_SI_AVOID,TRUE\r\n+NCONFIG:COMBINE_ATTACH,FALSE\r\n+NCONFIG:CELL_RESELECTION,FALSE\r\n+NCONFIG:ENABLE_BIP,FALSE\r\n\r\nOK\r\n': send_data = 'AT+NCONFIG?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+CGSN:869405035846048\r\n\r\nOK\r\n': send_data = 'AT+CGSN=1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nOK\r\n': send_data = 'AT+CFUN=1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n460043193006443\r\n\r\nOK\r\n': send_data = 'AT+CIMI' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nOK\r\n': send_data = 'AT+CGATT=0' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+CGATT:0\r\n\r\nOK\r\n': send_data = 'AT+CGATT?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\nOK\r\n': send_data = 'AT+CGATT=1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+CGATT:1\r\n\r\nOK\r\n': send_data = 'AT+CGATT?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = b'\r\n+CSQ:99,99\r\n\r\nOK\r\n' while data_judge == b'\r\n+CSQ:99,99\r\n\r\nOK\r\n': send_data = 'AT+CSQ' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+COPS:0,2,"46000"\r\n\r\nOK\r\n': send_data = 'AT+COPS?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) data_judge = '0' while data_judge != b'\r\n+CEREG:0,1\r\n\r\nOK\r\n': send_data = 'AT+CEREG?' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) print('nb-iot initial ok!') def update(self): while True: try: data_judge = '0' # sleep(1) while data_judge != b'\r\n0\r\n\r\nOK\r\n' and data_judge != b'\r\n+CME ERROR: 4\r\n': send_data = 'AT+NSOCR=DGRAM,17,25535,1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read( self.serial.inWaiting()) print(data_judge) # line = line.splitlines(True) legal = 'OK' data = {'order': 'ask'} data = json.dumps(data) l = len(data) judge = '0' while judge != legal: send_data = "AT+NSOST=0,139.199.105.136,25535," + str( len(data)) + "," + (binascii.b2a_hex( str(data).encode('utf-8'))).decode() print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) # line = ser.read(ser.inWaiting()) # line = line.decode() sleep(2) # line = line.splitlines(True) data = data + self.serial.read(self.serial.inWaiting()) data = str(data).split("\\r\\n") print(data) judge = data[3] if len(data) > 5: print("进入判断") data = data[5] l = data[10:len(data)] print(l) data_judge = '0' while data_judge != b'\r\n\r\nOK\r\n': print("发送请求") send_data = 'AT+NSORF=0,' + l + "\r\n" print(send_data) # send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(1) data = data + self.serial.read( self.serial.inWaiting()) # print(data.decode('utf-8')) data_judge = data[len(data) - 8:len(data)] data = str(data).split(",") # print((bytearray.fromhex(data[4])).decode('utf-8')) data = (bytearray.fromhex(data[4])).decode('utf-8') data = json.loads(data) self.recv = data print("receive:") print(data) #!!! change car state when arrive # if has_arrived=='arrive': # print("!!!!!!car_state change to c(waiting to get)") # judge = '0' # data = {'order': 'car_state'} # data = json.dumps(data) # l = len(data) # legal = "\r\n0," + str(len(data)) + "\r\n\r\nOK\r\n" # # while judge != legal.encode('utf-8'): # send_data = "AT+NSOST=0,139.199.105.136,25535," + str(len(data)) + "," + ( # binascii.b2a_hex(str(data).encode('utf-8'))).decode() # print(send_data) # send_data = send_data + '\r\n' # self.serial.write(send_data.encode()) # data = self.serial.read(1) # sleep(2) # data = data + self.serial.read(self.serial.inWaiting()) # #print(data) # judge = data[0:14] data_judge = '0' # sleep(1) while data_judge != b'\r\nOK\r\n': # line = ser.read(ser.inWaiting()) # line = line.decode() send_data = 'AT+NSOCL=0' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read( self.serial.inWaiting()) print(data_judge) except: while data_judge != b'\r\nOK\r\n': send_data = 'AT+NSOCL=0' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read( self.serial.inWaiting()) print(data_judge) def run_threaded(self, has_arrived): # !!! change car state when arrive if has_arrived == 'arrive': if self.arrived == 0: self.arrived += 1 print("!!!!!!car_state change to c(waiting to get)") try: data_judge = '0' # sleep(1) while data_judge != b'\r\n0\r\n\r\nOK\r\n' and data_judge != b'\r\n+CME ERROR: 4\r\n': send_data = 'AT+NSOCR=DGRAM,17,25535,1' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read( self.serial.inWaiting()) print(data_judge) judge = '0' data = {'order': 'car_state'} data = json.dumps(data) l = len(data) legal = "\r\n0," + str(len(data)) + "\r\n\r\nOK\r\n" while judge != legal.encode('utf-8'): send_data = "AT+NSOST=0,139.199.105.136,25535," + str( len(data)) + "," + (binascii.b2a_hex( str(data).encode('utf-8'))).decode() print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) #sleep(2) sleep(1) data = data + self.serial.read( self.serial.inWaiting()) # print(data) judge = data[0:14] except: print("send arrive error!") else: self.arrived = 0 tmp = self.recv if tmp != '': return tmp['box_state'], tmp['car_state'], tmp['pos'] else: #!!!! return ['open', 'open', 'open'], 'a', 'a' def shuwtdonw(self): #关闭socket GPIO.cleanup() data_judge = '0' # sleep(1) print("clean udp socket") while data_judge != b'\r\nOK\r\n': send_data = 'AT+NSOCL=0' print(send_data) send_data = send_data + '\r\n' self.serial.write(send_data.encode()) data = self.serial.read(1) sleep(0.5) data_judge = data + self.serial.read(self.serial.inWaiting()) print(data_judge) communication = communication_class() #!!! V.add(communication, inputs=['arrive_signal'], outputs=['lock_state', 'car_state', 'pos'], threaded=True) # ********************************************* lock_ctr ******************************************** class lock_ctr_class: def __init__(self): GPIO.setmode(GPIO.BOARD) GPIO.setup(36, GPIO.OUT) # 1 GPIO.setmode(GPIO.BOARD) GPIO.setup(38, GPIO.OUT) # 2 GPIO.setmode(GPIO.BOARD) GPIO.setup(40, GPIO.OUT) # 3 def run(self, state): #1 if state[0] == 'close': GPIO.output(36, True) else: GPIO.output(36, False) #2 if state[1] == 'close': GPIO.output(38, True) else: GPIO.output(38, False) #3 if state[2] == 'close': GPIO.output(40, True) else: GPIO.output(40, False) # for x in range(3): # 0 1 2 # if state[x]=='open': # GPIO.output(11+x, False) # else: # close # GPIO.output(11+x, True) def shutdown(self): GPIO.cleanup() lock_ctr = lock_ctr_class() V.add(lock_ctr, inputs=['lock_state'], outputs=[]) # ********************************************* throttle filter ******************************************** #this throttle filter will allow one tap back for esc reverse #th_filter = ThrottleFilter() #V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) # #3 ********************************************* PilotCondition ******************************************** #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 PilotCondition: def run(self, car_state): if car_state == 'a' or car_state == '': return False, 'user' else: return True, 'local_pilot' V.add(PilotCondition(), inputs=['car_state'], outputs=['run_pilot', 'user/mode']) #zd d #4 ********************************************* RecordTracker ******************************************** 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']) #zd d #5 ********************************************* model ******************************************** # important def load_model(kl, model_path): start = time.time() try: print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start))) except Exception as e: print(e) print('ERR>> problems loading model', model_path) def load_weights(kl, weights_path): start = time.time() try: print('loading model weights', weights_path) kl.model.load_weights(weights_path) print('finished loading in %s sec.' % (str(time.time() - start))) except Exception as e: print(e) print('ERR>> problems loading weights', weights_path) def load_model_json(kl, json_fnm): start = time.time() print('loading model json', json_fnm) from tensorflow.python import keras 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 # most time kl = dk.utils.get_model_by_type(model_type, cfg) # model_type 默认是linear model_reload_cb = None if '.h5' in model_path: #when we have a .h5 extension #load everything from the model file # most time load_model(kl, model_path) def reload_model(filename): print("reloading start") #zd V.sudden_control(0) #print('***************', V.mem.items()) #V.mem.put(['angle', 'throttle', 'pilot/angle', 'pilot/throttle', 'user/angle', 'user/throttle'], [0, 0, 0, 0, 0 ,0]) #print('***************', V.mem.items()) load_model(kl, filename) print( '***************\r\nreloading is done\r\n**************\r\n' ) model_reload_cb = reload_model else: print("ERR>> Unknown extension type on model file!!") return #these parts will reload the model file, but only when ai is running so we don't interrupt user driving #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") #zd #V.add(TriggeredCallback("./models/mypilot_2.h5", model_reload_cb), inputs=["modelfile/reload","change_model"], run_condition="ai_running") #V.add(TriggeredCallback("./models/mypilot_2.h5", model_reload_cb), inputs=["modelfile/reload", "change_model"]) inputs = ['cam/image_array'] outputs = ['pilot/angle', 'pilot/throttle', 'pilot/arrival'] V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot') # ********************************************** turn_delay_trigger ************************************************ class turn_delay_class: def __init__(self, pause_time): self.pause = False self.trigger = 0 self.pause_time = pause_time self.now_direc = 0 #self.direc_list = ['S'] #p is pause self.direc_list = ['L'] #self.on = False def update(self): while 1: if self.pause == True: sleep(2) self.pause = False if self.trigger == 1: #self.on = False #time.sleep(3) #self.on = True time.sleep(self.pause_time) self.trigger = 0 self.now_direc += 1 def run_threaded(self, arrival_signal, mode, has_arrived, pos, car_state): # >0 = arrive <0 = not arrive if self.pause == True: return True, 'P', '' if self.trigger == 1: return True, self.direc_list[self.now_direc], '' # new deal if mode == 'user': #if car_state=='a': #待放 self.now_direc = 0 self.trigger = 0 self.pause = False if pos == 'a': self.direc_list = ['L'] elif pos == 'b': self.direc_list = ['S'] print('~~~~~~~~~~~~~~~~~~~direction restart') return False, '', '' if has_arrived == 'arrive': return False, '', 'arrive' if arrival_signal > 0: # judge arrive # print("now is {}".format(self.now_direc)) # if self.now_direc >= len(self.direc_list): # print("arrive--------------------------") # return False, '', 'arrive' if self.now_direc >= len(self.direc_list): print("arrive--------------------------") return False, '', 'arrive' self.trigger = 1 self.pause = True print("**************************start turn!!!, now is {}". format(self.direc_list[self.now_direc])) return True, self.direc_list[self.now_direc], '' else: return False, '', '' def shutdown(self): return turn_delay = turn_delay_class(1.65) #turn_delay = turn_delay_class(2) #V.add(turn_delay, inputs=['pilot/arrival', 'user/mode'], outputs=['turn/mode', 'turn_direction'], run_condition='run_pilot', threaded=True) V.add(turn_delay, inputs=[ 'pilot/arrival', 'user/mode', 'arrive_signal', 'pos', 'car_state' ], outputs=['turn/mode', 'turn_direction', 'arrive_signal'], threaded=True) # ********************************************** turn_module ************************************************ class turn_module_class: def __init__(self): pass def run(self, direc): if direc == 'P': #pause return 0, 0 elif direc == 'L': #left return -1, 1 elif direc == 'R': #right return 1, -1 elif direc == 'S': #straight return 0.4, 0.4 else: #pause return 0, 0 def shutdown(self): return turn_module = turn_module_class() V.add(turn_module, inputs=['turn_direction'], outputs=['turn/angle', 'turn/throttle'], run_condition='run_pilot') # pause module input=[] out=[pause/mode] #6 ********************************************* DriveMode ******************************************** #Choose what inputs should change the car. class DriveMode: def run(self, mode, turn_mode, user_angle, user_throttle, pilot_angle, pilot_throttle, turn_angle, turn_throttle, arrive_signal): if turn_mode == True: return turn_angle, turn_throttle if mode == 'user': return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: # run_pilot if arrive_signal == 'arrive': return 0, 0 return 0.3, 0.4 #return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT V.add(DriveMode(), inputs=[ 'user/mode', 'turn/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle', 'turn/angle', 'turn/throttle', 'arrive_signal' ], outputs=['angle', 'throttle']) #zd d #to give the car a boost when starting ai mode in a race. #7 ********************************************* AiRunCondition ******************************************** 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 zd d #8 ********************************************* Motor ******************************************** #Drive train setup # move power if cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL": print('DC start!') from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM LEFT_PID_DIR1 = 11 LEFT_PID_DIR2 = 13 LEFT_PWM = 15 RIGHT_PID_DIR1 = 12 RIGHT_PID_DIR2 = 16 RIGHT_PWM = 18 # 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) left_motor = Mini_HBridge_DC_Motor_PWM(LEFT_PID_DIR1, LEFT_PID_DIR2, LEFT_PWM) right_motor = Mini_HBridge_DC_Motor_PWM(RIGHT_PID_DIR1, RIGHT_PID_DIR2, RIGHT_PWM) 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']) #9 ********************************************* tub ******************************************** #add tub to save data ''' inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types=['image_array', 'float', 'float', 'str'] ''' inputs = [ 'cam/image_array', 'user/angle', 'user/throttle', 'user/arrival', 'user/mode' ] types = ['image_array', 'float', 'float', 'float', 'str'] 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') # zd d if type(ctr) is LocalWebController: print("You can now go to <your pi ip address>:8887 to drive your car.") #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, model_path_1=None, use_joystick=False, model_type=None, camera_type='single', 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: 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) 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 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', 'switch_mod', 'taga', 'tagb', 'tagc', 'tagd'], #2020.10.24 by zmx 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']) class Detect(): def __init__(self, cfg): self.cfg = cfg def run(self,img): #print('Adding part Detect') #test=cv2.imread('/home/pi/mycar/picture/test1.jpg',1) hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) lower_green = np.array([35,100,46]) upper_green = np.array([77,255,255]) mask = cv2.inRange(hsv, lower_green, upper_green) masked = cv2.bitwise_and(img,img, mask= mask) template=cv2.imread('/home/pi/mycar/picture/flag1.jpg',1) temp_b = cv2.cvtColor(template, cv2.COLOR_BGR2GRAY) #template=cv2.cvtColor(template,COLOR_BGR2GRAY) res=cv2.matchTemplate(mask,temp_b,cv2.TM_CCOEFF_NORMED) min_val,max_val,min_loc,max_loc=cv2.minMaxLoc(res) #print(max_val) if max_val > 0.60: print("match",max_val) return 1 else: print("no",max_val) return 0 V.add(Detect(cfg),inputs=['cam/image_array'],outputs=['mypilot_mod'],run_condition='run_pilot') #在自动驾驶模式下加入detect部分 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) #Behavioral state if cfg.TRAIN_BEHAVIORS: bh = BehaviorPart(cfg.BEHAVIOR_LIST) V.add(bh, outputs=['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]) try: ctr.set_button_down_trigger('L1', bh.increment_state) except: pass inputs = ['cam/image_array', "behavior/one_hot_state_array"] #IMU elif model_type == "imu": assert(cfg.HAVE_IMU) #Run the pilot if the mode is not user. inputs=['cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'] else: inputs=['cam/image_array'] def load_model(kl, model_path): start = time.time() try: print('loading model', model_path) kl.load(model_path) print('finished loading in %s sec.' % (str(time.time() - start)) ) except Exception as e: print(e) print('ERR>> problems loading model', model_path) def load_weights(kl, weights_path): start = time.time() try: print('loading model weights', weights_path) kl.model.load_weights(weights_path) print('finished loading in %s sec.' % (str(time.time() - start)) ) except Exception as e: print(e) print('ERR>> problems loading weights', weights_path) def load_model_json(kl, json_fnm): start = time.time() print('loading model json', json_fnm) from tensorflow.python import keras 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: #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 #by zmx 2020.11.30 试图添加第二个模型 if model_path_1: #When we have a model, first create an appropriate Keras part kl_1 = dk.utils.get_model_by_type(model_type, cfg) model_reload_cb_1 = None if '.h5' in model_path_1: #when we have a .h5 extension #load everything from the model file load_model(kl_1, model_path_1) def reload_model(filename): load_model(kl_1, filename) model_reload_cb_1 = reload_model elif '.json' in model_path_1: #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_1, model_path_1) weights_path_1 = model_path.replace('.json', '.weights') load_weights(kl_1, weights_path_1) def reload_weights(filename): weights_path_1 = filename.replace('.json', '.weights') load_weights(kl_1, weights_path_1) model_reload_cb_1 = 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_0=['pilot/angle_0', 'pilot/throttle_0'] #by zmx 2020.11.30 outputs_1=['pilot/angle_1', 'pilot/throttle_1'] #by zmx 2020.11.30 if cfg.TRAIN_LOCALIZER: outputs_0.append("pilot/loc") outputs_1.append("pilot/loc") V.add(kl, inputs=inputs, outputs=outputs_0, run_condition='run_pilot') V.add(kl_1, inputs=inputs, #by zmx 2020.11.30 把模型的的输出更名为 _1 和 _0 在选择器(MyPilot)里选择 outputs=outputs_1, run_condition='run_pilot') class MySwitch: def __init__(self): self.pilot_state = 0 #by zmx 2020.10.24 到时候会改成int型 self.mode = 0 self.mode_switch = False def run(self,mod_switch): if (mod_switch): #print("detected") self.pilot_state = 1 else: #print("not pushed") self.pilot_state = 0 print("pushed : ",mod_switch,"\tmode : ",self.pilot_state) return self.pilot_state def update(self): while(True): self.pilot_state = self.run(self.mod_switch) #V.add(MySwitch(), inputs=['switch_mod'], outputs=['web_switch']) class MyButton: def __init__(self): self.button_in = [False,False,False,False] self.state_out = [True,False,False,False] #by zmx 2020.12.7添加了4个状态量bool,其中默认0是默认状态 self.state_out1 = 1 #by zmx 2020.12.13 默认是n,接到后变化 def run(self,a,b,c,d): if a: self.state_out1 = 1 self.state_out = [True,False,False,False] print("A triggered") elif b: self.state_out1 = 2 self.state_out = [False,True,False,False] print("B triggered") elif c: self.state_out1 = 3 self.state_out = [False,False,True,False] print("X triggered") elif d: self.state_out1 = 4 self.state_out = [False,False,False,True] print("Y triggered") #print(self.state_out1) return self.state_out1 def update(self): while(True): a = self.button_in[0] b = self.button_in[1] c = self.button_in[2] d = self.button_in[3] self.state_out1 = self.run(a,b,c,d) def run_threaded(self,taga,tagb,tagc,tagd): self.button_in = [taga,tagb,tagc,tagd] #print(self.state_out1,"---") return self.state_out1 V.add(MyButton(),inputs=['taga','tagb','tagc','tagd'],outputs=['my_state'],run_condition='switch_mod') class MyPilot: def __init__(self): self.angle = 0 self.throttle = 0 self.angle_0 = 0 self.angle_1 = 0 self.throttle_0 = 0 self.throttle_1 = 0 self.mode = 0 self.state = [False,False,False,False] self.state1 = 0 def run_threaded(self,my_state,a0,t0,a1,t1): self.angle_0 = a0 self.angle_1 = a1 self.throttle_0 = t0 self.throttle_1 = t1 #self.mode = mode self.state1 = my_state return self.angle, self.throttle def update(self): while True: self.angle, self.throttle = self.run(self.state1,self.angle_0,self.angle_1,self.throttle_0,self.throttle_1) #print(self.angle) def run(self,state1,angle_0,throttle_0,angle_1,throttle_1): print(state1,"---",angle_0,throttle_0,"------",angle_1,throttle_1) if state1==1: #print("mode:",pilot_mode," angle:",angle_0," throttle:",throttle_0) return angle_0,throttle_0 elif state1==2: #print("mode:",pilot_mode," angle:",angle_1," throttle:",throttle_1) return angle_1,throttle_1 else: return 0,0 V.add(MyPilot(), inputs=['my_state','pilot/angle_0', 'pilot/throttle_0','pilot/angle_1', 'pilot/throttle_1'], outputs=['mypilot/angle','mypilot/throttle'],run_condition='switch_mod') #Choose what inputs should change the car. class DriveMode: def run(self, mode, user_angle, user_throttle, mypilot_angle, mypilot_throttle): #print("drivemode is running") #print(taga,"!!!!!!!!",pilot_mode) if mode == 'user': #print(taga,"!!!!!!!!",pilot_mode) return user_angle, user_throttle elif mode == 'local_angle': #print("local_angle") return mypilot_angle, user_throttle else: #print("mode:",pilot_mode," angle:",mypilot_angle," throttle:",mypilot_throttle) return(0.1,0.1) #return mypilot_angle, mypilot_throttle * cfg.AI_THROTTLE_MULT #return mypilot_angle, mypilot_throttle * cfg.AI_THROTTLE_MULT #by zmx 2020.11.30 临时的修改,把my的输出强加上去看看 V.add(DriveMode(), inputs=['user/mode', 'user/angle', 'user/throttle', 'pilot/angle_0', 'mypilot/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"]) #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_GAMEPAD: inputs += ['my_state'] types += ['int'] 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 pi ip address>:8887 to drive your car.") elif isinstance(ctr, JoystickController): print("You can now move your joystick to drive your car.") #tell the controller about the tub ctr.set_tub(tub) if cfg.BUTTON_PRESS_NEW_TUB: def new_tub_dir(): V.parts.pop() tub = th.new_tub_writer(inputs=inputs, types=types, 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() #run the vehicle for 20 seconds V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
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(): '''