def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, only_lead2=False, only_radar=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.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.acceleration = 0.0 # lead car self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy self.only_lead2=only_lead2 self.only_radar=only_radar self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate time.sleep(1) self.sm = messaging.SubMaster(['longitudinalPlan']) from selfdrive.car.hyundai.values import CAR from selfdrive.car.hyundai.interface import CarInterface self.planner = Planner(CarInterface.get_params(CAR.GRANDEUR_IG), init_v=self.speed)
pinv = np.linalg.pinv(X) return pinv def model_polyfit(points): path_pinv = compute_path_pinv() return np.dot(path_pinv, map(float, points)) xx = [] yy = [] deltas = [] psis = [] times = [] CP = CarInterface.get_params(CAR.GRANDEUR_HEV) print('CP.steerRatio1={}'.format(CP.steerRatio)) CP = CarInterface.live_tune(CP) print('CP.steerRatio2={}'.format(CP.steerRatio)) VM = VehicleModel(CP) v_ref = 32.00 # 45 mph curvature_factor = VM.curvature_factor(v_ref) print(curvature_factor) LANE_WIDTH = 3.9 p_l = map(float, model_polyfit(points_l)) p_r = map(float, model_polyfit(points_r)) p_p = map(float, model_polyfit(points_c))