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