def drive(cfg):
'''
Construct a working robotic vehicle from many parts.
Each part runs as a job in the Vehicle loop, calling either
it's run or run_threaded method depending on the constructor flag `threaded`.
All parts are updated one after another at the framerate given in
cfg.DRIVE_LOOP_HZ assuming each part finishes processing in a timely manner.
Parts may have named outputs and inputs. The framework handles passing named outputs
to parts requesting the same named input.
'''
#Initialize car
V = dk.vehicle.Vehicle()
if cfg.HAVE_SOMBRERO:
from donkeycar.utils import Sombrero
s = Sombrero()
ctr = get_js_controller(cfg)
V.add(ctr,
inputs=['null'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
if cfg.HAVE_ODOM:
pi = pigpio.pi()
enc = PiPGIOEncoder(cfg.ODOM_PIN, pi)
V.add(enc, outputs=['enc/ticks'])
odom = OdomDist(mm_per_tick=cfg.MM_PER_TICK, debug=cfg.ODOM_DEBUG)
V.add(odom,
inputs=['enc/ticks', 'user/throttle'],
outputs=['enc/dist_m', 'enc/vel_m_s', 'enc/delta_vel_m_s'])
if not os.path.exists(cfg.WHEEL_ODOM_CALIB):
print(
"You must supply a json file when using odom with T265. There is a sample file in templates."
)
print(
"cp donkeycar/donkeycar/templates/calibration_odometry.json .")
exit(1)
else:
# we give the T265 no calib to indicated we don't have odom
cfg.WHEEL_ODOM_CALIB = None
#This dummy part to satisfy input needs of RS_T265 part.
class NoOdom():
def run(self):
return 0.0
V.add(NoOdom(), outputs=['enc/vel_m_s'])
# This requires use of the Intel Realsense T265
rs = RS_T265(image_output=False, calib_filename=cfg.WHEEL_ODOM_CALIB)
V.add(rs,
inputs=['enc/vel_m_s'],
outputs=['rs/pos', 'rs/vel', 'rs/acc', 'rs/camera/left/img_array'],
threaded=True)
# Pull out the realsense T265 position stream, output 2d coordinates we can use to map.
class PosStream:
def run(self, pos):
#y is up, x is right, z is backwards/forwards
return pos.x, pos.z
V.add(PosStream(), inputs=['rs/pos'], outputs=['pos/x', 'pos/y'])
# This part will reset the car back to the origin. You must put the car in the known origin
# and push the cfg.RESET_ORIGIN_BTN on your controller. This will allow you to induce an offset
# in the mapping.
origin_reset = OriginOffset()
V.add(origin_reset, inputs=['pos/x', 'pos/y'], outputs=['pos/x', 'pos/y'])
class UserCondition:
def run(self, mode):
if mode == 'user':
return True
else:
return False
V.add(UserCondition(), inputs=['user/mode'], outputs=['run_user'])
#See if we should even run the pilot module.
#This is only needed because the part run_condition only accepts boolean
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
# This is the path object. It will record a path when distance changes and it travels
# at least cfg.PATH_MIN_DIST meters. Except when we are in follow mode, see below...
path = Path(min_dist=cfg.PATH_MIN_DIST)
V.add(path,
inputs=['pos/x', 'pos/y'],
outputs=['path'],
run_condition='run_user')
# When a path is loaded, we will be in follow mode. We will not record.
path_loaded = False
if os.path.exists(cfg.PATH_FILENAME):
path.load(cfg.PATH_FILENAME)
path_loaded = True
def save_path():
path.save(cfg.PATH_FILENAME)
print("saved path:", cfg.PATH_FILENAME)
def erase_path():
global mode, path_loaded
if os.path.exists(cfg.PATH_FILENAME):
os.remove(cfg.PATH_FILENAME)
mode = 'user'
path_loaded = False
print("erased path", cfg.PATH_FILENAME)
else:
print("no path found to erase")
def reset_origin():
print("Resetting origin")
origin_reset.init_to_last
# Here's a trigger to save the path. Complete one circuit of your course, when you
# have exactly looped, or just shy of the loop, then save the path and shutdown
# this process. Restart and the path will be loaded.
ctr.set_button_down_trigger(cfg.SAVE_PATH_BTN, save_path)
# Here's a trigger to erase a previously saved path.
ctr.set_button_down_trigger(cfg.ERASE_PATH_BTN, erase_path)
# Here's a trigger to reset the origin.
ctr.set_button_down_trigger(cfg.RESET_ORIGIN_BTN, reset_origin)
# Here's an image we can map to.
img = PImage(clear_each_frame=True)
V.add(img, outputs=['map/image'])
# This PathPlot will draw path on the image
plot = PathPlot(scale=cfg.PATH_SCALE, offset=cfg.PATH_OFFSET)
V.add(plot, inputs=['map/image', 'path'], outputs=['map/image'])
# This will use path and current position to output cross track error
cte = CTE()
V.add(cte,
inputs=['path', 'pos/x', 'pos/y'],
outputs=['cte/error'],
run_condition='run_pilot')
# This will use the cross track error and PID constants to try to steer back towards the path.
pid = PIDController(p=cfg.PID_P, i=cfg.PID_I, d=cfg.PID_D)
pilot = PID_Pilot(pid, cfg.PID_THROTTLE)
V.add(pilot,
inputs=['cte/error'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition="run_pilot")
def dec_pid_d():
pid.Kd -= 0.5
logging.info("pid: d- %f" % pid.Kd)
def inc_pid_d():
pid.Kd += 0.5
logging.info("pid: d+ %f" % pid.Kd)
# Buttons to tune PID constants
ctr.set_button_down_trigger("L2", dec_pid_d)
ctr.set_button_down_trigger("R2", inc_pid_d)
# Plot a circle on the map where the car is located
carcolor = 'green'
loc_plot = PlotCircle(scale=cfg.PATH_SCALE,
offset=cfg.PATH_OFFSET,
color=carcolor)
V.add(loc_plot,
inputs=['map/image', 'pos/x', 'pos/y'],
outputs=['map/image'])
#This web controller will create a web server. We aren't using any controls, just for visualization.
web_ctr = WebFpv()
V.add(web_ctr, inputs=['map/image'], threaded=True)
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
#print(user_angle, user_throttle)
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
if not cfg.DONKEY_GYM:
steering_controller = PCA9685(cfg.STEERING_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# Print Joystick controls
ctr.print_controls()
if path_loaded:
print(
"###############################################################################"
)
print("Loaded path:", cfg.PATH_FILENAME)
print("Make sure your car is sitting at the origin of the path.")
print("View web page and refresh. You should see your path.")
print("Hit 'select' twice to change to ai drive mode.")
print(
"You can press the X button (e-stop) to stop the car at any time.")
print("Delete file", cfg.PATH_FILENAME, "and re-start")
print("to record a new path.")
print(
"###############################################################################"
)
carcolor = "blue"
loc_plot = PlotCircle(scale=cfg.PATH_SCALE,
offset=cfg.PATH_OFFSET,
color=carcolor)
V.add(loc_plot,
inputs=['map/image', 'pos/x', 'pos/y'],
outputs=['map/image'])
else:
print(
"###############################################################################"
)
print("You are now in record mode. Open the web page to your car")
print("and as you drive you should see a path.")
print("Complete one circuit of your course.")
print("When you have exactly looped, or just shy of the ")
print("loop, then save the path (press %s)." % cfg.SAVE_PATH_BTN)
print("You can also erase a path with the Triangle button.")
print("When you're done, close this process with Ctrl+C.")
print("Place car exactly at the start. ")
print("Then restart the car with 'python manage drive'.")
print("It will reload the path and you will be ready to ")
print("follow the path using 'select' to change to ai drive mode.")
print("You can also press the Square button to reset the origin")
print(
"###############################################################################"
)
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def drive(cfg, model_path=None, meta=[]):
model_type = cfg.DEFAULT_MODEL_TYPE
#Initialize car
V = dk.vehicle.Vehicle()
if cfg.RS_PATH_PLANNER:
from donkeycar.parts.realsense2 import RS_T265
print("RS t265 with path tracking")
odometry = RS_T265(path_output=True,
stereo_output=False,
image_output=False)
V.add(odometry, outputs=['rs/trans', 'rs/yaw'], threaded=True)
V.add(PosStream(),
inputs=['rs/trans', 'rs/yaw'],
outputs=['pos/x', 'pos/y', 'pos/yaw'])
else:
raise Exception("This script is for RS_PATH_PLANNER only")
from donkeycar.parts.realsense2 import RS_D435i
cam = RS_D435i(img_type=cfg.D435_IMG_TYPE,
image_w=cfg.D435_IMAGE_W,
image_h=cfg.D435_IMAGE_H,
frame_rate=cfg.D435_FRAME_RATE)
V.add(cam, inputs=[], outputs=['cam/image_array'], threaded=True)
# class Distance:
# def run(self, img):
# h,w = img.shape
# arr = img[50:w-50,0:h-60].flatten()
# mean = np.mean(arr)
# print(mean)
# V.add(Distance(),
# inputs=['cam/image_array'], outputs=['null'])
ctr = get_js_controller(cfg)
V.add(ctr,
inputs=['null'],
outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
threaded=True)
# class debug:
# def run(self, angle):
# print(angle)
# V.add(debug(), inputs=['user/angle'], outputs=['null'])
#This web controller will create a web server
web_ctr = LocalWebControllerPlanner()
V.add(
web_ctr,
inputs=[
'map/image', 'cam/image_array', 'user/mode', 'recording',
'throttle', 'angle'
],
outputs=['web/angle', 'web/throttle', 'web/mode',
'web/recording'], # no part consumes this
threaded=True)
class UserCondition:
def run(self, mode):
if mode == 'user':
return True
else:
return False
V.add(UserCondition(), inputs=['user/mode'], outputs=['run_user'])
class PilotCondition:
def run(self, mode):
if mode == 'user':
return False
else:
return True
V.add(PilotCondition(), inputs=['user/mode'], outputs=['run_pilot'])
myRoute = Route(min_dist=cfg.PATH_MIN_DIST)
V.add(myRoute,
inputs=['pos/x', 'pos/y'],
outputs=['nav/path', 'nav/waypoints'],
threaded=True)
if os.path.exists(cfg.RS_ROUTE_FILE):
myRoute.load(cfg.RS_ROUTE_FILE)
print("loaded route:", cfg.RS_ROUTE_FILE)
def save_waypt():
myRoute.save_waypoint()
ctr.set_button_down_trigger(cfg.SAVE_WPT_BTN, save_waypt)
def save_route():
myRoute.save_route(cfg.RS_ROUTE_FILE)
ctr.set_button_down_trigger(cfg.SAVE_ROUTE_BTN, save_route)
def clear_route():
myRoute.clear()
ctr.set_button_down_trigger(cfg.CLEAR_ROUTE_BTN, clear_route)
img = PImage(resolution=(cfg.D435_IMAGE_W, cfg.D435_IMAGE_H),
clear_each_frame=True)
V.add(img, outputs=['map/image'])
plot = WaypointPlot(scale=cfg.PATH_SCALE,
offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2),
color=(0, 0, 255))
V.add(plot,
inputs=['map/image', 'nav/waypoints'],
outputs=['map/image'],
threaded=True)
plot = PathPlot(scale=cfg.PATH_SCALE,
offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2),
color=(0, 0, 255))
V.add(plot,
inputs=['map/image', 'nav/path'],
outputs=['map/image'],
threaded=True)
# this is error function for PID control
nav = Navigator(wpt_reach_tolerance=cfg.WPT_TOLERANCE)
V.add(nav,
inputs=['nav/waypoints', 'pos/x', 'pos/y', 'pos/yaw'],
outputs=['nav/shouldRun', 'nav/error'],
threaded=True,
run_condition="run_pilot")
ctr.set_button_down_trigger("left_shoulder", nav.decrease_target)
ctr.set_button_down_trigger("right_shoulder", nav.increase_target)
pid = PIDController(p=cfg.PID_P, i=cfg.PID_I, d=cfg.PID_D, debug=False)
pilot = PID_Pilot(pid, cfg.PID_THROTTLE)
V.add(pilot,
inputs=['nav/shouldRun', 'nav/error'],
outputs=['pilot/angle', 'pilot/throttle'],
run_condition="run_pilot")
pos_plot = PlotPose(scale=cfg.PATH_SCALE,
offset=(cfg.D435_IMAGE_W // 4, cfg.D435_IMAGE_H // 2))
V.add(pos_plot,
inputs=['map/image', 'pos/x', 'pos/y', 'pos/yaw'],
outputs=['map/image'])
if model_path:
def load_model(kl, model_path):
start = time.time()
print('loading model', model_path)
kl.load(model_path)
print('finished loading in %s sec.' % (str(time.time() - start)))
#When we have a model, first create an appropriate Keras part
kl = dk.utils.get_model_by_type(model_type, cfg)
load_model(kl, model_path)
outputs = ['pilot/angle', 'pilot/throttle']
V.add(kl,
inputs=['cam/image_array'],
outputs=outputs,
run_condition='run_pilot')
#Choose what inputs should change the car.
class DriveMode:
def run(self, mode, user_angle, user_throttle, pilot_angle,
pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
return pilot_angle, user_throttle
else:
return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
V.add(DriveMode(),
inputs=[
'user/mode', 'user/angle', 'user/throttle', 'pilot/angle',
'pilot/throttle'
],
outputs=['angle', 'throttle'])
from donkeycar.parts.actuator import PCA9685, PWMSteering, PWMThrottle
steering_controller = PCA9685(cfg.STEERING_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
steering = PWMSteering(controller=steering_controller,
left_pulse=cfg.STEERING_LEFT_PWM,
right_pulse=cfg.STEERING_RIGHT_PWM)
throttle_controller = PCA9685(cfg.THROTTLE_CHANNEL,
cfg.PCA9685_I2C_ADDR,
busnum=cfg.PCA9685_I2C_BUSNUM)
throttle = PWMThrottle(controller=throttle_controller,
max_pulse=cfg.THROTTLE_FORWARD_PWM,
zero_pulse=cfg.THROTTLE_STOPPED_PWM,
min_pulse=cfg.THROTTLE_REVERSE_PWM)
V.add(steering, inputs=['angle'])
V.add(throttle, inputs=['throttle'])
# data collection
inputs = ['cam/image_array', 'user/angle', 'user/throttle', 'user/mode']
types = ['image_array', 'float', 'float', 'str']
def get_record_alert_color(num_records):
col = (0, 0, 0)
for count, color in cfg.RECORD_ALERT_COLOR_ARR:
if num_records >= count:
col = color
return col
class RecordTracker:
def __init__(self):
self.last_num_rec_print = 0
self.dur_alert = 0
self.force_alert = 0
def run(self, num_records):
if num_records is None:
return 0
if self.last_num_rec_print != num_records or self.force_alert:
self.last_num_rec_print = num_records
if num_records % 10 == 0:
print("recorded", num_records, "records")
if num_records % cfg.REC_COUNT_ALERT == 0 or self.force_alert:
self.dur_alert = num_records // cfg.REC_COUNT_ALERT * cfg.REC_COUNT_ALERT_CYC
self.force_alert = 0
if self.dur_alert > 0:
self.dur_alert -= 1
if self.dur_alert != 0:
return get_record_alert_color(num_records)
return 0
V.add(RecordTracker(),
inputs=["tub/num_records"],
outputs=['records/alert'])
th = TubHandler(path=cfg.DATA_PATH)
tub = th.new_tub_writer(inputs=inputs, types=types, user_meta=meta)
V.add(tub,
inputs=inputs,
outputs=["tub/num_records"],
run_condition='recording')
#tell the controller about the tub
ctr.set_tub(tub)
ctr.print_controls()
#run the vehicle
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)