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, model_path=None, use_joystick=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
#Camera
#cam = MockCamera(resolution=cfg.CAMERA_RESOLUTION)
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
#Cropper
#ImgCrop works by cropping num pixels in from side of each border
mask = ImgCrop(200, 80, 0, 0) #top, bottom, left, right, but it's all backwards because OpenCV
#V.add(mask, inputs=['cam/image_array'], outputs=['cam/filtered1'])
#Greyscale filter
#grey_filter = ImgGreyscale()
#V.add(grey_filter, inputs=['cam/filtered1'], outputs=['cam/filtered2'])
#Gaussian blur
#gaus_blur = ImgGaussianBlur()
#V.add(gaus_blur, inputs=['cam/filtered2'], outputs=['cam/filtered3'])
#Adaptive threshold
#adaptive_thresh = AdaptiveThreshold()
#V.add(adaptive_thresh, inputs=['cam/filtered3'], outputs=['cam/filtered4'])
#Birds eye viewpoint transformation
#birds_eye = BirdsEyePerspectiveTxfrm()
#V.add(birds_eye, inputs=['cam/filtered4'], outputs=['cam/filtered_final'])
#Draw line
#draw_line = DrawLine((0, 200), (480, 200))
#V.add(draw_line, inputs=['cam/filtered4'], outputs=['cam/filtered_final'])
#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
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
#Lukes notes: This is another kind of a splitter, more like a flag, that gets piped into KerasCategorical to decide whether or not the autopilot has control over steering parts (technically, it is used as a run condition for the autopilot)
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/filtered_final'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
#Lukes notes: This is essentially a splitter - it links input channels to output (control) channels based on which mode the car was run in
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs=['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types=['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
class Timer:
def __init__(self, loops_timed):
self.start_time = time.time()
self.loop_counter = 0
self.loops_timed = loops_timed
def run(self):
self.loop_counter += 1
if self.loop_counter == self.loops_timed:
seconds = time.time() - self.start_time
print("{} loops takes {} seconds".format(
self.loops_timed, seconds))
self.loop_counter = 0
self.start_time = time.time()
timer = Timer(50)
V.add(timer)
#Part to save multiple image arrays from camera
class ImageArrays:
def __init__(self, steps):
tmp = np.zeros((120, 160, 3))
self.steps = steps
self.storage = [tmp for i in range(self.steps * 2 + 1)]
self.active_images = [tmp for i in range(3)]
def run(self, image):
self.storage.pop(0)
self.storage.append(image)
for i in range(3):
self.active_images[i] = self.storage[i * self.steps]
return np.array(self.active_images)
image_arrays = ImageArrays(10)
V.add(image_arrays,
inputs=['cam/image_array'],
outputs=['cam/image_arrays'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_arrays'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
ctr = SerialController()
V.add(ctr,
inputs=[],
outputs=['user/angle', 'user/throttle', 'user/mode'],
threaded=True)
V.add(LocalWebController(use_chaos=use_chaos),
inputs=['cam/image_array'],
outputs=['a', 'b', 'c', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, 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 drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single'):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
if model_type is None:
if cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = "categorical"
if model_type == "streamline":
camera_type = "streamline"
#Initialize car
V = dk.vehicle.Vehicle()
if camera_type == "stereo":
if cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
camA = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
camA = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
else:
raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE))
V.add(camA, outputs=['cam/image_array_a'], threaded=True)
V.add(camB, outputs=['cam/image_array_b'], threaded=True)
def stereo_pair(image_a, image_b):
'''
This will take the two images and combine them into a single image
One in red, the other in green, and diff in blue channel.
'''
if image_a is not None and image_b is not None:
width, height, _ = image_a.shape
grey_a = dk.utils.rgb2gray(image_a)
grey_b = dk.utils.rgb2gray(image_b)
grey_c = grey_a - grey_b
stereo_image = np.zeros([width, height, 3],
dtype=np.dtype('B'))
stereo_image[..., 0] = np.reshape(grey_a, (width, height))
stereo_image[..., 1] = np.reshape(grey_b, (width, height))
stereo_image[..., 2] = np.reshape(grey_c, (width, height))
else:
stereo_image = []
return np.array(stereo_image)
image_sterero_pair_part = Lambda(stereo_pair)
V.add(image_sterero_pair_part,
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
#elif camera_type == "streamline":
# print("using grayscale pi cam")
# assert(cfg.CAMERA_TYPE == "PICAM")
# from donkeycar.parts.camera import PiCamera
# cam = PiCamera(image_w=cfg.STREAMLINE_IMAGE_W, image_h=cfg.STREAMLINE_IMAGE_H, image_d=1)
else:
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.CAMERA_TYPE == "PICAM":
from donkeycar.parts.camera import PiCamera
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
cam = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
cam = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
from donkeycar.parts.controller import PS3JoystickController, PS4JoystickController
#cont_class = PS3JoystickController
cont_class = MyJoystickController
if cfg.CONTROLLER_TYPE == "ps4":
cont_class = PS4JoystickController
ctr = cont_class(throttle_scale=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
if cfg.USE_NETWORKED_JS:
from donkeycar.parts.controller import JoyStickSub
netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
V.add(netwkJs, threaded=True)
ctr.js = netwkJs
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#this throttle filter will allow one tap back for esc reverse
th_filter = ThrottleFilter()
V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
def led_cond(mode, recording, recording_alert, behavior_state,
reloaded_model):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if reloaded_model:
led.set_rgb(cfg.MODEL_RELOADED_LED_R, cfg.MODEL_RELOADED_LED_G,
cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = cfg.BEHAVIOR_LED_COLORS[behavior_state]
led.set_rgb(r, g, b)
return -1 #solid on
if recording:
return -1 #solid on
elif mode == 'user':
return 1
elif mode == 'local_angle':
return 0.5
elif mode == 'local':
return 0.1
return 0
if cfg.HAVE_RGB_LED:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
led_cond_part = Lambda(led_cond)
V.add(led_cond_part,
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'reloaded/model'
],
outputs=['led/blink_rate'])
V.add(led, inputs=['led/blink_rate'])
def get_record_alert_color(num_records):
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
def record_tracker(num_records):
if num_records is None:
return 0
if record_tracker.last_num_rec_print != num_records or record_tracker.force_alert:
record_tracker.last_num_rec_print = num_records
if num_records % 10 == 0:
print("recorded", num_records, "records")
if num_records % cfg.REC_COUNT_ALERT == 0 or record_tracker.force_alert:
record_tracker.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
record_tracker.force_alert = 0
if record_tracker.dur_alert > 0:
record_tracker.dur_alert -= 1
if record_tracker.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
record_tracker.last_num_rec_print = 0
record_tracker.dur_alert = 0
record_tracker.force_alert = 0
rec_tracker_part = Lambda(record_tracker)
V.add(rec_tracker_part,
inputs=["tub/num_records"],
outputs=['records/alert'])
if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
#then we are not using the circle button. hijack that to force a record count indication
def show_record_acount_status():
record_tracker.last_num_rec_print = 0
record_tracker.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#IMU
if cfg.HAVE_IMU:
imu = Mpu6050()
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
#Behavioral state
if cfg.TRAIN_BEHAVIORS:
bh = BehaviorPart(cfg.BEHAVIOR_LIST)
V.add(bh,
outputs=[
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
])
try:
ctr.set_button_down_trigger('L1', bh.increment_state)
except:
pass
inputs = ['cam/image_array', "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs = [
'cam/image_array', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = ['cam/image_array']
def load_model(kl, model_path):
start = time.time()
try:
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except:
print('problems loading model', model_path)
def load_weights(kl, weights_path):
start = time.time()
try:
print('loading model weights', weights_path)
kl.model.load_weights(weights_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except:
print('problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
import keras
with open(json_fnm, 'r') as handle:
contents = handle.read()
kl.model = keras.models.model_from_json(contents)
print('finished loading json in %s sec.' % (str(time.time() - start)))
if model_path:
#When we have a model, first create an appropriate Keras part
#if model_type == "streamline":
#kl = KerasStreamline()
#else:
kl = dk.utils.get_model_by_type(model_type, cfg)
if '.h5' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
print(filename, "was changed!")
load_model(kl, filename)
fw_part = FileWatcher(model_path,
reload_model,
wait_for_write_stop=10.0)
V.add(fw_part, outputs=['reloaded/model'])
elif '.json' in model_path:
#when we have a .json extension
#load the model from their and look for a matching
#.wts file with just weights
load_model_json(kl, model_path)
weights_path = model_path.replace('.json', '.weights')
load_weights(kl, weights_path)
def reload_weights(filename):
print(filename, "was changed!")
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
fw_part = FileWatcher(model_path,
reload_weights,
wait_for_write_stop=1.0)
V.add(fw_part, outputs=['reloaded/model'])
else:
#previous default behavior
load_model(kl, model_path)
V.add(kl,
inputs=inputs,
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
#Drive train setup
if cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC":
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
steering_controller = PCA9685(cfg.STEERING_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE":
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT,
cfg.HBRIDGE_PIN_RIGHT)
throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM
left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD,
cfg.HBRIDGE_PIN_LEFT_BWD)
right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD,
cfg.HBRIDGE_PIN_RIGHT_BWD)
two_wheel_control = TwoWheelSteeringThrottle()
V.add(two_wheel_control,
inputs=['throttle', 'angle'],
outputs=['left_motor_speed', 'right_motor_speed'])
V.add(left_motor, inputs=['left_motor_speed'])
V.add(right_motor, inputs=['right_motor_speed'])
elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM":
from donkeycar.parts.actuator import ServoBlaster, PWMSteering
steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin
#PWM pulse values should be in the range of 100 to 200
assert (cfg.STEERING_LEFT_PWM <= 200)
assert (cfg.STEERING_RIGHT_PWM <= 200)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(motor, inputs=["throttle"])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
if cfg.TRAIN_BEHAVIORS:
inputs += [
'behavior/state', 'behavior/label', "behavior/one_hot_state_array"
]
types += ['int', 'str', 'vector']
if cfg.HAVE_IMU:
inputs += [
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
]
types += ['float', 'float', 'float', 'float', 'float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if type(ctr) is LocalWebController:
print("You can now go to <your pi ip address>:8887 to drive your car.")
elif isinstance(ctr, JoystickController):
print("You can now move your joystick to drive your car.")
#tell the controller about the tub
ctr.set_tub(tub)
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, model_type=None):
'''
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:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
V.add(cam, outputs=['cam/image_array'], threaded=True)
V.add(LocalWebController(),
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
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)
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)
if '.h5' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
elif '.json' in model_path:
#when we have a .json extension
#load the model from their and look for a matching
#.wts file with just weights
load_model_json(kl, model_path)
weights_path = model_path.replace('.json', '.weights')
load_weights(kl, weights_path)
outputs = ['pilot/angle', 'pilot/throttle']
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'])
#Drive train setup
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'])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
print("You can now go to <your pi ip address>:8887 to drive your car.")
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialisation the CAR
V = dk.vehicle.Vehicle()
# Initialisation of the CAMERA
if cfg.OS == cfg.LINUX_OS:
if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK:
print("Camera {} can't be used.".format(cfg.CAMERA_MACBOOK))
sys.exit()
elif cfg.CAMERA_ID == cfg.CAMERA_MOCK:
cam = MockCamera()
else:
cam = PiCamera(camera=cfg.CAMERA_ID,
resolution=cfg.CAMERA_RESOLUTION)
elif cfg.OS == cfg.MAC_OS:
if cfg.CAMERA_ID == cfg.CAMERA_MACBOOK:
cam = MacWebcam()
else:
cam = VideoStream(camera=cfg.CAMERA_ID,
framerate=cfg.DRIVE_LOOP_HZ)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, modes, and more.
ctr = LocalWebController()
if (cfg.LANE_DETECTOR):
ctr_inputs = ['cam/image_overlaid_lanes', 'ld/runtime']
#Lane Detector
#Detects the lane the car is in and outputs a best estimate for the 2 lines
lane_detector = LanesDetector(cfg.CAMERA_ID, cfg.LD_PARAMETERS)
V.add(lane_detector,
inputs=['cam/image_array', 'algorithm/reset'],
outputs=['cam/image_overlaid_lanes', 'ld/runtime'])
else:
ctr_inputs = ['cam/image_array']
V.add(
ctr,
ctr_inputs,
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording'
#'algorithm/reset'
],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasLinear()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
if cfg.OS == cfg.LINUX_OS:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
elif cfg.OS == cfg.MAC_OS:
steering_controller = MockController()
throttle_controller = MockController()
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
center_pulse=cfg.STEERING_CENTER_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle = PWMThrottle(
controller=throttle_controller,
cal_max_pulse=cfg.THROTTLE_FORWARD_CAL_PWM,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
cal_min_pulse=cfg.THROTTLE_REVERSE_CAL_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
print("You can now go to <your pi ip address>:8887 to drive your car.")
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg):
'''
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=['cam/image_array'],
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)
# #This web controller will create a web server. We aren't using any controls, just for visualization.
web_ctr = LocalWebController(port=cfg.WEB_CONTROL_PORT,
mode=cfg.WEB_INIT_MODE)
V.add(web_ctr,
inputs=['map/image'],
outputs=['web/angle', 'web/throttle', 'web/mode', 'web/recording'],
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,
steering_zero_pulse=cfg.STEERING_STOPPED_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,
throttle_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("###############################################################################")
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'])
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, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
memory = RedisMemory()
V = dk.vehicle.Vehicle(mem=memory)
clock = Timestamp()
V.add(clock, outputs='timestamp')
# ***** CAMERA *****
print("Starting camera")
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
# ***** Web Controller *****
print("Starting web controller")
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# ***** SPEKTRUM/MOVE32 REMOTE *****
print("Starting Spektrum/Move32")
#rc = SpektrumRemoteReceiver(cfg.SPEKTRUM_OFFSET, cfg.SPEKTRUM_SCALE, cfg.SPEKTRUM_DEFAULT, cfg.SPEKTRUM_SERIALPORT)
rc = Move32Receiver(cfg.MOVE32_OFFSET, cfg.MOVE32_SCALE,
cfg.MOVE32_DEFAULT, cfg.MOVE32_SERIALPORT,
cfg.MOVE32_RXTYPE, cfg.MOVE32_RXAUTO,
cfg.MOVE32_TIMEOUT)
V.add(rc,
threaded=True,
outputs=['rc0', 'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7'])
def rc_convert_func(*args):
angle = args[0]
throttle = args[1]
mode = 'manual' if args[
2] > 0.3 else 'auto' if args[2] < -0.3 else 'auto_angle'
recording = args[3] <= 0.3
return angle, throttle, mode, recording
V.add(Lambda(rc_convert_func),
inputs=['rc0', 'rc2', 'rc5', 'rc4'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'])
# ***** user/mode -> run_pilot *****
V.add(Lambda(lambda mode: mode.lower() != 'manual'),
inputs=['user/mode'],
outputs=['run_pilot'])
# ***** cam/image_array -> pilot/angle,pilot_throttle *****
# Run the pilot if the mode is not user.
print("Starting KerasCategorical")
kl = KerasCategorical()
if model_path:
print("Loading model...")
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# ***** user/*, pilot/* -> angle, throttle *****
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'manual':
return user_angle, user_throttle
elif mode == 'auto_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
# ***** throttle, angle -> motor_left, motor_right *****
ackermann_to_diff_converter = AckermannToDifferentialDriveConverter(
cfg.ACKERMANN_LENGTH, cfg.ACKERMANN_WIDTH)
V.add(ackermann_to_diff_converter,
inputs=['angle', 'throttle'],
outputs=['motor_left', 'motor_right'])
# ***** motor_left, motor_right -> DRIVE *****
motors_part = DifferentialDriveActuator_MotorHat(
cfg.MOTORHAT_ADDR, cfg.MOTORHAT_LEFT_FRONT_ID,
cfg.MOTORHAT_LEFT_REAR_ID, cfg.MOTORHAT_RIGHT_FRONT_ID,
cfg.MOTORHAT_RIGHT_REAR_ID)
V.add(motors_part, inputs=['motor_left', 'motor_right'])
# ***** output debug data *****
debug_keys = [
'user/mode',
'recording',
'run_pilot',
"angle",
"throttle",
"motor_left",
"motor_right",
] #'rc1', 'rc2', 'rc3', 'rc4', 'rc5', 'rc6', 'rc7', 'rc8']
def debug_func(*args):
print(args[0], " ", args[1], " ", args[2],
" ".join("{:5.2f}".format(e) for e in args[3:]))
V.add(Lambda(debug_func), inputs=debug_keys)
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg ):
'''
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 = LocalWebController()
V.add(ctr,
inputs=['cam/image_array', 'tub320x240_train/num_records'],
outputs=['angle', '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=['throttle'], outputs=['throttle'])
drive_train = None
#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)
drive_train = dict()
drive_train['steering'] = steering
drive_train['throttle'] = throttle
V.add(steering, inputs=['angle'], threaded=True)
V.add(throttle, inputs=['throttle'], threaded=True)
elif cfg.DRIVE_TRAIN_TYPE == "MM1":
from donkeycar.parts.robohat import RoboHATDriver
drive_train = RoboHATDriver(cfg)
V.add(drive_train, inputs=['angle', 'throttle'])
ctr.drive_train = drive_train
ctr.drive_train_type = cfg.DRIVE_TRAIN_TYPE
class ShowHowTo:
def __init__(self):
print(f"Go to http://{gethostname()}.local:8887/calibrate to calibrate ")
def run(self):
pass
V.add(ShowHowTo())
#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):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
#this part stacks the last 3 images into channels of a single output image
img_stack = ImgStack()
V.add(img_stack,
inputs=['cam/image_array'],
outputs=['img_stack'])
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['img_stack'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_contion only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['img_stack'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = JankyJoystick(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = JankyJoystickcfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
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):
'''
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.
'''
# Do imports.
from donkeycar.parts.transform import Lambda
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
from donkeycar.parts.datastore import TubHandler
from donkeycar.parts.controller import LocalWebController, JoystickController
from donkeyturbo.pilot import DTKerasPilot
# Initialize a car.
V = dk.vehicle.Vehicle()
# Camera.
from donkeyturbo.camera import DTPiCamera
cam = DTPiCamera()
V.add(cam, outputs=['cam/image_array'], threaded=True)
# Controller.
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
# NOTE(r7vme): Always use DTKerasPilot
kl = DTKerasPilot(config=cfg.DT_PILOT_CONFIG)
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# Add tub to save data.
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# Run the vehicle.
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg,
model_path=None,
use_joystick=False,
use_tx=False,
use_pirf=False):
'''
Start the drive loop
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
# Initialize car
V = dk.vehicle.Vehicle()
if cfg.USE_WEB_CAMERA:
cam = Webcam(resolution=cfg.CAMERA_RESOLUTION)
else:
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
# modify max_throttle closer to 1.0 to have more power
# modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
steering_axis=cfg.JOYSTICK_STEERING_AXIS,
btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON,
btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON,
btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON,
btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON,
btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON,
btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON,
btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON,
btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON,
btn_toggle_const_throttle=cfg.
JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON,
verbose=cfg.JOYSTICK_VERBOSE)
elif use_tx or cfg.USE_TX_AS_DEFAULT:
ctr = TxController(throttle_tx_min=cfg.TX_THROTTLE_MIN,
throttle_tx_max=cfg.TX_THROTTLE_MAX,
steering_tx_min=cfg.TX_STEERING_MIN,
steering_tx_max=cfg.TX_STEERING_MAX,
throttle_tx_thresh=cfg.TX_THROTTLE_TRESH,
verbose=cfg.TX_VERBOSE)
fpv = FPVWebController()
V.add(fpv, inputs=['cam/image_array'], threaded=True)
elif use_pirf or cfg.USE_PI_RF_AS_DEFAULT:
ctr = PiRfController(throttle_tx_min=cfg.PI_RF_THROTTLE_MIN,
throttle_tx_max=cfg.PI_RF_THROTTLE_MAX,
steering_tx_min=cfg.PI_RF_STEERING_MIN,
steering_tx_max=cfg.PI_RF_STEERING_MAX,
throttle_tx_thresh=cfg.PI_RF_THROTTLE_TRESH,
steering_pin=cfg.PI_RF_STEERING_PIN,
throttle_pin=cfg.PI_RF_THROTTLE_PIN,
verbose=cfg.PI_RF_VERBOSE)
fpv = FPVWebController()
V.add(fpv, inputs=['cam/image_array'], threaded=True)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
emergencyCtrl = EmergencyController()
V.add(emergencyCtrl,
inputs=['user/mode'],
outputs=['user/mode'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
if not use_tx and not use_pirf:
# Run the pilot if the mode is not user and not Tx.
kl = KerasCategorical()
#kl = KerasLinear()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
if cfg.USE_PWM_ACTUATOR:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle', 'user/mode'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'pilot/angle', 'pilot/throttle'
]
types = [
'image_array', 'float', 'float', 'str', 'numpy.float32',
'numpy.float32'
]
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False, use_reader=False):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
elif use_reader or cfg.USE_READER_AS_DEFAULT:
pi = pigpio.pi()
ctr = reader(pi,
12,
18,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
#Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
#Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None, use_joystick=False, use_tx=False):
global myConfig
global throttle
global ctr
global V
'''
Start the drive loop
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
# Initialize car
V = dk.vehicle.Vehicle()
configCtrl = ConfigController(cfg.CONFIG_PATH)
logger = logging.getLogger(myConfig['DEBUG']['PARTS']['MAIN']['NAME'])
logger.setLevel(CONFIG2LEVEL[myConfig['DEBUG']['PARTS']['MAIN']['LEVEL']])
V.add(configCtrl, threaded=True)
def get_tsc():
return int(round(time.time() * 1000))
logger.info("Init timestamper")
get_tsc_part = Lambda(get_tsc)
V.add(get_tsc_part, outputs=['ms'])
logger.info("Init Cam part")
if cfg.USE_WEB_CAMERA:
cam = Webcam(resolution=cfg.CAMERA_RESOLUTION,
fps=cfg.CAMERA_FPS,
framerate=cfg.CAMERA_FRAMERATE)
else:
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
logger.info("Init Controller part")
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
# modify max_throttle closer to 1.0 to have more power
# modify steering_scale lower than 1.0 to have less responsive steering
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE,
throttle_axis=cfg.JOYSTICK_THROTTLE_AXIS,
steering_axis=cfg.JOYSTICK_STEERING_AXIS,
btn_mode=cfg.JOYSTICK_DRIVING_MODE_BUTTON,
btn_record_toggle=cfg.JOYSTICK_RECORD_TOGGLE_BUTTON,
btn_inc_max_throttle=cfg.JOYSTICK_INCREASE_MAX_THROTTLE_BUTTON,
btn_dec_max_throttle=cfg.JOYSTICK_DECREASE_MAX_THROTTLE_BUTTON,
btn_inc_throttle_scale=cfg.JOYSTICK_INCREASE_THROTTLE_SCALE_BUTTON,
btn_dec_throttle_scale=cfg.JOYSTICK_DECREASE_THROTTLE_SCALE_BUTTON,
btn_inc_steer_scale=cfg.JOYSTICK_INCREASE_STEERING_SCALE_BUTTON,
btn_dec_steer_scale=cfg.JOYSTICK_DECREASE_STEERING_SCALE_BUTTON,
btn_toggle_const_throttle=cfg.
JOYSTICK_TOGGLE_CONSTANT_THROTTLE_BUTTON,
verbose=cfg.JOYSTICK_VERBOSE)
V.add(
ctr,
inputs=['cam/image_array', 'pilot/annoted_img'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
elif use_tx or cfg.USE_TX_AS_DEFAULT:
#This is Tx controller (pilot Donkey from a RC Tx transmiter/receiver)
ctr = TxController(verbose=cfg.TX_VERBOSE)
V.add(ctr,
inputs=[
'user/mode', 'vehicle_armed', 'cam/image_array',
'pilot/annoted_img'
],
outputs=[
'user/angle', 'user/throttle', 'recording', 'lane', 'ch5',
'ch6', 'speedometer', 'sensor_left', 'sensor_right'
],
threaded=True)
actionctr = TxAuxCh()
V.add(actionctr,
inputs=['user/mode', 'vehicle_armed', 'ch5', 'ch6', 'recording'],
outputs=['user/mode', 'vehicle_armed', 'flag', 'recording'],
threaded=False)
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(
ctr,
inputs=['cam/image_array', 'pilot/annoted_img'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
if cfg.USE_THROTTLEINLINE:
logger.info("Init throttleInLine part")
throttleinline = ThrottleInLine(cfg.THROTTLEINLINE_ANGLE_MIN,
cfg.THROTTLEINLINE_ANGLE_MAX)
V.add(throttleinline,
inputs=['cam/image_array'],
outputs=['pilot/throttle_boost', 'pilot/annoted_img'],
threaded=True)
logger.info("Init emergency part")
emergencyCtrl = EmergencyController()
V.add(emergencyCtrl,
inputs=['user/mode'],
outputs=['user/mode'],
threaded=True)
perfMngt = dumpPerf()
V.add(perfMngt, inputs=['user/mode'], threaded=False)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
logger.info("Init pilot part")
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
logger.info("Init Model part")
# Run the pilot if the mode is not user and not Tx.
if (myConfig['MODEL']['MODEL_IN_USE'] == 0):
kl = KerasCategorical()
if (myConfig['MODEL']['MODEL_IN_USE'] == 1):
kl = KerasCategorical1()
#kl = KerasLinear()
if model_path:
if (os.path.exists(model_path)):
logger.info("IA : Load integrated model")
kl.load(model_path)
else:
# Model reconstruction from JSON file
logger.info("IA : Load Weights + Model Architecture model")
kl.load2(model_path)
if (myConfig['MODEL']['MODEL_IN_USE'] == 0):
V.add(kl,
inputs=['cam/image_array'],
outputs=[
'pilot/angle', 'pilot/throttle', 'pilot/fullspeed',
'pilot/lane', 'pilot/angle_bind'
],
run_condition='run_pilot')
if (myConfig['MODEL']['MODEL_IN_USE'] == 1):
V.add(kl,
inputs=['cam/image_array', 'speedometer'],
outputs=[
'pilot/angle', 'pilot/throttle', 'pilot/fullspeed',
'pilot/brake', 'pilot/angle_bind'
],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle, throttle_boost):
if mode == 'user':
return user_angle, user_throttle
else:
if cfg.USE_THROTTLEINLINE:
if throttle_boost:
pilot_throttle = pilot_throttle * cfg.THROTTLEINLINE_BOOST_FACTOR
logger.debug("Apply Boost")
if mode == 'local_angle':
return pilot_angle, user_throttle
else:
logger.debug(
'drive_mode: Pilot return angle={:01.2f} throttle={:01.2f}'
.format(pilot_angle, pilot_throttle))
if (pilot_angle >
myConfig['POST_PILOT']['STEERING_TRIM_RIGHT_THRES']):
pilot_angle = pilot_angle * myConfig['POST_PILOT'][
'STEERING_TRIM_RIGHT_FACTOR']
if (pilot_angle <
-myConfig['POST_PILOT']['STEERING_TRIM_LEFT_THRES']):
pilot_angle = pilot_angle * myConfig['POST_PILOT'][
'STEERING_TRIM_LEFT_FACTOR']
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle', 'pilot/throttle_boost'
],
outputs=['angle', 'throttle'])
if cfg.USE_PWM_ACTUATOR:
logger.info("Init Actuator part")
if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1:
steering_controller = ctr
else:
steering_controller = PCA9685(channel=cfg.STEERING_CHANNEL,
busnum=cfg.STEERING_I2C_BUS)
steering = PWMSteering(controller=steering_controller)
if myConfig['ACTUATOR']['ACTUATOR_CTRL_SERIAL'] == 1:
throttle_controller = ctr
else:
throttle_controller = PCA9685(channel=cfg.THROTTLE_CHANNEL,
busnum=cfg.THROTTLE_I2C_BUS)
throttle = PWMThrottle(controller=throttle_controller)
V.add(steering, inputs=['angle'])
V.add(throttle,
inputs=[
'throttle', 'user/mode', 'vehicle_armed', 'pilot/fullspeed',
None, 'pilot/lane', 'sensor_left', 'sensor_right'
])
if cfg.BATTERY_USE_MONITOR:
logger.info("Init Battery Monitor part")
battery_controller = BatteryController(nbCells=cfg.BATTERY_NCELLS)
V.add(battery_controller, outputs=['battery'], threaded=True)
# add tub to save data
inputs = [
'cam/image_array', 'ms', 'user/angle', 'user/throttle', 'user/mode',
'pilot/angle', 'pilot/throttle', 'flag', 'speedometer', 'lane'
]
types = [
'image_array', 'int', 'float', 'float', 'str', 'numpy.float32',
'numpy.float32', 'str', 'float', 'int'
]
logger.info("Init Tub Handler part")
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
if use_tx or cfg.USE_TX_AS_DEFAULT:
fpv = FPVWebController()
V.add(fpv,
inputs=[
'cam/image_array', 'pilot/annoted_img', 'user/angle',
'user/throttle', 'user/mode', 'pilot/angle',
'pilot/throttle', 'pilot/throttle_boost', 'pilot/fullspeed',
'pilot/angle_bind'
],
threaded=True)
logger.info("Start main loop")
# run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
print("You can now go to <your pi ip address>:8887 to drive your car.")
def drive(cfg, model_path=None):
V = dk.vehicle.Vehicle()
V.mem.put(['square/angle', 'square/throttle'], (100, 100))
# display square box given by cooridantes.
cam = SquareBoxCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam,
inputs=['square/angle', 'square/throttle'],
outputs=['cam/image_array'])
# display the image and read user values from a local web controller
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle',
'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_contion only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# See if we should even run the pilot module.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'pilot_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
clock = Timestamp()
V.add(clock, outputs=['timestamp'])
# transform angle and throttle values to coordinate values
def f(x):
return int(x * 100 + 100)
l = Lambda(f)
V.add(l, inputs=['user/angle'], outputs=['square/angle'])
V.add(l, inputs=['user/throttle'], outputs=['square/throttle'])
# add tub to save data
inputs=['cam/image_array',
'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle',
'square/angle', 'square/throttle',
'user/mode',
'timestamp']
types=['image_array',
'float', 'float',
'float', 'float',
'float', 'float',
'str',
'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle for 20 seconds
V.start(rate_hz=50, max_loop_count=10000)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
def get_timestamp():
return time.time()
clock = Lambda(get_timestamp)
V.add(clock, outputs=['timestamp'])
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
curr_angle = 0.0
def smooth_angle(in_angle):
nonlocal curr_angle
delta = in_angle - curr_angle
if abs(delta) < 0.02:
curr_angle = in_angle
else:
curr_angle = curr_angle + delta * 0.15
print('smoothed angle:', curr_angle)
return curr_angle
angleSmoother = Lambda(smooth_angle)
V.add(angleSmoother, inputs=['user/angle'], outputs=['user/angle'])
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'],
outputs=['run_pilot'])
# Add velocity detector
vd = Lambda(make_velocity_detector())
V.add(vd, inputs=['cam/image_array'], outputs=['user/velocity'])
""""
def print_velocity(v):
print('velocity:', v)
pv = Lambda(print_velocity)
V.add(pv, inputs=['user/velocity'])
"""
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl, inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/velocity'],
run_condition='run_pilot')
def calc_throttle(inferred_v, actual_v):
throttle = 0.38
"""
if inferred_v > actual_v:
throttle = min((inferred_v - actual_v) * 1.0, 0.5)
print('V: inferred={} actual={} => throttle={}'.format(inferred_v, actual_v, throttle));
"""
if (inferred_v > 5):
throttle = 0.48
elif (inferred_v > 4):
throttle = 0.44
elif (inferred_v > 3):
throttle = 0.42
elif (inferred_v > 2):
throttle = 0.41
elif (inferred_v > 1):
throttle = 0.39
return throttle
ct = Lambda(calc_throttle)
V.add(ct, inputs=['pilot/velocity', 'user/velocity'],
outputs=['pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
# manual steer, manual throttle
return user_angle, user_throttle
elif mode == 'local_angle':
# auto steer, manual throttle
return pilot_angle, user_throttle
else:
# auto steer, auto throttle
print('PILOT: angle={} throttle={}'.format(pilot_angle, pilot_throttle))
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/velocity', 'user/throttle', 'user/mode', 'timestamp']
types = ['image_array', 'float', 'float', 'float', 'str', 'float']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
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, 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.
'''
#Initialize car
V = dk.vehicle.Vehicle()
from donkeycar.parts.camera import PiCamera
V.add(PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH),
outputs=['cam/image_array'],
threaded=True)
V.add(LocalWebController(),
inputs=['cam/image_array_face_box'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
print("You can now go to <your pi ip address>:8887 to drive your car.")
#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 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)
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=['cam/normalized/cropped'],
run_condition='run_pilot')
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)))
kl = dk.utils.get_model_by_type('jaws', cfg)
load_model(kl, model_path)
V.add(kl,
inputs=['cam/normalized/cropped'],
outputs=['face_x', 'face_y', 'face_w', 'face_h', 'confidence'],
run_condition='run_pilot')
class FaceFollowingPilot:
def run(self, x, y, w, h, confidence):
return 0, 0
V.add(FaceFollowingPilot(),
inputs=['face_x', 'face_y', 'face_w', 'face_h', 'confidence'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
class DrawBoxForFaceDetection:
def __init__(self, cfg):
self.cfg = cfg
def run(self, img, x, y, w, h, confidence):
if img is not None:
color = np.array([0, 255, 0], dtype=np.uint8)
color2 = np.array([255, 255, 0], dtype=np.uint8)
img_out = np.copy(img)
img_w, img_h = self.cfg.IMAGE_W, self.cfg.IMAGE_H
x_px = int(img_w * x)
y_px = int(img_h * y)
w_px = int(img_w * w)
h_px = int(img_h * h)
print('x:{}, y:{}, w:{}, h:{}'.format(x_px, y_px, w_px, h_px))
u = y_px
d = y_px + h_px
l = x_px
r = x_px + w_px
u = min(img_h - 1, max(0, u))
d = min(img_h - 1, max(0, d))
l = min(img_w - 1, max(0, l))
r = min(img_w - 1, max(0, r))
img_out[u:d, l] = color
img_out[u:d, r] = color
img_out[u, l:r] = color2
img_out[d, l:r] = color
return img_out
return img
V.add(DrawBoxForFaceDetection(cfg),
inputs=[
'cam/image_array', 'face_x', 'face_y', 'face_w', 'face_h',
'confidence'
],
outputs=['cam/image_array_face_box'],
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'])
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'])
# drive chain
# V.add(steering, inputs=['angle'])
# V.add(throttle, inputs=['throttle'])
class Dummy:
def run(self):
return 0
V.add(Dummy(), outputs=['climbing'])
V.add(JawsController(),
inputs=['angle', 'climbing', 'throttle'],
threaded=True)
#add tub to save data
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(
inputs=[
'cam/image_array', 'face_x', 'face_y', 'face_w', 'face_h',
'confidence'
],
types=['image_array', 'float', 'float', 'float', 'float', 'float'],
user_meta=meta)
V.add(tub,
inputs=[
'cam/image_array', 'face_x', 'face_y', 'face_w', 'face_h',
'confidence'
],
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'])
#run the vehicle for 20 seconds
V.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, model_wrapper=None, debug=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
if not debug:
from donkeycar.parts.camera import PiCamera
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
else:
print("Debug : ignoring camera.")
def fake_cam():
from PIL import Image
import numpy as np
dir_path = os.path.dirname(os.path.realpath(__file__))
img = Image.open(os.path.join(dir_path, 'img.jpg'))
a = np.array(img)
return a
fake_cam_part = Lambda(fake_cam)
V.add(fake_cam_part, outputs=['cam/image_array'])
ctr = LocalWebController(use_chaos=False)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'],
outputs=['run_pilot'])
# Run the pilot if the mode is not user.
if model_path:
model = load_model(model_path)
model_wrapper_class = getattr(model_wrappers, model_wrapper)
model_instance = model_wrapper_class(model=model)
V.add(model_instance, inputs=['cam/image_array'],
outputs=['pilot/angle1', 'pilot/throttle1'],
run_condition='run_pilot')
def bound_throttle(pilot_angle1, pilot_throttle1):
if pilot_angle1 is None:
pilot_angle1 = 0
if pilot_throttle1 is None:
pilot_throttle1 = 0
pilot_throttle = min(1, pilot_throttle1)
pilot_throttle = max(0, pilot_throttle)
return pilot_angle1, pilot_throttle
bound_throttle_part = Lambda(bound_throttle)
V.add(bound_throttle_part,
inputs=['pilot/angle1', 'pilot/throttle1'],
outputs=['pilot/angle', 'pilot/throttle'])
# add tub to save data
set_path = os.path.join(ROOT_PATH, "set_{}".format(datetime.now().strftime("%Y%m%d%H%M%S")))
os.makedirs(set_path)
inputs = ['cam/image_array', 'user/angle', 'user/throttle'] # 'user/mode'
rec = Recorder(path=set_path)
V.add(rec, inputs=inputs, run_condition='recording')
# Choose what inputs should change the car.
def drive_mode(mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=['user/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle'],
outputs=['angle', 'throttle'])
if not debug:
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
else:
print("Debug : ignoring controller part.")
# run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ,
max_loop_count=cfg.MAX_LOOPS)
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single',
meta=[]):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
if cfg.DONKEY_GYM:
#the simulator will use cuda and then we usually run out of resources
#if we also try to use cuda. so disable for donkey_gym.
os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
if camera_type == "stereo":
if cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
camA = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
camA = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=0)
camB = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
iCam=1)
else:
raise (Exception("Unsupported camera type: %s" % cfg.CAMERA_TYPE))
V.add(camA, outputs=['cam/image_array_a'], threaded=True)
V.add(camB, outputs=['cam/image_array_b'], threaded=True)
from donkeycar.parts.image import StereoPair
V.add(StereoPair(),
inputs=['cam/image_array_a', 'cam/image_array_b'],
outputs=['cam/image_array'])
else:
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
inputs = []
threaded = True
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.DONKEY_GYM:
from donkeycar.parts.dgym import DonkeyGymEnv
cam = DonkeyGymEnv(cfg.DONKEY_SIM_PATH,
env_name=cfg.DONKEY_GYM_ENV_NAME)
threaded = True
inputs = ['angle', 'throttle']
elif cfg.CAMERA_TYPE == "PICAM":
from donkeycar.parts.camera import PiCamera
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "WEBCAM":
from donkeycar.parts.camera import Webcam
cam = Webcam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "CVCAM":
from donkeycar.parts.cv import CvCam
cam = CvCam(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "CSIC":
from donkeycar.parts.camera import CSICamera
cam = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM)
elif cfg.CAMERA_TYPE == "V4L":
from donkeycar.parts.camera import V4LCamera
cam = V4LCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE)
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam,
inputs=inputs,
outputs=['cam/image_array'],
threaded=threaded)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
#modify max_throttle closer to 1.0 to have more power
#modify steering_scale lower than 1.0 to have less responsive steering
from donkeycar.parts.controller import get_js_controller
ctr = get_js_controller(cfg)
if cfg.USE_NETWORKED_JS:
from donkeycar.parts.controller import JoyStickSub
netwkJs = JoyStickSub(cfg.NETWORK_JS_SERVER_IP)
V.add(netwkJs, threaded=True)
ctr.js = netwkJs
else:
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
#this throttle filter will allow one tap back for esc reverse
th_filter = ThrottleFilter()
V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
class LedConditionLogic:
def __init__(self, cfg):
self.cfg = cfg
def run(self, mode, recording, recording_alert, behavior_state,
model_file_changed, track_loc):
#returns a blink rate. 0 for off. -1 for on. positive for rate.
if track_loc is not None:
led.set_rgb(*self.cfg.LOC_COLORS[track_loc])
return -1
if model_file_changed:
led.set_rgb(self.cfg.MODEL_RELOADED_LED_R,
self.cfg.MODEL_RELOADED_LED_G,
self.cfg.MODEL_RELOADED_LED_B)
return 0.1
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if recording_alert:
led.set_rgb(*recording_alert)
return self.cfg.REC_COUNT_ALERT_BLINK_RATE
else:
led.set_rgb(self.cfg.LED_R, self.cfg.LED_G, self.cfg.LED_B)
if behavior_state is not None and model_type == 'behavior':
r, g, b = self.cfg.BEHAVIOR_LED_COLORS[behavior_state]
led.set_rgb(r, g, b)
return -1 #solid on
if recording:
return -1 #solid on
elif mode == 'user':
return 1
elif mode == 'local_angle':
return 0.5
elif mode == 'local':
return 0.1
return 0
if cfg.HAVE_RGB_LED and not cfg.DONKEY_GYM:
from donkeycar.parts.led_status import RGB_LED
led = RGB_LED(cfg.LED_PIN_R, cfg.LED_PIN_G, cfg.LED_PIN_B,
cfg.LED_INVERT)
led.set_rgb(cfg.LED_R, cfg.LED_G, cfg.LED_B)
V.add(LedConditionLogic(cfg),
inputs=[
'user/mode', 'recording', "records/alert", 'behavior/state',
'modelfile/modified', "pilot/loc"
],
outputs=['led/blink_rate'])
V.add(led, inputs=['led/blink_rate'])
def get_record_alert_color(num_records):
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.last_num_rec_print = num_records
if num_records % 10 == 0:
print("recorded", num_records, "records")
if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
V.add(rec_tracker_part,
inputs=["tub/num_records"],
outputs=['records/alert'])
if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
#then we are not using the circle button. hijack that to force a record count indication
def show_record_acount_status():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
#Sombrero
if cfg.HAVE_SOMBRERO:
from donkeycar.parts.sombrero import Sombrero
s = Sombrero()
#IMU
if cfg.HAVE_IMU:
from donkeycar.parts.imu import Mpu6050
imu = Mpu6050()
V.add(imu,
outputs=[
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
],
threaded=True)
class ImgPreProcess():
'''
preprocess camera image for inference.
normalize and crop if needed.
'''
def __init__(self, cfg):
self.cfg = cfg
def run(self, img_arr):
return normalize_and_crop(img_arr, self.cfg)
if "coral" in model_type:
inf_input = 'cam/image_array'
else:
inf_input = 'cam/normalized/cropped'
V.add(ImgPreProcess(cfg),
inputs=['cam/image_array'],
outputs=[inf_input],
run_condition='run_pilot')
#Behavioral state
if cfg.TRAIN_BEHAVIORS:
bh = BehaviorPart(cfg.BEHAVIOR_LIST)
V.add(bh,
outputs=[
'behavior/state', 'behavior/label',
"behavior/one_hot_state_array"
])
try:
ctr.set_button_down_trigger('L1', bh.increment_state)
except:
pass
inputs = [inf_input, "behavior/one_hot_state_array"]
#IMU
elif model_type == "imu":
assert (cfg.HAVE_IMU)
#Run the pilot if the mode is not user.
inputs = [
inf_input, 'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x',
'imu/gyr_y', 'imu/gyr_z'
]
else:
inputs = [inf_input]
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
def load_weights(kl, weights_path):
start = time.time()
try:
print('loading model weights', weights_path)
kl.model.load_weights(weights_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
with open(json_fnm, 'r') as handle:
contents = handle.read()
kl.model = keras.models.model_from_json(contents)
print('finished loading json in %s sec.' %
(str(time.time() - start)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
model_reload_cb = None
if '.h5' in model_path or '.uff' in model_path or 'tflite' in model_path or '.pkl' in model_path:
#when we have a .h5 extension
#load everything from the model file
load_model(kl, model_path)
def reload_model(filename):
load_model(kl, filename)
model_reload_cb = reload_model
elif '.json' in model_path:
#when we have a .json extension
#load the model from there and look for a matching
#.wts file with just weights
load_model_json(kl, model_path)
weights_path = model_path.replace('.json', '.weights')
load_weights(kl, weights_path)
def reload_weights(filename):
weights_path = filename.replace('.json', '.weights')
load_weights(kl, weights_path)
model_reload_cb = reload_weights
else:
print("ERR>> Unknown extension type on model file!!")
return
#this part will signal visual LED, if connected
V.add(FileWatcher(model_path, verbose=True),
outputs=['modelfile/modified'])
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
V.add(FileWatcher(model_path),
outputs=['modelfile/dirty'],
run_condition="ai_running")
V.add(DelayedTrigger(100),
inputs=['modelfile/dirty'],
outputs=['modelfile/reload'],
run_condition="ai_running")
V.add(TriggeredCallback(model_path, model_reload_cb),
inputs=["modelfile/reload"],
run_condition="ai_running")
outputs = ['pilot/angle', 'pilot/throttle']
if cfg.TRAIN_LOCALIZER:
outputs.append("pilot/loc")
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
#to give the car a boost when starting ai mode in a race.
aiLauncher = AiLaunch(cfg.AI_LAUNCH_DURATION, cfg.AI_LAUNCH_THROTTLE,
cfg.AI_LAUNCH_KEEP_ENABLED)
V.add(aiLauncher, inputs=['user/mode', 'throttle'], outputs=['throttle'])
if isinstance(ctr, JoystickController):
ctr.set_button_down_trigger(cfg.AI_LAUNCH_ENABLE_BUTTON,
aiLauncher.enable_ai_launch)
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording
class AiRecordingCondition:
'''
return True when ai mode, otherwize respect user mode recording flag
'''
def run(self, mode, recording):
if mode == 'user':
return recording
return True
if cfg.RECORD_DURING_AI:
V.add(AiRecordingCondition(),
inputs=['user/mode', 'recording'],
outputs=['recording'])
#Drive train setup
if cfg.DONKEY_GYM:
pass
elif cfg.DRIVE_TRAIN_TYPE == "SERVO_ESC":
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
steering_controller = PCA9685(cfg.STEERING_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_STEER_THROTTLE":
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
steering = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT,
cfg.HBRIDGE_PIN_RIGHT)
throttle = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
elif cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM
left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD,
cfg.HBRIDGE_PIN_LEFT_BWD)
right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD,
cfg.HBRIDGE_PIN_RIGHT_BWD)
two_wheel_control = TwoWheelSteeringThrottle()
V.add(two_wheel_control,
inputs=['throttle', 'angle'],
outputs=['left_motor_speed', 'right_motor_speed'])
V.add(left_motor, inputs=['left_motor_speed'])
V.add(right_motor, inputs=['right_motor_speed'])
elif cfg.DRIVE_TRAIN_TYPE == "SKID_STEER":
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, SBC_PiMotor
right_motor = SBC_PiMotor(1)
left_motor = SBC_PiMotor(2)
two_wheel_control = TwoWheelSteeringThrottle()
V.add(two_wheel_control,
inputs=['throttle', 'angle'],
outputs=['left_motor_speed', 'right_motor_speed'])
V.add(left_motor, inputs=['left_motor_speed'])
V.add(right_motor, inputs=['right_motor_speed'])
elif cfg.DRIVE_TRAIN_TYPE == "SERVO_HBRIDGE_PWM":
from donkeycar.parts.actuator import ServoBlaster, PWMSteering
steering_controller = ServoBlaster(cfg.STEERING_CHANNEL) #really pin
#PWM pulse values should be in the range of 100 to 200
assert (cfg.STEERING_LEFT_PWM <= 200)
assert (cfg.STEERING_RIGHT_PWM <= 200)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
from donkeycar.parts.actuator import Mini_HBridge_DC_Motor_PWM
motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_FWD,
cfg.HBRIDGE_PIN_BWD)
V.add(steering, inputs=['angle'])
V.add(motor, inputs=["throttle"])
#add tub to save data
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
if cfg.TRAIN_BEHAVIORS:
inputs += [
'behavior/state', 'behavior/label', "behavior/one_hot_state_array"
]
types += ['int', 'str', 'vector']
if cfg.HAVE_IMU:
inputs += [
'imu/acl_x', 'imu/acl_y', 'imu/acl_z', 'imu/gyr_x', 'imu/gyr_y',
'imu/gyr_z'
]
types += ['float', 'float', 'float', 'float', 'float', 'float']
if cfg.RECORD_DURING_AI:
inputs += ['pilot/angle', 'pilot/throttle']
types += ['float', 'float']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
if cfg.PUB_CAMERA_IMAGES:
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg
pub = TCPServeValue("camera")
V.add(ImgArrToJpg(), inputs=['cam/image_array'], outputs=['jpg/bin'])
V.add(pub, inputs=['jpg/bin'])
if type(ctr) is LocalWebController:
print(
"You can now go to <your pis hostname.local>:8887 to drive your car."
)
elif isinstance(ctr, JoystickController):
print("You can now move your joystick to drive your car.")
#tell the controller about the tub
ctr.set_tub(tub)
if cfg.BUTTON_PRESS_NEW_TUB:
def new_tub_dir():
V.parts.pop()
tub = th.new_tub_writer(inputs=inputs,
types=types,
user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
ctr.set_tub(tub)
ctr.set_button_down_trigger('cross', new_tub_dir)
ctr.print_controls()
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single',
meta=[]):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#zd d
if model_type is None:
if cfg.TRAIN_LOCALIZER:
model_type = "localizer"
elif cfg.TRAIN_BEHAVIORS:
model_type = "behavior"
else:
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
#zd d
#1 ********************************************* camera ********************************************
inputs = []
threaded = True
print("cfg.CAMERA_TYPE", cfg.CAMERA_TYPE)
if cfg.CAMERA_TYPE == "PICAM":
from donkeycar.parts.camera import PiCamera
cam = PiCamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH)
#zd d
else:
raise (Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
#2 ********************************************* controller ********************************************
#This web controller will create a web server that is capable
#of managing steering, throttle, and modes, and more.
ctr = LocalWebController()
if cfg.AUTO_RECORD_ON_THROTTLE and isinstance(ctr, JoystickController):
#then we are not using the circle button. hijack that to force a record count indication
def show_record_acount_status():
rec_tracker_part.last_num_rec_print = 0
rec_tracker_part.force_alert = 1
ctr.set_button_down_trigger('circle', show_record_acount_status)
V.add(
ctr,
inputs=['cam/image_array'],
#outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
outputs=[
'user/angle', 'user/throttle', 'user/mode', 'recording',
'change_model', 'user/arrival'
],
#outputs=['user/angle', 'user/throttle', 'user/mode', 'recording', 'change_model'], #zd
threaded=True)
# ********************************************* communication ********************************************
class communication_class:
def __init__(self):
self.recv = ''
self.arrived = 0
# ser = serial.Serial("/dev/ttyUSB0", 9600)
self.serial = serial.Serial('/dev/ttyUSB0', 9600, timeout=3600)
# serial = serial.Serial('COM9', 9600, timeout=3600)
if self.serial.isOpen():
print("open success")
else:
print("open failed")
data_judge = '0'
while data_judge != b'\r\nOK\r\n':
send_data = 'AT'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+CMEE=1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nQuectel\r\n\r\nOK\r\n':
send_data = 'AT+CGMI'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nBC95HB-02-STD_900\r\n\r\nOK\r\n':
send_data = 'AT+CGMM'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nSECURITY,V100R100C10B657SP3\r\n\r\nPROTOCOL,V100R100C10B657SP3\r\n\r\nAPPLICATION,V100R100C10B657SP3\r\n\r\nSEC_UPDATER,V100R100C10B657SP3\r\n\r\nAPP_UPDATER,V100R100C10B657SP3\r\n\r\nRADIO,BC95HB-02-STD_900\r\n\r\nOK\r\n':
send_data = 'AT+CGMR'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+NBAND:8\r\n\r\nOK\r\n':
send_data = 'AT+NBAND?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+NCONFIG:AUTOCONNECT,TRUE\r\n+NCONFIG:CR_0354_0338_SCRAMBLING,TRUE\r\n+NCONFIG:CR_0859_SI_AVOID,TRUE\r\n+NCONFIG:COMBINE_ATTACH,FALSE\r\n+NCONFIG:CELL_RESELECTION,FALSE\r\n+NCONFIG:ENABLE_BIP,FALSE\r\n\r\nOK\r\n':
send_data = 'AT+NCONFIG?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+CGSN:869405035846048\r\n\r\nOK\r\n':
send_data = 'AT+CGSN=1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+CFUN=1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n460043193006443\r\n\r\nOK\r\n':
send_data = 'AT+CIMI'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+CGATT=0'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+CGATT:0\r\n\r\nOK\r\n':
send_data = 'AT+CGATT?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+CGATT=1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+CGATT:1\r\n\r\nOK\r\n':
send_data = 'AT+CGATT?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = b'\r\n+CSQ:99,99\r\n\r\nOK\r\n'
while data_judge == b'\r\n+CSQ:99,99\r\n\r\nOK\r\n':
send_data = 'AT+CSQ'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+COPS:0,2,"46000"\r\n\r\nOK\r\n':
send_data = 'AT+COPS?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
data_judge = '0'
while data_judge != b'\r\n+CEREG:0,1\r\n\r\nOK\r\n':
send_data = 'AT+CEREG?'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
print('nb-iot initial ok!')
def update(self):
while True:
try:
data_judge = '0'
# sleep(1)
while data_judge != b'\r\n0\r\n\r\nOK\r\n' and data_judge != b'\r\n+CME ERROR: 4\r\n':
send_data = 'AT+NSOCR=DGRAM,17,25535,1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(
self.serial.inWaiting())
print(data_judge)
# line = line.splitlines(True)
legal = 'OK'
data = {'order': 'ask'}
data = json.dumps(data)
l = len(data)
judge = '0'
while judge != legal:
send_data = "AT+NSOST=0,139.199.105.136,25535," + str(
len(data)) + "," + (binascii.b2a_hex(
str(data).encode('utf-8'))).decode()
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
# line = ser.read(ser.inWaiting())
# line = line.decode()
sleep(2)
# line = line.splitlines(True)
data = data + self.serial.read(self.serial.inWaiting())
data = str(data).split("\\r\\n")
print(data)
judge = data[3]
if len(data) > 5:
print("进入判断")
data = data[5]
l = data[10:len(data)]
print(l)
data_judge = '0'
while data_judge != b'\r\n\r\nOK\r\n':
print("发送请求")
send_data = 'AT+NSORF=0,' + l + "\r\n"
print(send_data)
# send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(1)
data = data + self.serial.read(
self.serial.inWaiting())
# print(data.decode('utf-8'))
data_judge = data[len(data) - 8:len(data)]
data = str(data).split(",")
# print((bytearray.fromhex(data[4])).decode('utf-8'))
data = (bytearray.fromhex(data[4])).decode('utf-8')
data = json.loads(data)
self.recv = data
print("receive:")
print(data)
#!!! change car state when arrive
# if has_arrived=='arrive':
# print("!!!!!!car_state change to c(waiting to get)")
# judge = '0'
# data = {'order': 'car_state'}
# data = json.dumps(data)
# l = len(data)
# legal = "\r\n0," + str(len(data)) + "\r\n\r\nOK\r\n"
#
# while judge != legal.encode('utf-8'):
# send_data = "AT+NSOST=0,139.199.105.136,25535," + str(len(data)) + "," + (
# binascii.b2a_hex(str(data).encode('utf-8'))).decode()
# print(send_data)
# send_data = send_data + '\r\n'
# self.serial.write(send_data.encode())
# data = self.serial.read(1)
# sleep(2)
# data = data + self.serial.read(self.serial.inWaiting())
# #print(data)
# judge = data[0:14]
data_judge = '0'
# sleep(1)
while data_judge != b'\r\nOK\r\n':
# line = ser.read(ser.inWaiting())
# line = line.decode()
send_data = 'AT+NSOCL=0'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(
self.serial.inWaiting())
print(data_judge)
except:
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+NSOCL=0'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(
self.serial.inWaiting())
print(data_judge)
def run_threaded(self, has_arrived):
# !!! change car state when arrive
if has_arrived == 'arrive':
if self.arrived == 0:
self.arrived += 1
print("!!!!!!car_state change to c(waiting to get)")
try:
data_judge = '0'
# sleep(1)
while data_judge != b'\r\n0\r\n\r\nOK\r\n' and data_judge != b'\r\n+CME ERROR: 4\r\n':
send_data = 'AT+NSOCR=DGRAM,17,25535,1'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(
self.serial.inWaiting())
print(data_judge)
judge = '0'
data = {'order': 'car_state'}
data = json.dumps(data)
l = len(data)
legal = "\r\n0," + str(len(data)) + "\r\n\r\nOK\r\n"
while judge != legal.encode('utf-8'):
send_data = "AT+NSOST=0,139.199.105.136,25535," + str(
len(data)) + "," + (binascii.b2a_hex(
str(data).encode('utf-8'))).decode()
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
#sleep(2)
sleep(1)
data = data + self.serial.read(
self.serial.inWaiting())
# print(data)
judge = data[0:14]
except:
print("send arrive error!")
else:
self.arrived = 0
tmp = self.recv
if tmp != '':
return tmp['box_state'], tmp['car_state'], tmp['pos']
else:
#!!!!
return ['open', 'open', 'open'], 'a', 'a'
def shuwtdonw(self):
#关闭socket
GPIO.cleanup()
data_judge = '0'
# sleep(1)
print("clean udp socket")
while data_judge != b'\r\nOK\r\n':
send_data = 'AT+NSOCL=0'
print(send_data)
send_data = send_data + '\r\n'
self.serial.write(send_data.encode())
data = self.serial.read(1)
sleep(0.5)
data_judge = data + self.serial.read(self.serial.inWaiting())
print(data_judge)
communication = communication_class()
#!!!
V.add(communication,
inputs=['arrive_signal'],
outputs=['lock_state', 'car_state', 'pos'],
threaded=True)
# ********************************************* lock_ctr ********************************************
class lock_ctr_class:
def __init__(self):
GPIO.setmode(GPIO.BOARD)
GPIO.setup(36, GPIO.OUT) # 1
GPIO.setmode(GPIO.BOARD)
GPIO.setup(38, GPIO.OUT) # 2
GPIO.setmode(GPIO.BOARD)
GPIO.setup(40, GPIO.OUT) # 3
def run(self, state):
#1
if state[0] == 'close':
GPIO.output(36, True)
else:
GPIO.output(36, False)
#2
if state[1] == 'close':
GPIO.output(38, True)
else:
GPIO.output(38, False)
#3
if state[2] == 'close':
GPIO.output(40, True)
else:
GPIO.output(40, False)
# for x in range(3): # 0 1 2
# if state[x]=='open':
# GPIO.output(11+x, False)
# else: # close
# GPIO.output(11+x, True)
def shutdown(self):
GPIO.cleanup()
lock_ctr = lock_ctr_class()
V.add(lock_ctr, inputs=['lock_state'], outputs=[])
# ********************************************* throttle filter ********************************************
#this throttle filter will allow one tap back for esc reverse
#th_filter = ThrottleFilter()
#V.add(th_filter, inputs=['user/throttle'], outputs=['user/throttle'])
#
#3 ********************************************* PilotCondition ********************************************
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
# class PilotCondition:
# def run(self, mode):
# if mode == 'user':
# return False
# else:
# return True
#
# V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
#!!!!
class PilotCondition:
def run(self, car_state):
if car_state == 'a' or car_state == '':
return False, 'user'
else:
return True, 'local_pilot'
V.add(PilotCondition(),
inputs=['car_state'],
outputs=['run_pilot', 'user/mode'])
#zd d
#4 ********************************************* RecordTracker ********************************************
def get_record_alert_color(num_records):
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.last_num_rec_print = num_records
if num_records % 10 == 0:
print("recorded", num_records, "records")
if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
rec_tracker_part = RecordTracker()
V.add(rec_tracker_part,
inputs=["tub/num_records"],
outputs=['records/alert'])
#zd d
#5 ********************************************* model ********************************************
# important
def load_model(kl, model_path):
start = time.time()
try:
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except Exception as e:
print(e)
print('ERR>> problems loading model', model_path)
def load_weights(kl, weights_path):
start = time.time()
try:
print('loading model weights', weights_path)
kl.model.load_weights(weights_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
except Exception as e:
print(e)
print('ERR>> problems loading weights', weights_path)
def load_model_json(kl, json_fnm):
start = time.time()
print('loading model json', json_fnm)
from tensorflow.python import keras
try:
with open(json_fnm, 'r') as handle:
contents = handle.read()
kl.model = keras.models.model_from_json(contents)
print('finished loading json in %s sec.' %
(str(time.time() - start)))
except Exception as e:
print(e)
print("ERR>> problems loading model json", json_fnm)
if model_path:
#When we have a model, first create an appropriate Keras part
# most time
kl = dk.utils.get_model_by_type(model_type, cfg)
# model_type 默认是linear
model_reload_cb = None
if '.h5' in model_path:
#when we have a .h5 extension
#load everything from the model file
# most time
load_model(kl, model_path)
def reload_model(filename):
print("reloading start")
#zd
V.sudden_control(0)
#print('***************', V.mem.items())
#V.mem.put(['angle', 'throttle', 'pilot/angle', 'pilot/throttle', 'user/angle', 'user/throttle'], [0, 0, 0, 0, 0 ,0])
#print('***************', V.mem.items())
load_model(kl, filename)
print(
'***************\r\nreloading is done\r\n**************\r\n'
)
model_reload_cb = reload_model
else:
print("ERR>> Unknown extension type on model file!!")
return
#these parts will reload the model file, but only when ai is running so we don't interrupt user driving
#V.add(DelayedTrigger(100), inputs=['modelfile/dirty'], outputs=['modelfile/reload'], run_condition="ai_running")
#V.add(TriggeredCallback(model_path, model_reload_cb), inputs=["modelfile/reload"], run_condition="ai_running")
#zd
#V.add(TriggeredCallback("./models/mypilot_2.h5", model_reload_cb), inputs=["modelfile/reload","change_model"], run_condition="ai_running")
#V.add(TriggeredCallback("./models/mypilot_2.h5", model_reload_cb), inputs=["modelfile/reload", "change_model"])
inputs = ['cam/image_array']
outputs = ['pilot/angle', 'pilot/throttle', 'pilot/arrival']
V.add(kl, inputs=inputs, outputs=outputs, run_condition='run_pilot')
# ********************************************** turn_delay_trigger ************************************************
class turn_delay_class:
def __init__(self, pause_time):
self.pause = False
self.trigger = 0
self.pause_time = pause_time
self.now_direc = 0
#self.direc_list = ['S'] #p is pause
self.direc_list = ['L']
#self.on = False
def update(self):
while 1:
if self.pause == True:
sleep(2)
self.pause = False
if self.trigger == 1:
#self.on = False
#time.sleep(3)
#self.on = True
time.sleep(self.pause_time)
self.trigger = 0
self.now_direc += 1
def run_threaded(self, arrival_signal, mode, has_arrived, pos,
car_state):
# >0 = arrive <0 = not arrive
if self.pause == True:
return True, 'P', ''
if self.trigger == 1:
return True, self.direc_list[self.now_direc], ''
# new deal
if mode == 'user':
#if car_state=='a': #待放
self.now_direc = 0
self.trigger = 0
self.pause = False
if pos == 'a':
self.direc_list = ['L']
elif pos == 'b':
self.direc_list = ['S']
print('~~~~~~~~~~~~~~~~~~~direction restart')
return False, '', ''
if has_arrived == 'arrive':
return False, '', 'arrive'
if arrival_signal > 0:
# judge arrive
# print("now is {}".format(self.now_direc))
# if self.now_direc >= len(self.direc_list):
# print("arrive--------------------------")
# return False, '', 'arrive'
if self.now_direc >= len(self.direc_list):
print("arrive--------------------------")
return False, '', 'arrive'
self.trigger = 1
self.pause = True
print("**************************start turn!!!, now is {}".
format(self.direc_list[self.now_direc]))
return True, self.direc_list[self.now_direc], ''
else:
return False, '', ''
def shutdown(self):
return
turn_delay = turn_delay_class(1.65)
#turn_delay = turn_delay_class(2)
#V.add(turn_delay, inputs=['pilot/arrival', 'user/mode'], outputs=['turn/mode', 'turn_direction'], run_condition='run_pilot', threaded=True)
V.add(turn_delay,
inputs=[
'pilot/arrival', 'user/mode', 'arrive_signal', 'pos', 'car_state'
],
outputs=['turn/mode', 'turn_direction', 'arrive_signal'],
threaded=True)
# ********************************************** turn_module ************************************************
class turn_module_class:
def __init__(self):
pass
def run(self, direc):
if direc == 'P': #pause
return 0, 0
elif direc == 'L': #left
return -1, 1
elif direc == 'R': #right
return 1, -1
elif direc == 'S': #straight
return 0.4, 0.4
else: #pause
return 0, 0
def shutdown(self):
return
turn_module = turn_module_class()
V.add(turn_module,
inputs=['turn_direction'],
outputs=['turn/angle', 'turn/throttle'],
run_condition='run_pilot')
# pause module input=[] out=[pause/mode]
#6 ********************************************* DriveMode ********************************************
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, turn_mode, user_angle, user_throttle, pilot_angle,
pilot_throttle, turn_angle, turn_throttle, arrive_signal):
if turn_mode == True:
return turn_angle, turn_throttle
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else: # run_pilot
if arrive_signal == 'arrive':
return 0, 0
return 0.3, 0.4
#return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
V.add(DriveMode(),
inputs=[
'user/mode', 'turn/mode', 'user/angle', 'user/throttle',
'pilot/angle', 'pilot/throttle', 'turn/angle', 'turn/throttle',
'arrive_signal'
],
outputs=['angle', 'throttle'])
#zd d
#to give the car a boost when starting ai mode in a race.
#7 ********************************************* AiRunCondition ********************************************
class AiRunCondition:
'''
A bool part to let us know when ai is running.
'''
def run(self, mode):
if mode == "user":
return False
return True
V.add(AiRunCondition(), inputs=['user/mode'], outputs=['ai_running'])
#Ai Recording zd d
#8 ********************************************* Motor ********************************************
#Drive train setup
# move power
if cfg.DRIVE_TRAIN_TYPE == "DC_TWO_WHEEL":
print('DC start!')
from donkeycar.parts.actuator import TwoWheelSteeringThrottle, Mini_HBridge_DC_Motor_PWM
LEFT_PID_DIR1 = 11
LEFT_PID_DIR2 = 13
LEFT_PWM = 15
RIGHT_PID_DIR1 = 12
RIGHT_PID_DIR2 = 16
RIGHT_PWM = 18
# left_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_LEFT_FWD, cfg.HBRIDGE_PIN_LEFT_BWD)
# right_motor = Mini_HBridge_DC_Motor_PWM(cfg.HBRIDGE_PIN_RIGHT_FWD, cfg.HBRIDGE_PIN_RIGHT_BWD)
left_motor = Mini_HBridge_DC_Motor_PWM(LEFT_PID_DIR1, LEFT_PID_DIR2,
LEFT_PWM)
right_motor = Mini_HBridge_DC_Motor_PWM(RIGHT_PID_DIR1, RIGHT_PID_DIR2,
RIGHT_PWM)
two_wheel_control = TwoWheelSteeringThrottle()
V.add(two_wheel_control,
inputs=['throttle', 'angle'],
outputs=['left_motor_speed', 'right_motor_speed'])
V.add(left_motor, inputs=['left_motor_speed'])
V.add(right_motor, inputs=['right_motor_speed'])
#9 ********************************************* tub ********************************************
#add tub to save data
'''
inputs=['cam/image_array',
'user/angle', 'user/throttle',
'user/mode']
types=['image_array',
'float', 'float',
'str']
'''
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/arrival',
'user/mode'
]
types = ['image_array', 'float', 'float', 'float', 'str']
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
# zd d
if type(ctr) is LocalWebController:
print("You can now go to <your pi ip address>:8887 to drive your car.")
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, use_joystick=False, use_chaos=False):
"""
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
"""
V = dk.vehicle.Vehicle()
clock = Timestamp()
V.add(clock, outputs='timestamp')
cam = PiCamera(resolution=cfg.CAMERA_RESOLUTION)
V.add(cam, outputs=['cam/image_array'], threaded=True)
if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
ctr = JoystickController(
max_throttle=cfg.JOYSTICK_MAX_THROTTLE,
steering_scale=cfg.JOYSTICK_STEERING_SCALE,
auto_record_on_throttle=cfg.AUTO_RECORD_ON_THROTTLE)
else:
# This web controller will create a web server that is capable
# of managing steering, throttle, and modes, and more.
ctr = LocalWebController(use_chaos=use_chaos)
V.add(ctr,
inputs=['cam/image_array'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# See if we should even run the pilot module.
# This is only needed because the part run_condition only accepts boolean
def pilot_condition(mode):
if mode == 'user':
return False
else:
return True
pilot_condition_part = Lambda(pilot_condition)
V.add(pilot_condition_part, inputs=['user/mode'], outputs=['run_pilot'])
# Run the pilot if the mode is not user.
kl = KerasCategorical()
if model_path:
kl.load(model_path)
V.add(kl,
inputs=['cam/image_array'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition='run_pilot')
# Choose what inputs should change the car.
def drive_mode(mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
drive_mode_part = Lambda(drive_mode)
V.add(drive_mode_part,
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
steering_controller = PCA9685(cfg.STEERING_CHANNEL)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# add tub to save data
inputs = [
'cam/image_array', 'user/angle', 'user/throttle', 'user/mode',
'timestamp'
]
types = ['image_array', 'float', 'float', 'str', 'str']
#multiple tubs
#th = TubHandler(path=cfg.DATA_PATH)
#tub = th.new_tub_writer(inputs=inputs, types=types)
# single tub
tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
V.add(tub, inputs=inputs, run_condition='recording')
#setup AWS IoT
if cfg.IOT_ENABLED:
aws = AWSHandler(vehicle_id=cfg.VEHICLE_ID,
endpoint=cfg.AWS_ENDPOINT,
ca=cfg.CA_PATH,
private_key=cfg.PRIVATE_KEY_PATH,
certificate=cfg.CERTIFICATE_PATH)
iot = aws.new_iot_publisher(intputs=inputs, types=types)
V.add(iot, inputs=inputs, run_condition='recording')
#run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg,
model_path=None,
use_joystick=False,
model_type=None,
camera_type='single',
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, 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)
