def manager_thread(spinner=None): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) ignore = [] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") # start offroad if EON and "QT" not in os.environ: pm_apply_packages('enable') start_offroad() ensure_running(managed_processes.values(), started=False, not_run=ignore) if spinner: # close spinner when ui has started spinner.close() started_prev = False params = Params() sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get("IsDriverViewEnabled") == b"1" ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc ] cloudlog.debug(' '.join(running_list)) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [ p.get_process_state_msg() for p in managed_processes.values() ] pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, can_error_counter): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0) ) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float( max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override( CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = sm['plan'].hasLead right_lane_visible = sm['pathPlan'].rProb > 0.5 left_lane_visible = sm['pathPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) recent_blinker = ( sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown calibrated = sm['liveCalibration'].calStatus == Calibration.CALIBRATED ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive( state) and calibrated md = sm['model'] if len(md.meta.desirePrediction): l_lane_change_prob = md.meta.desirePrediction[ log.PathPlan.Desire.laneChangeLeft - 1] r_lane_change_prob = md.meta.desirePrediction[ log.PathPlan.Desire.laneChangeRight - 1] l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) if ldw_allowed: CC.hudControl.leftLaneDepart = bool( l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool( r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: AM.add(sm.frame, 'ldwPermanent', False) events.append(create_event('ldw', [ET.PERMANENT])) AM.process_alerts(sm.frame) CC.hudControl.visualAlert = AM.visual_alert if not read_only: # send car controls over can can_sends = CI.apply(CC) pm.send( 'sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = sm['dMonitoringState'].awarenessStatus < 0. # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": AM.audible_alert, "driverMonitoringOn": sm['dMonitoringState'].faceDetected, "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'], "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature( (CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(sm['plan'].jerkFactor), "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": sm['plan'].vCurvature, "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, "forceDecel": bool(force_decel), "canErrorCounter": can_error_counter, } if CP.lateralTuning.which() == 'pid': dat.controlsState.lateralControlState.pidState = lac_log elif CP.lateralTuning.which() == 'lqr': dat.controlsState.lateralControlState.lqrState = lac_log elif CP.lateralTuning.which() == 'indi': dat.controlsState.lateralControlState.indiState = lac_log pm.send('controlsState', dat) # carState cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events pm.send('carState', cs_send) # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): ce_send = messaging.new_message('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) return CC, events_bytes
def publish_logs(self, CS, start_time, actuators, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = self.enabled CC.active = self.active CC.actuators = actuators if len(self.sm['liveLocationKalman'].orientationNED.value) > 2: CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0] CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1] CC.cruiseControl.cancel = CS.cruiseState.enabled and ( not self.enabled or not self.CP.pcmCruise) if self.joystick_mode and self.sm.rcv_frame[ 'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: CC.cruiseControl.cancel = True CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = self.enabled CC.hudControl.lanesVisible = self.enabled CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead CC.hudControl.rightLaneVisible = True CC.hudControl.leftLaneVisible = True recent_blinker = (self.sm.frame - self.last_blinker_frame ) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED meta = self.sm['modelV2'].meta if len(meta.desirePrediction) and ldw_allowed: right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] l_lane_close = left_lane_visible and ( self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET)) r_lane_close = right_lane_visible and ( self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET)) CC.hudControl.leftLaneDepart = bool( l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool( r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: self.events.add(EventName.ldw) clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None alerts = self.events.create_alerts( self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer]) self.AM.add_many(self.sm.frame, alerts) self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert if not self.read_only and self.initialized: # send car controls over can can_sends = self.CI.apply(CC) self.pm.send( 'sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) # Curvature & Steering angle params = self.sm['liveParameters'] steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo) # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid controlsState = dat.controlsState controlsState.alertText1 = self.AM.alert_text_1 controlsState.alertText2 = self.AM.alert_text_2 controlsState.alertSize = self.AM.alert_size controlsState.alertStatus = self.AM.alert_status controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertType = self.AM.alert_type controlsState.alertSound = self.AM.audible_alert controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[ 'longitudinalPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled controlsState.active = self.active controlsState.curvature = curvature controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter if self.joystick_mode: controlsState.lateralControlState.debugState = lac_log elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log elif self.CP.lateralTuning.which() == 'pid': controlsState.lateralControlState.pidState = lac_log elif self.CP.lateralTuning.which() == 'lqr': controlsState.lateralControlState.lqrState = lac_log elif self.CP.lateralTuning.which() == 'indi': controlsState.lateralControlState.indiState = lac_log self.pm.send('controlsState', dat) # carState car_events = self.events.to_msg() cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = car_events self.pm.send('carState', cs_send) # carEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('carEvents', len(self.events)) ce_send.carEvents = car_events self.pm.send('carEvents', ce_send) self.events_prev = self.events.names.copy() # carParams - logged every 50 seconds (> 1 per segment) if (self.sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message('carParams') cp_send.carParams = self.CP self.pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) # copy CarControl to pass to CarInterface on the next iteration self.CC = CC
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model=True): gen_signals, gen_checks = get_can_signals(CP) sgs = [s[0] for s in gen_signals] msgs = [s[1] for s in gen_signals] cks_msgs = set(check[0] for check in gen_checks) cks_msgs.add(0x18F) cks_msgs.add(0x30C) # ******** get messages sent to the car ******** can_strings = messaging.drain_sock_raw(Plant.sendcan, wait_for_one=self.response_seen) # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd if can_strings: self.response_seen = True self.cp.update_strings(can_strings, sendcan=True) # ******** get controlsState messages for plotting *** controls_state_msgs = [] for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen): controls_state_msgs.append(a.controlsState) fcw = None for a in messaging.drain_sock(Plant.plan): if a.longitudinalPlan.fcw: fcw = True if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248 else: brake = 0.0 if self.cp.vl[0x200]['GAS_COMMAND'] > 0: gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0 else: gas = 0.0 if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']: steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00 else: steer_torque = 0.0 distance_lead = self.distance_lead_prev + v_lead * self.ts # ******** run the car ******** speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake) distance = self.distance_prev + speed * self.ts speed = self.speed_prev + self.ts * acceleration if speed <= 0: speed = 0 acceleration = 0 # ******** lateral ******** self.angle_steer -= (steer_torque / 10.0) * self.ts # *** radar model *** if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) v_rel = v_lead - speed else: d_rel = 200. v_rel = 0. lateral_pos_rel = 0. # print at 5hz if (self.frame % (self.rate // 5)) == 0: print( "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)) # ******** publish the car ******** vls_tuple = namedtuple('vls', [ 'XMISSION_SPEED', 'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR', 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR', 'LEFT_BLINKER', 'RIGHT_BLINKER', 'GEAR', 'WHEELS_MOVING', 'BRAKE_ERROR_1', 'BRAKE_ERROR_2', 'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED', 'BRAKE_PRESSED', 'BRAKE_SWITCH', 'CRUISE_BUTTONS', 'ESP_DISABLED', 'HUD_LEAD', 'USER_BRAKE', 'STEER_STATUS', 'GEAR_SHIFTER', 'PEDAL_GAS', 'CRUISE_SETTING', 'ACC_STATUS', 'CRUISE_SPEED_PCM', 'CRUISE_SPEED_OFFSET', 'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR', 'CAR_GAS', 'MAIN_ON', 'EPB_STATE', 'BRAKE_HOLD_ACTIVE', 'INTERCEPTOR_GAS', 'INTERCEPTOR_GAS2', 'IMPERIAL_UNIT', 'MOTOR_TORQUE', ]) vls = vls_tuple( self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, self.angle_steer_rate, 0, 0, # Steer torque sensor 0, 0, # Blinkers self.gear_choice, speed != 0, self.brake_error, self.brake_error, not self.seatbelt, self.seatbelt, # Seatbelt self.brake_pressed, 0., # Brake pressed, Brake switch cruise_buttons, self.esp_disabled, 0, # HUD lead self.user_brake, self.steer_error, self.gear_shifter, self.pedal_gas, self.cruise_setting, self.acc_status, self.v_cruise, 0, # Cruise speed offset 0, 0, 0, 0, # Doors self.user_gas, self.main_on, 0, # EPB State 0, # Brake hold 0, # Interceptor feedback 0, # Interceptor 2 feedback False, 0, ) # TODO: publish each message at proper frequency can_msgs = [] for msg in set(msgs): msg_struct = {} indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]] for i in indxs: msg_struct[sgs[i]] = getattr(vls, sgs[i]) if "COUNTER" in honda.get_signals(msg): msg_struct["COUNTER"] = self.frame % 4 if "COUNTER_PEDAL" in honda.get_signals(msg): msg_struct["COUNTER_PEDAL"] = self.frame % 0xf msg = honda.lookup_msg_id(msg) msg_data = honda.encode(msg, msg_struct) if "CHECKSUM" in honda.get_signals(msg): msg_data = fix(msg_data, msg) if "CHECKSUM_PEDAL" in honda.get_signals(msg): msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1]) msg_data = honda.encode(msg, msg_struct) can_msgs.append([msg, 0, msg_data, 0]) # add the radar message # TODO: use the DBC if self.frame % 5 == 0: radar_state_msg = b'\x79\x00\x00\x00\x00\x00\x00\x00' radar_msg = to_3_byte(d_rel * 16.0) + \ to_3_byte(int(lateral_pos_rel * 16.0) & 0x3ff) + \ to_3s_byte(int(v_rel * 32.0)) + \ b"0f00000" radar_msg = binascii.unhexlify(radar_msg) can_msgs.append([0x400, 0, radar_state_msg, 1]) can_msgs.append([0x445, 0, radar_msg, 1]) # add camera msg so controlsd thinks it's alive msg_struct["COUNTER"] = self.frame % 4 msg = honda.lookup_msg_id(0xe4) msg_data = honda.encode(msg, msg_struct) msg_data = fix(msg_data, 0xe4) can_msgs.append([0xe4, 0, msg_data, 2]) # Fake sockets that controlsd subscribes to live_parameters = messaging.new_message('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True live_parameters.liveParameters.posenetValid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) dmon_state = messaging.new_message('driverMonitoringState') dmon_state.driverMonitoringState.isDistracted = False Plant.driverMonitoringState.send(dmon_state.to_bytes()) pandaState = messaging.new_message('pandaState') pandaState.pandaState.safetyModel = car.CarParams.SafetyModel.hondaNidec pandaState.pandaState.controlsAllowed = True Plant.pandaState.send(pandaState.to_bytes()) deviceState = messaging.new_message('deviceState') deviceState.deviceState.freeSpacePercent = 100 deviceState.deviceState.batteryPercent = 100 Plant.deviceState.send(deviceState.to_bytes()) live_location_kalman = messaging.new_message('liveLocationKalman') live_location_kalman.liveLocationKalman.inputsOK = True live_location_kalman.liveLocationKalman.gpsOK = True Plant.live_location_kalman.send(live_location_kalman.to_bytes()) # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step if publish_model and self.frame % 5 == 0: md = messaging.new_message('modelV2') cal = messaging.new_message('liveCalibration') md.modelV2.frameId = 0 if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) v_rel = v_lead - speed prob = 1.0 else: d_rel = 200. v_rel = 0. prob = 0.0 lead = log.ModelDataV2.LeadDataV2.new_message() lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0] lead.xyvaStd = [1.0, 1.0, 1.0, 1.0] lead.prob = prob md.modelV2.leads = [lead, lead] cal.liveCalibration.calStatus = 1 cal.liveCalibration.calPerc = 100 cal.liveCalibration.rpyCalib = [0.] * 3 # fake values? Plant.model.send(md.to_bytes()) Plant.cal.send(cal.to_bytes()) for s in Plant.pm.sock.keys(): try: Plant.pm.send(s, messaging.new_message(s)) except Exception: Plant.pm.send(s, messaging.new_message(s, 1)) Plant.logcan.send(can_list_to_can_capnp(can_msgs)) # ******** update prevs ******** self.frame += 1 if self.response_seen: self.rk.monitor_time() self.speed = speed self.distance = distance self.distance_lead = distance_lead self.speed_prev = speed self.distance_prev = distance self.distance_lead_prev = distance_lead else: # Don't advance time when controlsd is not yet ready self.rk.keep_time() self.rk._frame = 0 return { "distance": distance, "speed": speed, "acceleration": acceleration, "distance_lead": distance_lead, "brake": brake, "gas": gas, "steer_torque": steer_torque, "fcw": fcw, "controls_state_msgs": controls_state_msgs, }
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo stand_still = sm['carState'].standStill angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset anglesteer_current = sm['controlsState'].angleSteers anglesteer_desire = sm['controlsState'].angleSteersDes v_cruise_kph = sm['controlsState'].vCruise lateral_control_method = sm['controlsState'].lateralControlMethod if lateral_control_method == 0: output_scale = sm[ 'controlsState'].lateralControlState.pidState.output elif lateral_control_method == 1: output_scale = sm[ 'controlsState'].lateralControlState.indiState.output elif lateral_control_method == 2: output_scale = sm[ 'controlsState'].lateralControlState.lqrState.output # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc self.new_steer_actuator_delay = interp(v_ego, self.steer_actuator_delay_vel, self.steer_actuator_delay_range) # 가변 SR if not self.live_sr: self.angle_diff = abs(anglesteer_desire) - abs(anglesteer_current) if abs(output_scale) >= 1 and v_ego > 8: self.new_steerRatio_prev = interp(self.angle_diff, self.angle_differ_range, self.steerRatio_range) if self.new_steerRatio_prev > self.new_steerRatio: self.new_steerRatio = self.new_steerRatio_prev else: self.mpc_frame += 1 if self.mpc_frame % 10 == 0: self.new_steerRatio -= 0.1 if self.new_steerRatio <= CP.steerRatio: self.new_steerRatio = CP.steerRatio self.mpc_frame = 0 # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) if self.live_sr: sr = max(sm['liveParameters'].steerRatio, 0.1) #Live SR else: sr = max(self.new_steerRatio, 0.1) #가변 SR VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not self.lane_change_enabled) or (abs(output_scale) >= 0.9 and self.lane_change_timer > 1): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ( (sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 self.lane_change_wait_timer = 0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif not blindspot_detected and ( torque_applied or (self.lane_change_auto_delay and self.lane_change_wait_timer > self.lane_change_auto_delay)): self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_adjust_new = interp( v_ego, self.lane_change_adjust_vel, self.lane_change_adjust) self.lane_change_ll_prob = max( self.lane_change_ll_prob - self.lane_change_adjust_new * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.03 and self.lane_change_ll_prob < 0.02: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.98: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego, sm) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, self.new_steer_actuator_delay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 3 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) if self.angle_offset_select == 0: plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) else: plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffset) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.steerRatio = VM.sR plan_send.pathPlan.steerActuatorDelay = self.new_steer_actuator_delay plan_send.pathPlan.steerRateCost = self.steer_rate_cost plan_send.pathPlan.outputScale = output_scale plan_send.pathPlan.vCruiseSet = v_cruise_kph if stand_still: self.standstill_elapsed_time += DT_MDL else: self.standstill_elapsed_time = 0.0 plan_send.pathPlan.standstillElapsedTime = int( self.standstill_elapsed_time) pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 if len(sm['model'].path.poly): path = list(sm['model'].path.poly) # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b # k = y'' / (1 + y'^2)^1.5 # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2] y_pp = 6 * path[0] * self.path_x + 2 * path[1] curv = y_pp / (1. + y_p**2)**1.5 a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = 255 if self.mpc_frame % 1000 == 0: self.kegman = kegman_conf() self.mpc_frame = 0 self.mpc_frame += 1 # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following, self.kegman.conf['accelerationMode'])] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start, model_speed, 2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol self.first_loop = False
def dmonitoringd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(53) params = Params() # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster( ['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].standstill = True cal_rpy = [0, 0, 0] v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: if len(sm['liveCalibration'].rpyCalib) == 3: cal_rpy = sm['liveCalibration'].rpyCalib # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed if driver_engaged: driver_status.update(Events(), True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise # Get model meta if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld if sm.updated['driverState']: events = Events() driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block any engage after certain distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "rhdChecked": driver_status.is_rhd_region_checked, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isPreview": False, } pm.send('dMonitoringState', dat)
def update(self, sm, rr, enable_lead): self.current_time = 1e-9 * max(sm.logMonoTime.values()) if sm.updated['controlsState']: self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['model']: self.ready = True ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): if ids not in ar_pts: self.tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement v_lead = rpt[2] + self.v_ego_hist[0] # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track(v_lead, self.kalman_params) self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) idens = list(sorted(self.tracks.keys())) track_pts = list( [self.tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: cluster_idxs = cluster_points_centroid(track_pts, 2.5) clusters = [None] * (max(cluster_idxs) + 1) for idx in range(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(self.tracks[idens[idx]]) elif len(track_pts) == 1: # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 cluster_idxs = [0] clusters = [Cluster()] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] # if a new point, reset accel to the rest of the cluster for idx in range(len(track_pts)): if self.tracks[idens[idx]].cnt <= 1: aLeadK = clusters[cluster_idxs[idx]].aLeadK aLeadTau = clusters[cluster_idxs[idx]].aLeadTau self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) # *** publish radarState *** dat = messaging.new_message('radarState') dat.valid = sm.all_alive_and_valid() radarState = dat.radarState radarState.mdMonoTime = sm.logMonoTime['model'] radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] if True: radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) return dat
#!/usr/bin/env python3 import time import cereal.messaging as messaging from selfdrive.manager import start_managed_process, kill_managed_process services = ['controlsState', 'deviceState', 'radarState' ] # the services needed to be spoofed to start ui offroad procs = ['camerad', 'ui', 'modeld', 'calibrationd'] [start_managed_process(p) for p in procs] # start needed processes pm = messaging.PubMaster(services) dat_controlsState, dat_deviceState, dat_radar = [ messaging.new_message(s) for s in services ] dat_deviceState.deviceState.started = True try: while True: pm.send('controlsState', dat_controlsState) pm.send('deviceState', dat_deviceState) pm.send('radarState', dat_radar) time.sleep( 1 / 100) # continually send, rate doesn't matter for deviceState except KeyboardInterrupt: [kill_managed_process(p) for p in procs]
def model_replay(lr, frs): spinner = Spinner() spinner.update("starting model replay") vipc_server = VisionIpcServer("camerad") vipc_server.create_buffers( VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size)) vipc_server.create_buffers( VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size)) vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) vipc_server.start_listener() sm = messaging.SubMaster(['modelV2', 'driverState']) pm = messaging.PubMaster([ 'roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan' ]) try: managed_processes['modeld'].start() managed_processes['dmonitoringmodeld'].start() time.sleep(5) sm.update(1000) log_msgs = [] last_desire = None recv_cnt = defaultdict(lambda: 0) frame_idxs = defaultdict(lambda: 0) # init modeld with valid calibration cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"] for _ in range(5): pm.send(cal_msgs[0].which(), cal_msgs[0].as_builder()) time.sleep(0.1) msgs = defaultdict(list) for msg in lr: msgs[msg.which()].append(msg) for cam_msgs in zip_longest(msgs['roadCameraState'], msgs['wideRoadCameraState'], msgs['driverCameraState']): # need a pair of road/wide msgs if TICI and None in (cam_msgs[0], cam_msgs[1]): break for msg in cam_msgs: if msg is None: continue if SEND_EXTRA_INPUTS: if msg.which() == "liveCalibration": last_calib = list(msg.liveCalibration.rpyCalib) pm.send(msg.which(), replace_calib(msg, last_calib)) elif msg.which() == "lateralPlan": last_desire = msg.lateralPlan.desire dat = messaging.new_message('lateralPlan') dat.lateralPlan.desire = last_desire pm.send('lateralPlan', dat) if msg.which() in VIPC_STREAM: msg = msg.as_builder() camera_state = getattr(msg, msg.which()) img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0] frame_idxs[msg.which()] += 1 # send camera state and frame camera_state.frameId = frame_idxs[msg.which()] pm.send(msg.which(), msg) vipc_server.send(VIPC_STREAM[msg.which()], img.flatten().tobytes(), camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) recv = None if msg.which() in ('roadCameraState', 'wideRoadCameraState'): if not TICI or min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState'] ) > recv_cnt['modelV2']: recv = "modelV2" elif msg.which() == 'driverCameraState': recv = "driverState" # wait for a response with Timeout(15, f"timed out waiting for {recv}"): if recv: recv_cnt[recv] += 1 log_msgs.append(messaging.recv_one(sm.sock[recv])) spinner.update( "replaying models: road %d/%d, driver %d/%d" % (frame_idxs['roadCameraState'], frs['roadCameraState'].frame_count, frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)) if any(frame_idxs[c] >= frs[c].frame_count for c in frame_idxs.keys()): break finally: spinner.close() managed_processes['modeld'].stop() managed_processes['dmonitoringmodeld'].stop() return log_msgs
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = self.enabled CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = self.CP.enableCruise and not self.enabled and CS.cruiseState.enabled # Some override values for Honda # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget) CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = self.enabled CC.hudControl.lanesVisible = self.enabled CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED meta = self.sm['modelV2'].meta if len(meta.desirePrediction) and ldw_allowed: l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] cameraOffset = ntune_get("cameraOffset") l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + cameraOffset)) r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - cameraOffset)) CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: self.events.add(EventName.ldw) clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) self.AM.add_many(self.sm.frame, alerts, self.enabled) self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert if not self.read_only: # send car controls over can can_sends = self.CI.apply(CC, self) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) # Curvature & Steering angle params = self.sm['liveParameters'] lat_plan = self.sm['lateralPlan'] steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo) self.angle_steers_des = math.degrees(self.VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) self.angle_steers_des += params.angleOffsetDeg # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid controlsState = dat.controlsState controlsState.alertText1 = self.AM.alert_text_1 controlsState.alertText2 = self.AM.alert_text_2 controlsState.alertSize = self.AM.alert_size controlsState.alertStatus = self.AM.alert_status controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertType = self.AM.alert_type controlsState.alertSound = self.AM.audible_alert controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled controlsState.active = self.active controlsState.curvature = curvature controlsState.steeringAngleDesiredDeg = self.angle_steers_des controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.cruiseVirtualMaxSpeed if self.CP.openpilotLongitudinalControl else self.v_cruise_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.vTargetLead = float(v_acc) controlsState.aTarget = float(a_acc) controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter controlsState.angleSteers = steer_angle_without_offset * CV.RAD_TO_DEG controlsState.cluSpeedMs = self.clu_speed_ms controlsState.applyAccel = self.apply_accel controlsState.fusedAccel = self.fused_accel controlsState.leadDist = self.lead_drel controlsState.aReqValue = self.aReqValue controlsState.aReqValueMin = self.aReqValueMin controlsState.aReqValueMax = self.aReqValueMax controlsState.steerRatio = self.VM.sR controlsState.steerRateCost = ntune_get('steerRateCost') controlsState.steerActuatorDelay = ntune_get('steerActuatorDelay') if self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log elif self.CP.lateralTuning.which() == 'pid': controlsState.lateralControlState.pidState = lac_log elif self.CP.lateralTuning.which() == 'lqr': controlsState.lateralControlState.lqrState = lac_log elif self.CP.lateralTuning.which() == 'indi': controlsState.lateralControlState.indiState = lac_log self.pm.send('controlsState', dat) # carState car_events = self.events.to_msg() cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = car_events self.pm.send('carState', cs_send) # carEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('carEvents', len(self.events)) ce_send.carEvents = car_events self.pm.send('carEvents', ce_send) self.events_prev = self.events.names.copy() # carParams - logged every 50 seconds (> 1 per segment) if (self.sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message('carParams') cp_send.carParams = self.CP self.pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) # copy CarControl to pass to CarInterface on the next iteration self.CC = CC
def model_replay(lr, fr, desire=None, calib=None): spinner = Spinner() spinner.update("starting model replay") vipc_server = None pm = messaging.PubMaster( ['roadCameraState', 'liveCalibration', 'lateralPlan']) sm = messaging.SubMaster(['modelV2']) # TODO: add dmonitoringmodeld try: managed_processes['modeld'].start() time.sleep(5) sm.update(1000) desires_by_index = { v: k for k, v in log.LateralPlan.Desire.schema.enumerants.items() } cal = [msg for msg in lr if msg.which() == "liveCalibration"] for msg in cal[:5]: pm.send(msg.which(), replace_calib(msg, calib)) log_msgs = [] frame_idx = 0 for msg in tqdm(lr): if msg.which() == "liveCalibration": pm.send(msg.which(), replace_calib(msg, calib)) elif msg.which() == "roadCameraState": if desire is not None: for i in desire[frame_idx].nonzero()[0]: dat = messaging.new_message('lateralPlan') dat.lateralPlan.desire = desires_by_index[i] pm.send('lateralPlan', dat) f = msg.as_builder() pm.send(msg.which(), f) img = fr.get(frame_idx, pix_fmt="yuv420p")[0] if vipc_server is None: w, h = { int(3 * w * h / 2): (w, h) for (w, h) in [tici_f_frame_size, eon_f_frame_size] }[len(img)] vipc_server = VisionIpcServer("camerad") vipc_server.create_buffers( VisionStreamType.VISION_STREAM_YUV_BACK, 40, False, w, h) vipc_server.start_listener() time.sleep(1) # wait for modeld to connect vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK, img.flatten().tobytes(), f.roadCameraState.frameId, f.roadCameraState.timestampSof, f.roadCameraState.timestampEof) with Timeout(seconds=15): log_msgs.append(messaging.recv_one(sm.sock['modelV2'])) spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count)) frame_idx += 1 if frame_idx >= fr.frame_count: break except KeyboardInterrupt: pass finally: spinner.close() managed_processes['modeld'].stop() return log_msgs
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset lca_left = sm['carState'].leftBlindspot lca_right = sm['carState'].rightBlindspot # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc #VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) VM.update_params(sm['liveParameters'].stiffnessFactor, CP.steerRatio) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if self.lane_change_direction == LaneChangeDirection.left: torque_applied = sm['carState'].steeringTorque > 0 and sm[ 'carState'].steeringPressed if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not lca_left: torque_applied = True # Enable auto LCA only once after 2 sec else: torque_applied = sm['carState'].steeringTorque < 0 and sm[ 'carState'].steeringPressed if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not lca_right: torque_applied = True # Enable auto LCA only once after 2 sec lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: if self.prev_torque_applied or self.lane_change_direction == LaneChangeDirection.left and not lca_left or \ self.lane_change_direction == LaneChangeDirection.right and not lca_right: self.lane_change_state = LaneChangeState.laneChangeStarting else: if self.pre_auto_LCA_timer < 10.: self.pre_auto_LCA_timer = 10. else: if self.pre_auto_LCA_timer > 10.3: self.prev_torque_applied = True # bsm elif self.lane_change_state == LaneChangeState.laneChangeStarting: if lca_left and self.lane_change_direction == LaneChangeDirection.left and not self.prev_torque_applied: self.lane_change_BSM = LaneChangeBSM.left self.lane_change_state = LaneChangeState.preLaneChange elif lca_right and self.lane_change_direction == LaneChangeDirection.right and not self.prev_torque_applied: self.lane_change_BSM = LaneChangeBSM.right self.lane_change_state = LaneChangeState.preLaneChange else: # starting self.lane_change_BSM = LaneChangeBSM.off if self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out lanelines over .2s self.lane_change_ll_prob = max( self.lane_change_ll_prob - DT_MDL / 5, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # starting #elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out lanelines over .2s #self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL/5, 0.0) # 98% certainty #if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: #self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 if self.lane_change_BSM == LaneChangeBSM.right: if not lca_right: self.lane_change_BSM = LaneChangeBSM.off if self.lane_change_BSM == LaneChangeBSM.left: if not lca_left: self.lane_change_BSM = LaneChangeBSM.off else: self.lane_change_timer += DT_MDL if self.lane_change_state == LaneChangeState.off: self.pre_auto_LCA_timer = 0.0 self.prev_torque_applied = False elif not (3. < self.pre_auto_LCA_timer < 10.): # stop afer 3 sec resume from 10 when torque applied self.pre_auto_LCA_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool( sm['liveParameters'].posenetValid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.laneChangeBSM = self.lane_change_BSM pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) sr = max(sm['liveParameters'].steerRatio, 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) md = sm['modelV2'] self.LP.parse_model(sm['modelV2']) if len(md.position.x) == TRAJECTORY_SIZE and len( md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack( [md.position.x, md.position.y, md.position.z]) self.t_idxs = list(md.position.t) self.plan_yaw = list(md.orientation.z) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or ( not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ( (sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max( self.lane_change_ll_prob - 2 * DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min( self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [ LaneChangeState.off, LaneChangeState.preLaneChange ]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) y_pts = np.interp(self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1) / v_ego, d_path_xyz[:, 1]) heading_pts = np.interp(self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1) / v_ego, self.plan_yaw) # init state self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 # TODO negative sign, still run mpc in ENU, make NED self.cur_state.delta = -math.radians(angle_steers - angle_offset) / VM.sR v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed v_poly = [0.0, 0.0, 0.0, v_ego_mpc] assert len(v_poly) == 4 assert len(y_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, v_poly, curvature_factor, CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 # TODO negative sign, still run mpc in ENU, make NED next_delta = -interp(DT_MDL + delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.delta) next_rate = -interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.rate) # reset to current steer angle if not active or overriding if active: delta_desired = next_delta rate_desired = math.degrees(next_rate * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution.delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'modelV2' ]) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [0, 0, 0, 0] plan_send.pathPlan.lPoly = [0, 0, 0, 0] plan_send.pathPlan.rPoly = [0, 0, 0, 0] plan_send.pathPlan.lProb = float(self.LP.lll_prob) plan_send.pathPlan.rProb = float(self.LP.rll_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if EON: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is not None: del managed_processes["pandad"] if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] started_prev = False logger_dead = False params = Params() thermal_sock = messaging.sub_sock('thermal') pm = messaging.PubMaster(['managerState']) while 1: msg = messaging.recv_sock(thermal_sock, wait=True) if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False driver_view = params.get("IsDriverViewEnabled") == b"1" # TODO: refactor how manager manages processes for p in reversed(car_started_processes): if p not in driver_view_processes or not driver_view: kill_managed_process(p) for p in driver_view_processes: if driver_view: start_managed_process(p) else: kill_managed_process(p) # trigger an update after going offroad if started_prev: os.sync() send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.thermal.started # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # send managerState states = [] for p in managed_processes: state = log.ManagerState.ProcessState.new_message() state.name = p if p in running: state.running = running[p].is_alive() state.pid = running[p].pid state.exitCode = running[p].exitcode or 0 states.append(state) msg = messaging.new_message('managerState') msg.managerState.processes = states pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def dmonitoringd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster( ['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].standstill = True cal_rpy = [0, 0, 0] v_cruise_last = 0 driver_engaged = False # dragonpilot last_ts = 0 dp_last_modified = None dp_enable_driver_monitoring = True # 10Hz <- dmonitoringmodeld while True: cur_time = sec_since_boot() if cur_time - last_ts >= 5.: modified = get_last_modified() if dp_last_modified != modified: dp_enable_driver_monitoring = False if params.get( "DragonEnableDriverMonitoring", encoding='utf8') == "0" else True try: dp_awareness_time = int( params.get("DragonSteeringMonitorTimer", encoding='utf8')) except (TypeError, ValueError): dp_awareness_time = 70. driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60. dp_last_modified = modified last_ts = cur_time # reset all awareness val and set to rhd region, this will enforce steering monitor. if not dp_enable_driver_monitoring: driver_status.active_monitoring_mode = False driver_status.awareness = 1. driver_status.awareness_active = 1. driver_status.awareness_passive = 1. driver_status.terminal_alert_cnt = 0 driver_status.terminal_time = 0 driver_status.face_detected = False driver_status.hi_stds = 0 sm.update() # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: if len(sm['liveCalibration'].rpyCalib) == 3: cal_rpy = sm['liveCalibration'].rpyCalib # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed if driver_engaged: _ = driver_status.update([], True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise # Get model meta if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld if sm.updated['driverState']: events = [] driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block any engage after certain distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) # Update events from driver state events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events, "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "rhdChecked": driver_status.is_rhd_region_checked, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isPreview": False, } pm.send('dMonitoringState', dat)
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset if not self.param_OpkrEnableLearner: kyd = kyd_conf() #self.steer_rate_cost = float(kyd.conf['steerRateCost']) self.sRBP = kyd.conf['sR_BP'] self.sRBoost = kyd.conf['sR_Boost'] boost_rate = interp(abs(angle_steers), self.sRBP, self.sRBoost) self.kyd_steerRatio = self.steerRatio + boost_rate self.sR_Cost = [1.5,1.32,1.15,0.99,0.84,0.72,0.62,0.53,0.36,0.275,0.20,0.175,0.15,0.11,0.05] #self.sR_Cost = [1.0,0.90,0.81,0.73,0.66,0.60,0.54,0.48,0.36,0.275,0.20,0.175,0.15,0.11,0.05] #self.sR_Cost = [0.75,0.67,0.60,0.54,0.48,0.425,0.37,0.32,0.24,0.19,0.15,0.14,0.13,0.11,0.05] #self.sR_Cost = [0.50,0.46,0.425,0.395,0.37,0.34,0.315,0.29,0.23,0.185,0.15,0.14,0.13,0.11,0.05] #steerRatio = 10.0 #"sR_BP": [0.0,2.0,4.0,6.0,8.0,10.0,12.0,14.0,20.0,27.0,35.0,40.0,45.0,60.0,100], #"sR_Boost": [0.0,0.7,1.3,1.9,2.5,3.05,3.55,4.0,5.0,5.7,6.2,6.35,6.4,6.5,8.0], self.steer_rate_cost = interp(abs(angle_steers), self.sRBP, self.sR_Cost) # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) sr = max(sm['liveParameters'].steerRatio, 0.1) #VM.update_params(0.8, CP.steerRatio) # 타이어 강성계수 고정,조향비율 고정 계산 VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_run_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 self.lane_change_wait_timer = 0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif not blindspot_detected and (torque_applied or (self.lane_change_auto_delay and self.lane_change_wait_timer > self.lane_change_auto_delay)): self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max(self.lane_change_ll_prob - 1.5*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.laneChangeDone # done elif self.lane_change_state == LaneChangeState.laneChangeDone: if not one_blinker: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_run_timer = 0.0 else: self.lane_change_run_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego) # account for actuation delay if self.param_OpkrEnableLearner: self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) else: self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.kyd_steerRatio, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] if self.param_OpkrEnableLearner: rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: rate_desired = math.degrees(self.mpc_solution[0].rate[0] * self.kyd_steerRatio) else: if self.param_OpkrEnableLearner: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR else: delta_desired = math.radians(angle_steers - angle_offset) / self.kyd_steerRatio rate_desired = 0.0 self.cur_state[0].delta = delta_desired if self.param_OpkrEnableLearner: self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) else: self.angle_steers_des_mpc = float(math.degrees(delta_desired * self.kyd_steerRatio) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) if self.param_OpkrEnableLearner: self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR else: self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / self.kyd_steerRatio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 3 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffset) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction if not self.param_OpkrEnableLearner: plan_send.pathPlan.steerRatio = float(self.kyd_steerRatio) pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def bridge_keep_alive(q: Any): while 1: try: bridge(q) break except RuntimeError: print("Restarting bridge...") if __name__ == "__main__": # make sure params are in a good state set_params_enabled() msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] Params().put("CalibrationParams", msg.to_bytes()) q: Any = Queue() p = Process(target=bridge_keep_alive, args=(q, ), daemon=True) p.start() if args.joystick: # start input poll for joystick from lib.manual_ctrl import wheel_poll_thread wheel_poll_thread(q) p.join() else: # start input poll for keyboard
def main(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState']) if pm is None: pm = messaging.PubMaster(['liveParameters']) params_reader = Params() # wait for stats about the car to come in from controls cloudlog.info("paramsd is waiting for CarParams") CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") params = params_reader.get("LiveParameters") # Check if car model matches if params is not None: params = json.loads(params) if params.get('carFingerprint', None) != CP.carFingerprint: cloudlog.info("Parameter learner found parameters for wrong car.") params = None if params is None: params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': CP.steerRatio, 'stiffnessFactor': 1.0, 'angleOffsetAverage': 0.0, } cloudlog.info("Parameter learner resetting to default values") learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio while True: sm.update() for which, updated in sm.updated.items(): if not updated: continue t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) if sm.updated['carState'] and (learner.carstate_counter % CARSTATE_DECIMATION == 0): msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True x = learner.kf.x msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverage = math.degrees( x[States.ANGLE_OFFSET]) msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees( x[States.ANGLE_OFFSET_FAST]) msg.liveParameters.valid = all(( abs(msg.liveParameters.angleOffsetAverage) < 10.0, abs(msg.liveParameters.angleOffset) < 10.0, 0.5 <= msg.liveParameters.stiffnessFactor <= 2.0, min_sr <= msg.liveParameters.steerRatio <= max_sr, )) if learner.carstate_counter % 6000 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': msg.liveParameters.steerRatio, 'stiffnessFactor': msg.liveParameters.stiffnessFactor, 'angleOffsetAverage': msg.liveParameters.angleOffsetAverage, } put_nonblocking("LiveParameters", json.dumps(params)) pm.send('liveParameters', msg)
def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset steerRateCost = ntune_get('steerRateCost') if self.steer_rate_cost_prev != steerRateCost: self.steer_rate_cost_prev = steerRateCost self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) if self.use_dynamic_sr: sr = interp(abs(self.angle_steers_des_mpc), [5., 15.], [11.8, 16.2]) else: if ntune_isEnabled('useLiveSteerRatio'): sr = max(sm['liveParameters'].steerRatio, 0.1) else: sr = max(ntune_get('steerRatio'), 0.1) VM.update_params(x, sr) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \ self.auto_lane_change_enabled and \ (AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied and (not blindspot_detected or self.prev_torque_applied): self.lane_change_state = LaneChangeState.laneChangeStarting elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0: self.auto_lane_change_timer = 10.0 elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied: self.prev_torque_applied = True # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL if self.lane_change_state == LaneChangeState.off: self.auto_lane_change_timer = 0.0 self.prev_torque_applied = False elif self.auto_lane_change_timer < (AUTO_LCA_START_TIME+0.25): # stop afer 3 sec resume from 10 when torque applied self.auto_lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.LP.update_d_poly(v_ego) steerActuatorDelay = ntune_get('steerActuatorDelay') # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 3 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffset) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.autoLaneChangeEnabled = self.auto_lane_change_enabled plan_send.pathPlan.autoLaneChangeTimer = int(AUTO_LCA_START_TIME) - int(self.auto_lane_change_timer) plan_send.pathPlan.steerRatio = VM.sR plan_send.pathPlan.steerRateCost = steerRateCost plan_send.pathPlan.steerActuatorDelay = steerActuatorDelay pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)
def main(sm=None, pm=None): gc.disable() set_realtime_priority(5) if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) if pm is None: pm = messaging.PubMaster(['liveParameters']) params_reader = Params() # wait for stats about the car to come in from controls cloudlog.info("paramsd is waiting for CarParams") CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") # Check if car model matches if params is not None: params = json.loads(params) if params.get('carFingerprint', None) != CP.carFingerprint: cloudlog.info("Parameter learner found parameters for wrong car.") params = None # Check if starting values are sane if params is not None: try: angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr params_sane = angle_offset_sane and steer_ratio_sane if not params_sane: cloudlog.info(f"Invalid starting values found {params}") params = None except Exception as e: cloudlog.info(f"Error reading params {params}: {str(e)}") params = None # TODO: cache the params with the capnp struct if params is None: params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': CP.steerRatio, 'stiffnessFactor': 1.0, 'angleOffsetAverageDeg': 0.0, } cloudlog.info("Parameter learner resetting to default values") # When driving in wet conditions the stiffness can go down, and then be too low on the next drive # Without a way to detect this we have to reset the stiffness every drive params['stiffnessFactor'] = 1.0 learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) angle_offset_average = params['angleOffsetAverageDeg'] angle_offset = angle_offset_average while True: sm.update() for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: x = learner.kf.x P = np.sqrt(learner.kf.P.diagonal()) if not all(map(math.isfinite, x)): cloudlog.error( "NaN in liveParameters estimate. Resetting to default values" ) learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) x = learner.kf.x angle_offset_average = clip( math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) angle_offset = clip( math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] liveParameters = msg.liveParameters liveParameters.posenetValid = True liveParameters.sensorValid = True liveParameters.steerRatio = float(x[States.STEER_RATIO]) liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) liveParameters.roll = float(x[States.ROAD_ROLL]) liveParameters.angleOffsetAverageDeg = angle_offset_average liveParameters.angleOffsetDeg = angle_offset liveParameters.valid = all(( abs(liveParameters.angleOffsetAverageDeg) < 10.0, abs(liveParameters.angleOffsetDeg) < 10.0, 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) liveParameters.steerRatioStd = float(P[States.STEER_RATIO]) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS]) liveParameters.angleOffsetAverageStd = float( P[States.ANGLE_OFFSET]) liveParameters.angleOffsetFastStd = float( P[States.ANGLE_OFFSET_FAST]) if sm.frame % 1200 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': liveParameters.steerRatio, 'stiffnessFactor': liveParameters.stiffnessFactor, 'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg, } put_nonblocking("LiveParameters", json.dumps(params)) pm.send('liveParameters', msg)
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() ignore = [] if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID: ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc ] cloudlog.debug(' '.join(running_list)) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [ p.get_process_state_msg() for p in managed_processes.values() ] pm.send('managerState', msg) # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): cloudlog.warning(f"Shutting down manager - {param} set") shutdown = True if shutdown: break
#!/usr/bin/env python3 import time import cereal.messaging as messaging from selfdrive.manager.process_config import managed_processes if __name__ == "__main__": procs = ['camerad', 'ui', 'modeld', 'calibrationd'] for p in procs: managed_processes[p].start() pm = messaging.PubMaster( ['controlsState', 'deviceState', 'pandaStates', 'carParams']) msgs = { s: messaging.new_message(s) for s in ['controlsState', 'deviceState', 'carParams'] } msgs['deviceState'].deviceState.started = True msgs['carParams'].carParams.openpilotLongitudinalControl = True msgs['pandaStates'] = messaging.new_message('pandaStates', 1) msgs['pandaStates'].pandaStates[0].ignitionLine = True try: while True: time.sleep(1 / 100) # continually send, rate doesn't matter for s in msgs: pm.send(s, msgs[s]) except KeyboardInterrupt: for p in procs:
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = self.enabled CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not self.CP.enableCruise or ( not self.enabled and CS.cruiseState.enabled) # Some override values for Honda # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) CC.cruiseControl.speedOverride = float( speed_override if self.CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = self.CI.calc_accel_override( CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget) CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = self.enabled CC.hudControl.lanesVisible = self.enabled CC.hudControl.leadVisible = self.sm['plan'].hasLead right_lane_visible = self.sm['pathPlan'].rProb > 0.5 left_lane_visible = self.sm['pathPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) recent_blinker = (self.sm.frame - self.last_blinker_frame ) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED meta = self.sm['model'].meta if len(meta.desirePrediction) and ldw_allowed: l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) CC.hudControl.leftLaneDepart = bool( l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool( r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: self.events.add(EventName.ldw) alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) self.AM.add_many(self.sm.frame, alerts, self.enabled) self.AM.process_alerts(self.sm.frame) CC.hudControl.visualAlert = self.AM.visual_alert if not self.read_only: # send car controls over can can_sends = self.CI.apply(CC) self.pm.send( 'sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid controlsState = dat.controlsState controlsState.alertText1 = self.AM.alert_text_1 controlsState.alertText2 = self.AM.alert_text_2 controlsState.alertSize = self.AM.alert_size controlsState.alertStatus = self.AM.alert_status controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertType = self.AM.alert_type controlsState.alertSound = self.AM.audible_alert controlsState.driverMonitoringOn = self.sm[ 'dMonitoringState'].faceDetected controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.planMonoTime = self.sm.logMonoTime['plan'] controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan'] controlsState.enabled = self.enabled controlsState.active = self.active controlsState.vEgo = CS.vEgo controlsState.vEgoRaw = CS.vEgoRaw controlsState.angleSteers = CS.steeringAngle controlsState.curvature = self.VM.calc_curvature( steer_angle_rad, CS.vEgo) controlsState.steerOverride = CS.steeringPressed controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.angleSteersDes = float(self.LaC.angle_steers_des) controlsState.vTargetLead = float(v_acc) controlsState.aTarget = float(a_acc) controlsState.jerkFactor = float(self.sm['plan'].jerkFactor) controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive controlsState.vCurvature = self.sm['plan'].vCurvature controlsState.decelForModel = self.sm[ 'plan'].longitudinalPlanSource == LongitudinalPlanSource.model controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.mapValid = self.sm['plan'].mapValid controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter if self.CP.lateralTuning.which() == 'pid': controlsState.lateralControlState.pidState = lac_log elif self.CP.lateralTuning.which() == 'lqr': controlsState.lateralControlState.lqrState = lac_log elif self.CP.lateralTuning.which() == 'indi': controlsState.lateralControlState.indiState = lac_log self.pm.send('controlsState', dat) # carState car_events = self.events.to_msg() cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = car_events self.pm.send('carState', cs_send) # carEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('carEvents', len(self.events)) ce_send.carEvents = car_events self.pm.send('carEvents', ce_send) self.events_prev = self.events.names.copy() # carParams - logged every 50 seconds (> 1 per segment) if (self.sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message('carParams') cp_send.carParams = self.CP self.pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) # copy CarControl to pass to CarInterface on the next iteration self.CC = CC
def manager_thread(): Process(name="shutdownd", target=launcher, args=("selfdrive.shutdownd", )).start() system("am startservice com.neokii.optool/.MainService") system("am startservice com.neokii.openpilot/.MainService") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log #subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) ignore = [] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False params = Params() sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc ] cloudlog.debug(' '.join(running_list)) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [ p.get_process_state_msg() for p in managed_processes.values() ] pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get_bool("DoUninstall"): break
def mapsd_thread(): global last_gps gps_sock = messaging.sub_sock('gpsLocation', conflate=True) gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True) map_data_sock = messaging.pub_sock('liveMapData') cur_way = None curvature_valid = False curvature = None upcoming_curvature = 0. dist_to_turn = 0. road_points = None while True: gps = messaging.recv_one(gps_sock) gps_ext = messaging.recv_one_or_none(gps_external_sock) if gps_ext is not None: gps = gps_ext.gpsLocationExternal else: gps = gps.gpsLocation last_gps = gps fix_ok = gps.flags & 1 if not fix_ok or last_query_result is None or not cache_valid: cur_way = None curvature = None curvature_valid = False upcoming_curvature = 0. dist_to_turn = 0. road_points = None map_valid = False else: map_valid = True lat = gps.latitude lon = gps.longitude heading = gps.bearing speed = gps.speed query_lock.acquire() cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) if cur_way is not None: pnts, curvature_valid = cur_way.get_lookahead( lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) xs = pnts[:, 0] ys = pnts[:, 1] road_points = [float(x) for x in xs], [float(y) for y in ys] if speed < 10: curvature_valid = False if curvature_valid and pnts.shape[0] <= 3: curvature_valid = False # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found if curvature_valid: # Compute the curvature for each point with np.errstate(divide='ignore'): circles = [ circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:]) ] circles = np.asarray(circles) radii = np.nan_to_num(circles[:, 2]) radii[radii < 10] = np.inf curvature = 1. / radii # Index of closest point closest = np.argmin(np.linalg.norm(pnts, axis=1)) dist_to_closest = pnts[ closest, 0] # We can use x distance here since it should be close # Compute distance along path dists = list() dists.append(0) for p, p_prev in zip(pnts, pnts[1:, :]): dists.append(dists[-1] + np.linalg.norm(p - p_prev)) dists = np.asarray(dists) dists = dists - dists[closest] + dist_to_closest dists = dists[1:-1] close_idx = np.logical_and(dists > 0, dists < 500) dists = dists[close_idx] curvature = curvature[close_idx] if len(curvature): # TODO: Determine left or right turn curvature = np.nan_to_num(curvature) # Outlier rejection new_curvature = np.percentile(curvature, 90, interpolation='lower') k = 0.6 upcoming_curvature = k * upcoming_curvature + ( 1 - k) * new_curvature in_turn_indices = curvature > 0.8 * new_curvature if np.any(in_turn_indices): dist_to_turn = np.min(dists[in_turn_indices]) else: dist_to_turn = 999 else: upcoming_curvature = 0. dist_to_turn = 999 query_lock.release() dat = messaging.new_message('liveMapData') if last_gps is not None: dat.liveMapData.lastGps = last_gps if cur_way is not None: dat.liveMapData.wayId = cur_way.id # Speed limit max_speed = cur_way.max_speed() if max_speed is not None: dat.liveMapData.speedLimitValid = True dat.liveMapData.speedLimit = max_speed # TODO: use the function below to anticipate upcoming speed limits #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) #if max_speed_ahead is not None and max_speed_ahead_dist is not None: # dat.liveMapData.speedLimitAheadValid = True # dat.liveMapData.speedLimitAhead = float(max_speed_ahead) # dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) advisory_max_speed = cur_way.advisory_max_speed() if advisory_max_speed is not None: dat.liveMapData.speedAdvisoryValid = True dat.liveMapData.speedAdvisory = advisory_max_speed # Curvature dat.liveMapData.curvatureValid = curvature_valid dat.liveMapData.curvature = float(upcoming_curvature) dat.liveMapData.distToTurn = float(dist_to_turn) if road_points is not None: dat.liveMapData.roadX, dat.liveMapData.roadY = road_points if curvature is not None: dat.liveMapData.roadCurvatureX = [float(x) for x in dists] dat.liveMapData.roadCurvature = [float(x) for x in curvature] dat.liveMapData.mapValid = map_valid map_data_sock.send(dat.to_bytes())
def manager_thread() -> None: Process(name="road_speed_limiter", target=launcher, args=("selfdrive.road_speed_limiter", "road_speed_limiter")).start() cloudlog.bind(daemon="manager") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) params = Params() ignore: List[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) pm = messaging.PubMaster(['managerState']) ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) while True: sm.update() started = sm['deviceState'].started ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) running = ' '.join( "%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc) print(running) cloudlog.debug(running) # send managerState msg = messaging.new_message('managerState') msg.managerState.processes = [ p.get_process_state_msg() for p in managed_processes.values() ] pm.send('managerState', msg) # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): shutdown = True params.put("LastManagerExitReason", param) cloudlog.warning(f"Shutting down manager - {param} set") if shutdown: break