|
|
@ -219,13 +219,10 @@ def main(demo=False): |
|
|
|
buf_extra = buf_main |
|
|
|
buf_extra = buf_main |
|
|
|
meta_extra = meta_main |
|
|
|
meta_extra = meta_main |
|
|
|
|
|
|
|
|
|
|
|
# TODO: path planner timeout? |
|
|
|
|
|
|
|
sm.update(0) |
|
|
|
sm.update(0) |
|
|
|
desire = DH.desire |
|
|
|
desire = DH.desire |
|
|
|
v_ego = sm["carState"].vEgo |
|
|
|
|
|
|
|
is_rhd = sm["driverMonitoringState"].isRHD |
|
|
|
is_rhd = sm["driverMonitoringState"].isRHD |
|
|
|
frame_id = sm["roadCameraState"].frameId |
|
|
|
frame_id = sm["roadCameraState"].frameId |
|
|
|
# TODO add lag |
|
|
|
|
|
|
|
lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) |
|
|
|
lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) |
|
|
|
if sm.updated["liveCalibration"]: |
|
|
|
if sm.updated["liveCalibration"]: |
|
|
|
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) |
|
|
|
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) |
|
|
@ -293,7 +290,7 @@ def main(demo=False): |
|
|
|
modelv2_send = messaging.new_message('modelV2') |
|
|
|
modelv2_send = messaging.new_message('modelV2') |
|
|
|
posenet_send = messaging.new_message('cameraOdometry') |
|
|
|
posenet_send = messaging.new_message('cameraOdometry') |
|
|
|
fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, |
|
|
|
fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, |
|
|
|
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen) |
|
|
|
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen) |
|
|
|
|
|
|
|
|
|
|
|
desire_state = modelv2_send.modelV2.meta.desireState |
|
|
|
desire_state = modelv2_send.modelV2.meta.desireState |
|
|
|
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] |
|
|
|
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] |
|
|
|