#!/usr/bin/env python3 import time import numpy as np from iqpilot.cereal import log import iqpilot.cereal.messaging as messaging from iqpilot.common.realtime import Ratekeeper, DT_MDL from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from iqpilot.selfdrive.iqmodeld.config import ModelConstants from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from iqpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: Plant.radar = messaging.pub_sock('radarState') Plant.controls_state = messaging.pub_sock('controlsState') Plant.selfdrive_state = messaging.pub_sock('selfdriveState') Plant.car_state = messaging.pub_sock('carState') Plant.plan = messaging.sub_sock('longitudinalPlan') Plant.messaging_initialized = True self.v_lead_prev = 0.0 self.distance = 0. self.speed = speed self.should_stop = False self.acceleration = 0.0 # lead car self.lead_relevancy = lead_relevancy self.distance_lead = distance_lead self.enabled = enabled self.only_lead2 = only_lead2 self.only_radar = only_radar self.e2e = e2e self.personality = personality self.force_decel = force_decel self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate time.sleep(0.1) self.sm = messaging.SubMaster(['longitudinalPlan']) from iqdbc.car.honda.values import CAR from iqdbc.car.honda.interface import CarInterface CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC) self.planner = LongitudinalPlanner(CP, CP_IQ, init_v=self.speed) @property def current_time(self): return float(self.rk.frame) / self.rate def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0): # ******** 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 radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') lp = messaging.new_message('vehicleParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') iq_car_state = messaging.new_message('iqCarState') iq_nav_state = messaging.new_message('iqNavState') live_map_data_iq = messaging.new_message('iqLiveData') gps_data = messaging.new_message('gpsLocation') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead if self.lead_relevancy: d_rel = np.maximum(0., self.distance_lead - self.distance) v_rel = v_lead - self.speed if self.only_radar: status = True elif prob_lead > .5: status = True else: status = False else: d_rel = 200. v_rel = 0. prob_lead = 0.0 status = False lead = log.RadarState.LeadData.new_message() lead.dRel = float(d_rel) lead.yRel = 0.0 lead.vRel = float(v_rel) lead.aRel = float(a_lead - self.acceleration) lead.vLead = float(v_lead) lead.vLeadK = float(v_lead) lead.aLeadK = float(a_lead) # TODO use real radard logic for this lead.aLeadTau = float(_LEAD_ACCEL_TAU) lead.status = status lead.modelProb = float(prob_lead) if not self.only_lead2: radar.radarState.leadOne = lead radar.radarState.leadTwo = lead # Simulate model predicting slightly faster speed # this is to ensure lead policy is effective when model # does not predict slowdown in e2e mode position = log.XYZTData.new_message() position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] model.modelV2.position = position model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.5) velocity = log.XYZTData.new_message() velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] velocity.x[0] = float(self.speed) # always start at current speed model.modelV2.velocity = velocity acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] lead_times = np.asarray(ModelConstants.LEAD_T_IDXS) lead_accel = np.clip(a_lead, -10.0, 5.0) stop_time = -v_lead / lead_accel if lead_accel < 0.0 else np.inf motion_times = np.minimum(lead_times, stop_time) lead_positions = d_rel + v_lead * motion_times + 0.5 * lead_accel * motion_times**2 lead_velocities = np.maximum(v_lead + lead_accel * lead_times, 0.0) for lead_prediction in model.modelV2.leadsV3: lead_prediction.prob = float(prob_lead) lead_prediction.x = [float(x) for x in lead_positions] lead_prediction.v = [float(v) for v in lead_velocities] control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off ss.selfdriveState.experimentalMode = self.e2e ss.selfdriveState.personality = self.personality control.controlsState.forceDecel = self.force_decel car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = bool(self.speed < 0.01) car_state.carState.vCruise = float(v_cruise * 3.6) car_control.carControl.orientationNED = [0., float(pitch), 0.] # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, 'carState': car_state.carState, 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, 'vehicleParameters': lp.vehicleParameters, 'modelV2': model.modelV2, 'iqCarState': iq_car_state.iqCarState, 'iqNavState': iq_nav_state.iqNavState, 'iqLiveData': live_map_data_iq.iqLiveData, 'gpsLocation': gps_data.gpsLocation} self.planner.update(sm) self.acceleration = self.planner.output_a_target if self.planner.output_should_stop: self.acceleration = min(-0.5, self.acceleration) self.speed = self.speed + self.acceleration * self.ts self.should_stop = self.planner.output_should_stop fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts # ******** run the car ******** #print(self.distance, speed) if self.speed <= 0: self.speed = 0 self.acceleration = 0 self.distance = self.distance + self.speed * self.ts # *** radar model *** if self.lead_relevancy: d_rel = np.maximum(0., self.distance_lead - self.distance) v_rel = v_lead - self.speed else: d_rel = 200. v_rel = 0. # print at 5hz # if (self.rk.frame % (self.rate // 5)) == 0: # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" # % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** self.rk.monitor_time() return { "distance": self.distance, "speed": self.speed, "acceleration": self.acceleration, "should_stop": self.should_stop, "distance_lead": self.distance_lead, "fcw": fcw, } # simple engage in standalone mode def plant_thread(): plant = Plant() while 1: plant.step() if __name__ == "__main__": plant_thread()