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))