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)
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
if self.read_only:
self.hyundai_lkas = self.read_only
elif CS.cruiseState.enabled and self.hyundai_lkas:
self.CP = CarInterface.live_tune(self.CP, True)
self.hyundai_lkas = False
self.init_flag = 1
self.update_events(CS)
if not self.hyundai_lkas:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_acc, a_acc, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
self.prof.checkpoint("Sent")
if not CS.cruiseState.enabled and not self.hyundai_lkas:
self.hyundai_lkas = True
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))