|
|
@ -184,7 +184,7 @@ def main(demo=False): |
|
|
|
|
|
|
|
|
|
|
|
# messaging |
|
|
|
# messaging |
|
|
|
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) |
|
|
|
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) |
|
|
|
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) |
|
|
|
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveActuatorDelay"]) |
|
|
|
|
|
|
|
|
|
|
|
publish_state = PublishState() |
|
|
|
publish_state = PublishState() |
|
|
|
params = Params() |
|
|
|
params = Params() |
|
|
@ -209,9 +209,6 @@ def main(demo=False): |
|
|
|
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) |
|
|
|
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) |
|
|
|
cloudlog.info("modeld got CarParams: %s", CP.brand) |
|
|
|
cloudlog.info("modeld got CarParams: %s", CP.brand) |
|
|
|
|
|
|
|
|
|
|
|
# TODO this needs more thought, use .2s extra for now to estimate other delays |
|
|
|
|
|
|
|
steer_delay = CP.steerActuatorDelay + .2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
DH = DesireHelper() |
|
|
|
DH = DesireHelper() |
|
|
|
|
|
|
|
|
|
|
|
while True: |
|
|
|
while True: |
|
|
@ -252,6 +249,7 @@ def main(demo=False): |
|
|
|
is_rhd = sm["driverMonitoringState"].isRHD |
|
|
|
is_rhd = sm["driverMonitoringState"].isRHD |
|
|
|
frame_id = sm["roadCameraState"].frameId |
|
|
|
frame_id = sm["roadCameraState"].frameId |
|
|
|
v_ego = max(sm["carState"].vEgo, 0.) |
|
|
|
v_ego = max(sm["carState"].vEgo, 0.) |
|
|
|
|
|
|
|
steer_delay = sm["liveActuatorDelay"].steerActuatorDelay |
|
|
|
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) |
|
|
|
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) |
|
|
|
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: |
|
|
|
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: |
|
|
|
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) |
|
|
|
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) |
|
|
|