def build_controller(self): if self.use_joystick or self.cfg.USE_JOYSTICK_AS_DEFAULT: from donkeycar.parts.controller import get_js_controller ctr = get_js_controller(self.cfg) if self.cfg.USE_NETWORKED_JS: from donkeycar.parts.controller import JoyStickSub netwkJs = JoyStickSub(self.cfg.NETWORK_JS_SERVER_IP) self.vehicle.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() self.vehicle.add( ctr, inputs=['cam/image_array' ], # TODO: figure out if this is necessary outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) self.controller = ctr #this throttle filter will allow one tap back for esc reverse self.vehicle.add(ThrottleFilter(), inputs=['user/throttle'], outputs=['user/throttle'])
def drive(cfg): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' #Initialize car V = dk.vehicle.Vehicle() ctr = get_js_controller(cfg) V.add(ctr, outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) #Drive train setup arduino_controller = ArduinoFirmata(servo_pin=cfg.STEERING_ARDUINO_PIN, esc_pin=cfg.THROTTLE_ARDUINO_PIN) steering = ArdPWMSteering(controller=arduino_controller, left_pulse=cfg.STEERING_ARDUINO_LEFT_PWM, right_pulse=cfg.STEERING_ARDUINO_RIGHT_PWM) throttle = ArdPWMThrottle(controller=arduino_controller, max_pulse=cfg.THROTTLE_ARDUINO_FORWARD_PWM, zero_pulse=cfg.THROTTLE_ARDUINO_STOPPED_PWM, min_pulse=cfg.THROTTLE_ARDUINO_REVERSE_PWM) V.add(steering, inputs=['user/angle']) V.add(throttle, inputs=['user/throttle']) #run the vehicle V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def __init__(self, cfg, model_path=None, model_type='DDPG', meta=[], training=True, batch_size=64, transfer=None): # init car self.V = dk.vehicle.Vehicle() self.cfg = cfg self.model_path = model_path self.model_type = model_type self.weight_path = transfer self.meta = meta self.training = training self.model_dict = { 'DDPG': DDPG, 'PPO': PPO, 'SAC': SAC, 'TD3': TD3, } self.BATCH_SIZE = batch_size self.th = TubHandler(path=self.cfg.DATA_PATH) self.ctr = get_js_controller(self.cfg) self.eps = 0 self._setup_vihecle()
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): ''' Construct a working robotic vehicle from many parts. Each part runs as a job in the Vehicle loop, calling either it's run or run_threaded method depending on the constructor flag `threaded`. All parts are updated one after another at the framerate given in cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner. Parts may have named outputs and inputs. The framework handles passing named outputs to parts requesting the same named input. ''' #Initialize car V = dk.vehicle.Vehicle() if cfg.HAVE_SOMBRERO: from donkeycar.utils import Sombrero s = Sombrero() ctr = get_js_controller(cfg) V.add(ctr, inputs=['null'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) if cfg.HAVE_ODOM: pi = pigpio.pi() enc = PiPGIOEncoder(cfg.ODOM_PIN, pi) V.add(enc, outputs=['enc/ticks']) odom = OdomDist(mm_per_tick=cfg.MM_PER_TICK, debug=cfg.ODOM_DEBUG) V.add(odom, inputs=['enc/ticks', 'user/throttle'], outputs=['enc/dist_m', 'enc/vel_m_s', 'enc/delta_vel_m_s']) if not os.path.exists(cfg.WHEEL_ODOM_CALIB): print( "You must supply a json file when using odom with T265. There is a sample file in templates." ) print( "cp donkeycar/donkeycar/templates/calibration_odometry.json .") exit(1) else: # we give the T265 no calib to indicated we don't have odom cfg.WHEEL_ODOM_CALIB = None #This dummy part to satisfy input needs of RS_T265 part. class NoOdom(): def run(self): return 0.0 V.add(NoOdom(), outputs=['enc/vel_m_s']) # This requires use of the Intel Realsense T265 rs = RS_T265(image_output=False, calib_filename=cfg.WHEEL_ODOM_CALIB) V.add(rs, inputs=['enc/vel_m_s'], outputs=['rs/pos', 'rs/vel', 'rs/acc', 'rs/camera/left/img_array'], threaded=True) # Pull out the realsense T265 position stream, output 2d coordinates we can use to map. class PosStream: def run(self, pos): #y is up, x is right, z is backwards/forwards return pos.x, pos.z V.add(PosStream(), inputs=['rs/pos'], outputs=['pos/x', 'pos/y']) # This part will reset the car back to the origin. You must put the car in the known origin # and push the cfg.RESET_ORIGIN_BTN on your controller. This will allow you to induce an offset # in the mapping. origin_reset = OriginOffset() V.add(origin_reset, inputs=['pos/x', 'pos/y'], outputs=['pos/x', 'pos/y']) class UserCondition: def run(self, mode): if mode == 'user': return True else: return False V.add(UserCondition(), inputs=['user/mode'], outputs=['run_user']) #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']) # This is the path object. It will record a path when distance changes and it travels # at least cfg.PATH_MIN_DIST meters. Except when we are in follow mode, see below... path = Path(min_dist=cfg.PATH_MIN_DIST) V.add(path, inputs=['pos/x', 'pos/y'], outputs=['path'], run_condition='run_user') # When a path is loaded, we will be in follow mode. We will not record. path_loaded = False if os.path.exists(cfg.PATH_FILENAME): path.load(cfg.PATH_FILENAME) path_loaded = True def save_path(): path.save(cfg.PATH_FILENAME) print("saved path:", cfg.PATH_FILENAME) def erase_path(): global mode, path_loaded if os.path.exists(cfg.PATH_FILENAME): os.remove(cfg.PATH_FILENAME) mode = 'user' path_loaded = False print("erased path", cfg.PATH_FILENAME) else: print("no path found to erase") def reset_origin(): print("Resetting origin") origin_reset.init_to_last # Here's a trigger to save the path. Complete one circuit of your course, when you # have exactly looped, or just shy of the loop, then save the path and shutdown # this process. Restart and the path will be loaded. ctr.set_button_down_trigger(cfg.SAVE_PATH_BTN, save_path) # Here's a trigger to erase a previously saved path. ctr.set_button_down_trigger(cfg.ERASE_PATH_BTN, erase_path) # Here's a trigger to reset the origin. ctr.set_button_down_trigger(cfg.RESET_ORIGIN_BTN, reset_origin) # Here's an image we can map to. img = PImage(clear_each_frame=True) V.add(img, outputs=['map/image']) # This PathPlot will draw path on the image plot = PathPlot(scale=cfg.PATH_SCALE, offset=cfg.PATH_OFFSET) V.add(plot, inputs=['map/image', 'path'], outputs=['map/image']) # This will use path and current position to output cross track error cte = CTE() V.add(cte, inputs=['path', 'pos/x', 'pos/y'], outputs=['cte/error'], run_condition='run_pilot') # This will use the cross track error and PID constants to try to steer back towards the path. pid = PIDController(p=cfg.PID_P, i=cfg.PID_I, d=cfg.PID_D) pilot = PID_Pilot(pid, cfg.PID_THROTTLE) V.add(pilot, inputs=['cte/error'], outputs=['pilot/angle', 'pilot/throttle'], run_condition="run_pilot") def dec_pid_d(): pid.Kd -= 0.5 logging.info("pid: d- %f" % pid.Kd) def inc_pid_d(): pid.Kd += 0.5 logging.info("pid: d+ %f" % pid.Kd) # Buttons to tune PID constants ctr.set_button_down_trigger("L2", dec_pid_d) ctr.set_button_down_trigger("R2", inc_pid_d) # Plot a circle on the map where the car is located carcolor = 'green' loc_plot = PlotCircle(scale=cfg.PATH_SCALE, offset=cfg.PATH_OFFSET, color=carcolor) V.add(loc_plot, inputs=['map/image', 'pos/x', 'pos/y'], outputs=['map/image']) #This web controller will create a web server. We aren't using any controls, just for visualization. web_ctr = WebFpv() V.add(web_ctr, inputs=['map/image'], threaded=True) #Choose what inputs should change the car. class DriveMode: def run(self, mode, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'user': #print(user_angle, user_throttle) return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle, user_throttle else: return pilot_angle, pilot_throttle V.add(DriveMode(), inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) if not cfg.DONKEY_GYM: 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']) # Print Joystick controls ctr.print_controls() if path_loaded: print( "###############################################################################" ) print("Loaded path:", cfg.PATH_FILENAME) print("Make sure your car is sitting at the origin of the path.") print("View web page and refresh. You should see your path.") print("Hit 'select' twice to change to ai drive mode.") print( "You can press the X button (e-stop) to stop the car at any time.") print("Delete file", cfg.PATH_FILENAME, "and re-start") print("to record a new path.") print( "###############################################################################" ) carcolor = "blue" loc_plot = PlotCircle(scale=cfg.PATH_SCALE, offset=cfg.PATH_OFFSET, color=carcolor) V.add(loc_plot, inputs=['map/image', 'pos/x', 'pos/y'], outputs=['map/image']) else: print( "###############################################################################" ) print("You are now in record mode. Open the web page to your car") print("and as you drive you should see a path.") print("Complete one circuit of your course.") print("When you have exactly looped, or just shy of the ") print("loop, then save the path (press %s)." % cfg.SAVE_PATH_BTN) print("You can also erase a path with the Triangle button.") print("When you're done, close this process with Ctrl+C.") print("Place car exactly at the start. ") print("Then restart the car with 'python manage drive'.") print("It will reload the path and you will be ready to ") print("follow the path using 'select' to change to ai drive mode.") print("You can also press the Square button to reset the origin") print( "###############################################################################" ) V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, model_type=None): """ Construct a minimal robotic vehicle from many parts. Here, we use a single camera, web or joystick controller, autopilot and tubwriter. Each part runs as a job in the Vehicle loop, calling either its 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. """ car = dk.vehicle.Vehicle() # add camera inputs = [] if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, delay=cfg.SIM_ARTIFICIAL_LATENCY) inputs = ['angle', 'throttle', 'brake'] 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) else: raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE)) car.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=True) # add lidar if cfg.USE_LIDAR: if cfg.LIDAR_TYPE == 'RP': print("adding RP lidar part") lidar = RPLidar(lower_limit=cfg.LIDAR_LOWER_LIMIT, upper_limit=cfg.LIDAR_UPPER_LIMIT) car.add(lidar, inputs=[], outputs=['lidar/dist_array'], threaded=True) if cfg.LIDAR_TYPE == 'YD': print("YD Lidar not yet supported") # add controller if cfg.USE_RC: rc_steering = RCReceiver(cfg.STEERING_RC_GPIO, invert=True) rc_throttle = RCReceiver(cfg.THROTTLE_RC_GPIO) rc_wiper = RCReceiver(cfg.DATA_WIPER_RC_GPIO, jitter=0.05, no_action=0) car.add(rc_steering, outputs=['user/angle', 'user/angle_on']) car.add(rc_throttle, outputs=['user/throttle', 'user/throttle_on']) car.add(rc_wiper, outputs=['user/wiper', 'user/wiper_on']) car.add(RCHelper(), outputs=['user/mode']) ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT, mode=cfg.WEB_INIT_MODE) car.add(ctr, inputs=['cam/image_array'], outputs=['recording'], threaded=True) else: if cfg.USE_JOYSTICK_AS_DEFAULT: 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) car.add(netwkJs, threaded=True) ctr.js = netwkJs else: ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT, mode=cfg.WEB_INIT_MODE) car.add( ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # pilot condition to determine if user or ai are driving car.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot']) # adding the auto-pilot if model_type is None: model_type = cfg.DEFAULT_MODEL_TYPE if model_path: kl = dk.utils.get_model_by_type(model_type, cfg) kl.load(model_path=model_path) if cfg.USE_LIDAR: inputs = ['cam/image_array', 'lidar/dist_array'] else: inputs = ['cam/image_array'] outputs = ['pilot/angle', 'pilot/throttle'] car.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot') # Choose what inputs should change the car. car.add(DriveMode(cfg=cfg), inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=['angle', 'throttle']) # Drive train setup if cfg.DONKEY_GYM or cfg.DRIVE_TRAIN_TYPE == "MOCK": pass else: 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) car.add(steering, inputs=['angle']) car.add(throttle, inputs=['throttle']) # 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'] # 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(base_path=tub_path, inputs=inputs, types=types) car.add(tub_writer, inputs=inputs, outputs=["tub/num_records"], run_condition='recording') if not model_path and cfg.USE_RC: tub_wiper = TubWiper(tub_writer.tub, num_records=cfg.DRIVE_LOOP_HZ) car.add(tub_wiper, inputs=['user/wiper_on']) # start the car car.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def train_drive_reinforcement(cfg, args, script_mode): ''' 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. ''' model_path = args['--model'] use_joystick = args['--js'] meta = args['--meta'] tub_in = args['--tub'] model_type = args['--type'] camera_type = args['--camera'] vae_path = args['--vae'] auto_mode = 0 if args['--auto']: auto_mode = 1 env_type = 'simulator' if args['--env']: env_type = args['--env'] print('VAE_PATH: %s' % vae_path) global r global ctr global cam 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() print("cfg.CAMERA_TYPE", 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 False and cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv inputs = [] threaded = True if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, 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) 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 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) else: #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) #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) 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') # Use the FPV preview, which will show the cropped image output, or the full frame. if False and 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 = [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))) #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 return user_angle, user_throttle 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 == "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 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.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'] 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: 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.") #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() if script_mode == 'train': _thread.start_new_thread( observe_and_learn, (cfg, model_path, vae_path, auto_mode, env_type)) _thread.start_new_thread(save_reconstruction, (cfg, )) elif script_mode == 'drive': _thread.start_new_thread(drive_model, (cfg, model_path, vae_path, env_type)) elif script_mode == 'optimize': _thread.start_new_thread( optimize_model, (cfg, model_path, vae_path, auto_mode, env_type)) elif script_mode == 'train_vae': print('collecting data for vae training...') #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. ''' 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() range = Sensor(pi, cfg.RANGE_GPIOS) # range.update_loop_body() # V.add(range, outputs=['range/cms'], threaded=True) print("cfg.CAMERA_TYPE", 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 = [] threaded = True if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, delay=cfg.SIM_ARTIFICIAL_LATENCY) threaded = True inputs = ['angle', 'throttle'] elif cfg.CAMERA_TYPE == "PICAM": from donkeycar.parts.camera import PiCamera print("NBNBNBNBNBNBNNBNBNBNBNBNB",666666) 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) # pos = cam.update() # print(pos) 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) 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 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) else: #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) #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) 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') # 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 = [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') 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. imgW = 160 imgH = 120 def JudgeLR(p): #设置缓冲距离 SILENT_LR_DISTANCE = imgW/12 #img中心点 center = [imgW/2, imgH/2] #如果目标位于图片中心左侧,小车左转 p1=(p[0]+p[2])/2 p2=center[0]-SILENT_LR_DISTANCE p3=(p[0]+p[2])/2 p4=center[0]+SILENT_LR_DISTANCE if p1<p2: print(-0.5) return -1 #如果目标位于图片中心右侧,小车右转 elif p3>p4: print(0.5) return 1 def JudgeFB(p): #设置缓冲距离 SILENT_FB_DISTANCE = imgH/12 #img中心点 center = [imgW/2, imgH/2] #如果目标上边框>imgH的1/3,小车前进 p5=p[3]/imgH if p5>1/3: return 0.75 else: return 0 class DriveMode: def run(self, mode, user_angle, user_throttle, pilot_angle, pilot_throttle): global force_stop global music_ing #获取超声传感器测距信息distance distance = range.distance_get() #获取目标检测结果 with open("/home/pi/projects/donkeycar/parts/pos_data.txt", "rb") as f: try: outputxmin = pickle.load(f) outputymin = pickle.load(f) outputxmax = pickle.load(f) outputymax = pickle.load(f) # music_controller = pickle.load(f) music_on = pickle.load(f) print(music_on) except EOFError: return None p=[outputxmin,outputymin,outputxmax,outputymax] print("music_ing",music_ing) print("nusic_on",music_on) # if music_controller == 1: # os.system('mpg123 music.mp3') # elif music_controller == 0: # os.system('q') imgW = 160 imgH = 120 #设置缓冲距离 SILENT_LR_DISTANCE = imgW/12 #img中心点 center = [imgW/2, imgH/2] p1=(p[0]+p[2])/2 p2=center[0]-SILENT_LR_DISTANCE p3=(p[0]+p[2])/2 p4=center[0]+SILENT_LR_DISTANCE d1=imgH-p[3] d2=p1-center[0] angle = -0.3 if p[0]<0:#若未检测到行人 user_throttle = 0.63 user_angle = angle print('未检测到行人!!') print("角度",angle) if distance < 40: print('distance:',distance) user_throttle = 0.0 user_angle = -0.3 else:#若检测到行人 if distance >= 40: #根据超声测距控制油门 throttle = (0.15/110)*distance+0.58 if throttle<0.7: user_throttle = throttle else: user_throttle = 0.7 #根据图像控制油门 # user_throttle = 0.65 # user_angle = -0.3 # #如果目标位于图片中心左侧,小车左转 # if p1<p2: # print("左转",-0.5) # user_angle = -0.5 # #如果目标位于图片中心右侧,小车右转 # elif p3>p4: # print("右转",0.2) # user_angle = 0.2 # # elif d1>imgH/3: # # print('加速') # # user_throttle = 0.65 # # elif d1<imgH/3 and d1>imgH/4: # # print('减速') # # user_throttle = 0.65 user_angle = (0.7/70)*d2-0.3 if user_angle<-0.3 angle = -0.3+(abs(user_angle+0.3))/3 elif user_angle>-0.3 angle = -0.3-(abs(user_angle+0.3))/3 elif distance < 40: print('distance:',distance) user_throttle = 0.0 user_angle = -0.3 angle = user_angle if music_on == 1 and music_ing==0: pygame.mixer.music.load("music1.mp3") pygame.mixer.music.play(-1) music_ing = 1 user_throttle = 0 elif music_on == 1 and music_ing == 1: user_throttle = 0 elif music_on == 0 and music_ing == 1: pygame.mixer.music.stop() music_ing = 0 if mode == 'user': print('油门',user_throttle) print('角度',user_angle) print(p) return user_angle, user_throttle elif mode == 'local_angle': return pilot_angle if pilot_angle else 0.0, user_throttle # else: # if distance < 30: # print('2:',distance) # pilot_throttle = 0.0 # return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0 else: if p[0]<0:#若未检测到行人 pilot_throttle = 0.0 pilot_angle = 0.0 print('未检测到行人',p) return pilot_angle,pilot_throttle else: print('检测到行人!!!') if distance >= 30: return JudgeLR(p),JudgeFB(p) elif distance < 30: print('2:',distance) pilot_throttle = 0.0 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 == "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 inputs=['cam/image_array', 'user/angle', 'user/throttle', 'range/cms', #新增传感器数据 'user/mode'] types=['image_array', 'float', 'float', 'float', #对应新增传感器数据的类型 'str'] 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'] 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: 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.") #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, meta=[]): model_type = cfg.DEFAULT_MODEL_TYPE #Initialize car V = dk.vehicle.Vehicle() if cfg.RS_PATH_PLANNER: from donkeycar.parts.realsense2 import RS_T265 print("RS t265 with path tracking") odometry = RS_T265(path_output=True, stereo_output=False, image_output=False) V.add(odometry, outputs=['rs/trans', 'rs/yaw'], threaded=True) V.add(PosStream(), inputs=['rs/trans', 'rs/yaw'], outputs=['pos/x', 'pos/y', 'pos/yaw']) else: raise Exception("This script is for RS_PATH_PLANNER only") from donkeycar.parts.realsense2 import RS_D435i cam = RS_D435i(img_type=cfg.D435_IMG_TYPE, image_w=cfg.D435_IMAGE_W, image_h=cfg.D435_IMAGE_H, frame_rate=cfg.D435_FRAME_RATE) V.add(cam, inputs=[], outputs=['cam/image_array'], threaded=True) # class Distance: # def run(self, img): # h,w = img.shape # arr = img[50:w-50,0:h-60].flatten() # mean = np.mean(arr) # print(mean) # V.add(Distance(), # inputs=['cam/image_array'], outputs=['null']) ctr = get_js_controller(cfg) V.add(ctr, inputs=['null'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) # class debug: # def run(self, angle): # print(angle) # V.add(debug(), inputs=['user/angle'], outputs=['null']) #This web controller will create a web server web_ctr = LocalWebControllerPlanner() V.add( web_ctr, inputs=[ 'map/image', 'cam/image_array', 'user/mode', 'recording', 'throttle', 'angle' ], outputs=['web/angle', 'web/throttle', 'web/mode', 'web/recording'], # no part consumes this threaded=True) class UserCondition: def run(self, mode): if mode == 'user': return True else: return False V.add(UserCondition(), inputs=['user/mode'], outputs=['run_user']) class PilotCondition: def run(self, mode): if mode == 'user': return False else: return True V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot']) myRoute = Route(min_dist=cfg.PATH_MIN_DIST) V.add(myRoute, inputs=['pos/x', 'pos/y'], outputs=['nav/path', 'nav/waypoints'], threaded=True) if os.path.exists(cfg.RS_ROUTE_FILE): myRoute.load(cfg.RS_ROUTE_FILE) print("loaded route:", cfg.RS_ROUTE_FILE) def save_waypt(): myRoute.save_waypoint() ctr.set_button_down_trigger(cfg.SAVE_WPT_BTN, save_waypt) def save_route(): myRoute.save_route(cfg.RS_ROUTE_FILE) ctr.set_button_down_trigger(cfg.SAVE_ROUTE_BTN, save_route) def clear_route(): myRoute.clear() ctr.set_button_down_trigger(cfg.CLEAR_ROUTE_BTN, clear_route) img = PImage(resolution=(cfg.D435_IMAGE_W, cfg.D435_IMAGE_H), clear_each_frame=True) V.add(img, outputs=['map/image']) plot = WaypointPlot(scale=cfg.PATH_SCALE, offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2), color=(0, 0, 255)) V.add(plot, inputs=['map/image', 'nav/waypoints'], outputs=['map/image'], threaded=True) plot = PathPlot(scale=cfg.PATH_SCALE, offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2), color=(0, 0, 255)) V.add(plot, inputs=['map/image', 'nav/path'], outputs=['map/image'], threaded=True) # this is error function for PID control nav = Navigator(wpt_reach_tolerance=cfg.WPT_TOLERANCE) V.add(nav, inputs=['nav/waypoints', 'pos/x', 'pos/y', 'pos/yaw'], outputs=['nav/shouldRun', 'nav/error'], threaded=True, run_condition="run_pilot") ctr.set_button_down_trigger("left_shoulder", nav.decrease_target) ctr.set_button_down_trigger("right_shoulder", nav.increase_target) pid = PIDController(p=cfg.PID_P, i=cfg.PID_I, d=cfg.PID_D, debug=False) pilot = PID_Pilot(pid, cfg.PID_THROTTLE) V.add(pilot, inputs=['nav/shouldRun', 'nav/error'], outputs=['pilot/angle', 'pilot/throttle'], run_condition="run_pilot") pos_plot = PlotPose(scale=cfg.PATH_SCALE, offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2)) V.add(pos_plot, inputs=['map/image', 'pos/x', 'pos/y', 'pos/yaw'], outputs=['map/image']) if model_path: 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))) #When we have a model, first create an appropriate Keras part kl = dk.utils.get_model_by_type(model_type, cfg) load_model(kl, model_path) outputs = ['pilot/angle', 'pilot/throttle'] V.add(kl, inputs=['cam/image_array'], 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']) 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']) # data collection inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode'] types = ['image_array', 'float', 'float', 'str'] 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 V.add(RecordTracker(), inputs=["tub/num_records"], outputs=['records/alert']) 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') #tell the controller about the tub ctr.set_tub(tub) ctr.print_controls() #run the vehicle 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() print("cfg.CAMERA_TYPE", 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 = [] threaded = True if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, 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) 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 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) else: #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) #this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) #滤除油门小于的数据,关于倒车zpy #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) 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) #目标检测部分zpy class Detect(): def __init__(self, cfg): self.switch = 0 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) template1 = cv2.imread('/home/pi/mycar/picture/GandB.jpg', 1) template2 = cv2.imread('/home/pi/mycar/picture/green2.jpg', 1) temp_b = cv2.cvtColor(template1, cv2.COLOR_BGR2GRAY) #template=cv2.cvtColor(template,COLOR_BGR2GRAY) res1 = cv2.matchTemplate(img, template1, cv2.TM_CCOEFF_NORMED) min_val1, max_val1, min_loc1, max_loc1 = cv2.minMaxLoc(res1) res2 = cv2.matchTemplate(img, template2, cv2.TM_CCOEFF_NORMED) min_val2, max_val2, min_loc2, max_loc2 = cv2.minMaxLoc(res2) #进门1,出门0 if max_val1 > 0.65 and max_val2 < 0.65: self.switch = 1 print("\tmatch-1\t", max_val1, max_val2) return self.switch elif max_val1 < 0.65 and max_val2 > 0.65: self.switch = 0 print("\tmatch-2\t", max_val1, max_val2) return self.switch else: print(max_val1, '---', max_val2) return self.switch 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') #利用imgprocess对输入的图像数据进行处理zpy #在自动驾驶模式下加入detect部分 V.add(Detect(cfg), inputs=['cam/image_array'], outputs=['match'], run_condition='run_pilot') # 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 = [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 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 = None if '.h5' in model_path_1 or '.uff' in model_path_1 or 'tflite' in model_path_1 or '.pkl' 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, filename) model_reload_cb = 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, model_path_1) weights_path = model_path_1.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'] outputs_0 = ['pilot/angle_0', 'pilot/throttle_0'] outputs_1 = ['pilot/angle_1', 'pilot/throttle_1'] if cfg.TRAIN_LOCALIZER: outputs.append("pilot/loc") #从模型中得到转向油门数据zpy V.add(kl, inputs=inputs, outputs=outputs_0, run_condition='run_pilot') V.add(kl_1, inputs=inputs, outputs=outputs_1, 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']) #zpy更改模式 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.match = 0 def run_threaded(self, match, a0, t0, a1, t1): self.angle_0 = a0 self.angle_1 = a1 self.throttle_0 = t0 self.throttle_1 = t1 #self.mode = mode self.match = match return self.angle, self.throttle def update(self): while True: self.angle, self.throttle = self.run(self.match, self.angle_0, self.angle_1, self.throttle_0, self.throttle_1) #print(self.angle) def run(self, match, angle_0, throttle_0, angle_1, throttle_1): #print(match,"\tm0",angle_0,throttle_0,"\tm1",angle_1,throttle_1) if match == 0: #print("mode:",pilot_mode," angle:",angle_0," throttle:",throttle_0) return angle_0, throttle_0 elif match == 1: #print("mode:",pilot_mode," angle:",angle_1," throttle:",throttle_1) return angle_1, throttle_1 else: return 0, 0 V.add(MyPilot(), inputs=[ 'match', 'pilot/angle_0', 'pilot/throttle_0', 'pilot/angle_1', 'pilot/throttle_1' ], outputs=['mypilot/angle', 'mypilot/throttle'], 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 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', 'mypilot/angle', '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 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 == "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 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.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'] 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: 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.") #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, 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)
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() print("cfg.CAMERA_TYPE", 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 = [] threaded = True if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, 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) 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 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', 'tub/num_records', 'showbutton'], outputs=[ 'user/angle', 'user/throttle', 'user/mode', 'recording', 'wim', 'wtg', 'gon' ], threaded=True) else: # 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', 'showbutton'], outputs=[ 'user/angle', 'user/throttle', 'user/mode', 'recording', 'wim', 'wtg', 'gon' ], 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) 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') # 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 = [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') 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. # Choose what inputs should change the car. '''下面代码能实现的重要条件是小车一开始就得处于原来的自动驾驶状态 ''' # wci=梁的where car is # wim=蔡的where i am, wtg=蔡的where to go, gon=蔡接收到button的值后,另一个变量go or not class Picmatch: def __init__(self): self.mylocation = '0' self.color = 'green' self.template = [] self.green_sign = 0 self.yellow_sign = 0 self.time1 = time.time() self.img = np.ones(shape=(120, 160, 3)) print('start!!!') self.template = cv2.imread(self.color + '_tem.jpg', 1) def run_threaded(self, img): self.img = img return self.mylocation def update(self): while True: self.mylocation = self.run(self.img) def run(self, img): try: img = np.array(img) img = np.array(img, dtype='uint8') img = img[70:120] except: return '0' # 相关系数匹配方法: cv2.TM_CCOEFF res1 = cv2.matchTemplate(img, self.template, cv2.TM_CCOEFF_NORMED) min_val1, max_val1, min_loc1, max_loc1 = cv2.minMaxLoc(res1) if (max_val1 >= 0.62): if (time.time() - self.time1 >= 2): self.green_sign += 1 if (self.green_sign == 7): self.green_sign = 1 self.time1 = time.time() self.mylocation = str(self.green_sign) #if (self.mylocation != '0'): #print(self.mylocation) #print(max_val1) return self.mylocation V.add(Picmatch(), inputs=['cam/image_array'], outputs=['wci'], threaded=True) class AfterPictureMatch: def __init__(self): self.extra = 0. self.showbutton = 0. # '输入位置后点此等待小车接应!' self.wci = '0' self.wim = '' self.wtg = '' self.gon = 0 self.music_sign = 0 self.pre_music_sign = 0 def run_threaded(self, wci, wim, wtg, gon): self.wci = wci self.wim = wim self.wtg = wtg self.gon = gon return self.extra, self.showbutton def update(self): while True: self.extra, self.showbutton = self.run(self.wci, self.wim, self.wtg, self.gon) def run(self, wci, wim, wtg, gon): # def run(self,wim,wtg,gon): print('wci:', wci, 'wim:', wim, 'wtg:', wtg, 'gon:', gon) if gon == 0: # gon默认为0 if wim == wci: print('可以准备出发') self.showbutton = 2. self.extra = 0 # '点此即可以出发!' # 蔡小程序接收到button值,蔡默认为0的gon改为1 # play(2)#提示音我在这里哦,点击按钮即可出发 if (self.pre_music_sign != 2): play(2) self.pre_music_sign = 2 return self.extra, self.showbutton elif wim != wci: if (wim == ''): self.extra = 0 self.showbutton = 0 self.pre_music_sign = 0 return self.extra, self.showbutton else: print('出发地不等于所在地') self.showbutton = 1. # '请等待小车到达!' self.extra = 1. # play(1)#"提示音:小车正在自动驾驶中" if (self.pre_music_sign != 1): play(1) self.pre_music_sign = 1 return self.extra, self.showbutton else: if wci == wtg: print('到达目的地') self.extra = 0 self.showbutton = 3. # '已经到达!',蔡gon由1改为0 # play(3)#提示音:已经到达祝主人一路顺风 if (self.pre_music_sign != 3): play(3) self.pre_music_sign = 3 return self.extra, self.showbutton elif wci != wtg: print('在前进过程中') self.extra = 1 self.showbutton = 1. # '请等待小车到达!' # play(1)#"提示音:小车正在自动驾驶中" if (self.pre_music_sign != 4): play(1) self.pre_music_sign = 4 return self.extra, self.showbutton V.add( AfterPictureMatch(), # inputs=['wim','wtg','gon'], inputs=['wci', 'wim', 'wtg', 'gon'], outputs=['extra', 'showbutton'], threaded=True) class PlayMusic: def __init__(self): self.play_sign = 0 self.pre_play_sign = 0 def run_threaded(self, music): self.play_sign = music if (music != 0 and self.pre_play_sign == 0): play(music) self.pre_play_sign = music if (music == 0): self.pre_play_sign = 0 def update(self): pass #V.add(PlayMusic,inputs=['music'],outputs=[],threaded=True) class Play_Music: def __init__(self): self.pre_play_sign = 0 def run(self, music=0): self.play_sign = music if (music != 0 and self.pre_play_sign == 0): play(music) self.pre_play_sign = music if (music == 0): self.pre_play_sign = 0 #V.add(Play_Music,inputs=['music'],outputs=[],) class DriveMode: def run(self, mode, extra, user_angle, user_throttle, pilot_angle, pilot_throttle): if mode == 'local_angle': return pilot_angle if pilot_angle else 0.0, user_throttle elif mode == 'user' or extra == 0: #extra默认为0,由梁的类输出的一个切换参量 return user_angle, user_throttle #已停止模式+进入沟里,控制驶出 else: return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0 #已进入自动驾驶模式 #下面是extra参量的来源 V.add(DriveMode(), inputs=[ 'user/mode', 'extra', '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 == "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 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.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'] 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: 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.") # 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 MAIN(): cfg = dk.load_config() CFG = cfg parts = [] # 1. power 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) # 2. driver drivers = [] ctr = get_js_controller(cfg) # 3. sensors cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) # 4. vehicle run configurations run_params = {'DRIVE_LOOP_HZ': 20, 'MAX_LOOPS': None} #### SHOULD END HERE """ V = dk.vehicle.Vehicle( cfg ) """ parts = [ { 'part': throttle, 'inputs': ['throttle'], 'outputs': [], 'threaded': False }, { 'part': steering, 'inputs': ['angle'], 'outputs': [], 'threaded': False }, { 'part': cam, 'inputs': [], 'outputs': ['cam/image_array'], 'threaded': True }, { 'part': ctr, 'inputs': [], 'outputs': ['angle', 'throttle', 'user/mode', 'recording'], 'threaded': True }, ] """ V.add(throttle, inputs=['throttle'], outputs=[]) V.add(steering, inputs=['angle'], outputs=[]) V.add( ctr, inputs=[], outputs=['angle', 'throttle', 'user/mode', 'recording'], threaded=True ) """ #V.add(cam, outputs=['cam/image_array'], threaded=True) return parts, CFG
"bno055/mag_x", "bno055/mag_y", "bno055/mag_z", "bno055/gyr_x", "bno055/gyr_y", "bno055/gyr_z", "bno055/acc_x", "bno055/acc_y", "bno055/acc_z", "bno055/lacc_x", "bno055/lacc_y", "bno055/lacc_z", "bno055/gra_x", "bno055/gra_y", "bno055/gra_z" ], threaded=True) # as5048a = AS5048A() # V.add(as5048a, outputs=['as5048a'], threaded=True) as5048a = speed() V.add(as5048a, outputs=['as5048a']) # cam = PiCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH) # V.add(cam, inputs=[], outputs=['cam/image_array'], threaded=True) ctr = get_js_controller(cfg) V.add(ctr, outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) pulse = Pulse(interval=4, cycle=0.5, length=20, min=0.0, max=0.3) V.add(pulse, outputs=['user/throttle', 'recording']) # this throttle filter will allow one tap back for esc reverse th_filter = ThrottleFilter() V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle']) steering_controller = PCA9685(cfg.STEERING_CHANNEL, cfg.PCA9685_I2C_ADDR, busnum=cfg.PCA9685_I2C_BUSNUM) steering = PWMSteering(controller=steering_controller,
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() from donkeycar.parts.dgym import DonkeyGymEnv inputs = [] cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, delay=cfg.SIM_ARTIFICIAL_LATENCY) threaded = True inputs = ['angle', 'throttle', 'brake'] 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 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) else: #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) #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) # 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"] 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, user_brake, pilot_angle, pilot_throttle, pilot_brake): if mode == 'user': return user_angle, user_throttle, user_brake elif mode == 'local_angle': return pilot_angle if pilot_angle else 0.0, user_throttle, user_brake else: return pilot_angle if pilot_angle else 0.0, pilot_throttle * cfg.AI_THROTTLE_MULT if pilot_throttle else 0.0, pilot_brake if pilot_brake else 0.0 V.add(DriveMode(), inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'user/brake', 'pilot/angle', 'pilot/throttle', 'pilot/brake' ], outputs=['angle', 'throttle', 'brake']) #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']) #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.RECORD_DURING_AI: inputs += ['pilot/angle', 'pilot/throttle'] types += ['float', 'float'] # 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') 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', 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, use_trt=False, use_half=False, model_type=None): # -------------------------------- # 1. Initialize car # -------------------------------- V = dk.vehicle.Vehicle() # -------------------------------- # 2. Add camera # -------------------------------- cam = CSICamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, framerate=cfg.CAMERA_FRAMERATE, crop_top=cfg.ROI_CROP_TOP, crop_bottom=cfg.ROI_CROP_BOTTOM) V.add(cam, inputs=[], outputs=['cam/image_array'], threaded=True) if cfg.USE_FPV: V.add(WebFpv(), inputs=['cam/image_array'], threaded=True) # send the FPV image through network at port 8890 # -------------------------------- # 3. Add gamepad or webpage controller # -------------------------------- if model_path: cfg.WEB_INIT_MODE = "local" if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT: from donkeycar.parts.controller import get_js_controller ctr = get_js_controller(cfg) V.add( ctr, inputs=['cam/image_array'], outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'], threaded=True) else: # This web controller will create a web server that is capable of managing steering, throttle, and modes, and more. ctr = LocalWebController(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) V = add_basic_modules(V, cfg) # e.g., record tracker # -------------------------------- # 4. Configure the AI neural network model # -------------------------------- if model_path: print('loading the self-driving model, model_path:', model_path) t0 = time.time() import torch device = torch.device('cuda') if not use_trt: if model_type == 'linear': drive_model = LinearModel().to(device) elif model_type == 'resnet18': drive_model = LinearResModel().to(device) elif model_type == 'rnn': drive_model = RNNModel().to(device) drive_model.load_state_dict( torch.load(model_path, map_location=lambda storage, loc: storage)) if use_half: drive_model.eval().half() else: drive_model.eval() else: from torch2trt import TRTModule drive_model = TRTModule() drive_model.load_state_dict(torch.load( model_path)) # no need to move to device if using torch2trt print('model loaded, time cost: %.2f s' % (time.time() - t0)) drive_class = DriveClass(cfg, model_type, drive_model, device, cam=cam, half=use_half) outputs = ['pilot/angle', 'pilot/throttle'] V.add(drive_class, inputs=['cam/image_array'], outputs=outputs, run_condition='run_pilot', threaded=False) # if use threaded mode, the input image will not be used, # because the thread will contineously read images from vehicle memory. # -------------------------------- # 5. Choose which control command to use # -------------------------------- out = ['angle', 'throttle'] if cfg.CONTROL_NOISE: print( '\n##########################################\nAdding Random Action Noise\n##########################################\n' ) out += ['user/angle_noise', 'user/throttle_noise'] V.add(DriveMode(cfg), inputs=[ 'user/mode', 'user/angle', 'user/throttle', 'pilot/angle', 'pilot/throttle' ], outputs=out) # -------------------------------- # 6. Configure the low-level controller that translates steering and throttle to PWM signals # -------------------------------- V = add_control_modules(V, cfg) # -------------------------------- # 7. Add tub to save data # -------------------------------- V, tub = add_tub_save_data(V, cfg) # -------------------------------- # 8. Print user guide # -------------------------------- if type(ctr) is LocalWebController: print("You can now go to <car_ip_address>:%d to drive your car." % cfg.WEB_CONTROL_PORT) 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) ctr.print_controls() # -------------------------------- # 9. press "Enter" to start if using AI mode, or else directly start the vehicle # -------------------------------- if model_path: enter = input("press ENTER to start racing") if enter == '': V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS ) #run the vehicle for DRIVE_LOOP_HZ, e.g., 20 HZ else: V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)