def test_loop():
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
CI, CP = get_car(logcan)
state = 0
states = [
"'seatbeltNotLatched' in CS.errors", "CS.gasPressed",
"CS.brakePressed", "CS.steeringPressed", "bpressed(CS, 'leftBlinker')",
"bpressed(CS, 'rightBlinker')", "bpressed(CS, 'cancel')",
"bpressed(CS, 'accelCruise')", "bpressed(CS, 'decelCruise')",
"bpressed(CS, 'altButton1')", "'doorOpen' in CS.errors", "False"
]
while 1:
CC = car.CarControl.new_message()
# read CAN
CS = CI.update(CC)
while eval(states[state]) == True:
state += 1
print "IN STATE %d: waiting for %s" % (state, states[state])
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# pub
self.live100 = messaging.pub_sock(context, service_list['live100'].port)
self.carstate = messaging.pub_sock(context, service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = bool(params.get("IsRearViewMirror"))
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
context = zmq.Context()
params = Params()
# pub
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
passive = params.get("Passive") != "0"
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
if CI is None:
if passive:
return
else:
raise Exception("unsupported car")
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
fcw_enabled = params.get("IsFcwEnabled") == "1"
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
if not passive:
AM.add("startup", False)
# write CarParams
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
soft_disable_timer = 0
v_cruise_kph = 255
overtemp = False
free_space = False
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 1.
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 1.5 # Default model bias
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
prof = Profiler()
while 1:
prof.reset() # avoid memory leak
# sample data and compute car events
CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status,
overtemp, free_space)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status)
prof.checkpoint("Plan")
if not passive:
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
if __name__ == "__main__":
# ***** connect to joystick *****
# we use a Mad Catz V.1
dev = InputDevice("/dev/input/event8")
print dev
button_values = [0] * 7
axis_values = [0.0, 0.0, 0.0]
# ***** connect to car *****
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
CI, CP = get_car(logcan, sendcan)
rk = Ratekeeper(100)
while 1:
# **** handle joystick ****
r, w, x = select([dev], [], [], 0.0)
if dev in r:
for event in dev.read():
# button event
if event.type == 1:
btn = event.code - 288
if btn >= 0 and btn < 7:
button_values[btn] = int(event.value)
# axis move event
