mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-08 09:03:42 +08:00
FrogPilot 0.9.7
This commit is contained in:
FrogAi
+69
-26
@@ -24,6 +24,8 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
|
||||
from openpilot.selfdrive.frogpilot.frogpilot_variables import METADATAS_PATH, MODELS_PATH, get_frogpilot_toggles
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
@@ -31,8 +33,6 @@ MODEL_PATHS = {
|
||||
ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
|
||||
|
||||
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
|
||||
|
||||
class FrameMeta:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
@@ -50,21 +50,32 @@ class ModelState:
|
||||
prev_desire: np.ndarray # for tracking the rising edge of the pulse
|
||||
model: ModelRunner
|
||||
|
||||
def __init__(self, context: CLContext):
|
||||
def __init__(self, context: CLContext, model: str, model_version: str):
|
||||
# FrogPilot variables
|
||||
MODEL_PATHS[ModelRunner.THNEED] = MODELS_PATH / f'{model}.thneed'
|
||||
|
||||
with open(METADATAS_PATH / f'supercombo_metadata_{model_version}.pkl', 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
|
||||
input_shapes = model_metadata.get('input_shapes')
|
||||
self.use_desired_curvature = 'lateral_control_params' in input_shapes and 'prev_desired_curv' in input_shapes
|
||||
|
||||
self.frame = ModelFrame(context)
|
||||
self.wide_frame = ModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
||||
self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
||||
self.prev_desired_curv_20hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
|
||||
|
||||
# img buffers are managed in openCL transform code
|
||||
self.inputs = {
|
||||
'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
|
||||
'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
|
||||
'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
|
||||
'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
|
||||
**({'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32)} if self.use_desired_curvature else {}),
|
||||
**({'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32)} if self.use_desired_curvature else {}),
|
||||
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
|
||||
}
|
||||
|
||||
with open(METADATA_PATH, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
|
||||
self.output_slices = model_metadata['output_slices']
|
||||
net_output_size = model_metadata['output_shapes']['outputs'][1]
|
||||
self.output = np.zeros(net_output_size, dtype=np.float32)
|
||||
@@ -86,17 +97,19 @@ class ModelState:
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire'][0] = 0
|
||||
self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
|
||||
self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
self.desire_20Hz[:-1] = self.desire_20Hz[1:]
|
||||
self.desire_20Hz[-1] = new_desire
|
||||
self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten()
|
||||
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
|
||||
if self.use_desired_curvature:
|
||||
self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
|
||||
|
||||
# if getCLBuffer is not None, frame will be None
|
||||
self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
|
||||
if wbuf is not None:
|
||||
self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
|
||||
self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
|
||||
|
||||
if prepare_only:
|
||||
return None
|
||||
@@ -104,10 +117,18 @@ class ModelState:
|
||||
self.model.execute()
|
||||
outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
|
||||
|
||||
self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
|
||||
self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
|
||||
self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:]
|
||||
self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :]
|
||||
self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
|
||||
self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
|
||||
|
||||
if self.use_desired_curvature:
|
||||
self.prev_desired_curv_20hz[:-1] = self.prev_desired_curv_20hz[1:]
|
||||
self.prev_desired_curv_20hz[-1] = outputs['desired_curvature'][0, :]
|
||||
|
||||
idxs = np.arange(-4,-100,-4)[::-1]
|
||||
self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
|
||||
if self.use_desired_curvature:
|
||||
# TODO model only uses last value now, once that changes we need to input strided action history buffer
|
||||
self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = 0. * self.prev_desired_curv_20hz[-4, :]
|
||||
return outputs
|
||||
|
||||
|
||||
@@ -122,7 +143,16 @@ def main(demo=False):
|
||||
cloudlog.warning("setting up CL context")
|
||||
cl_context = CLContext()
|
||||
cloudlog.warning("CL context ready; loading model")
|
||||
model = ModelState(cl_context)
|
||||
|
||||
# FrogPilot variables
|
||||
frogpilot_toggles = get_frogpilot_toggles()
|
||||
|
||||
model_name = frogpilot_toggles.model
|
||||
model_version = frogpilot_toggles.model_version
|
||||
|
||||
planner_curves = frogpilot_toggles.planner_curvature_model
|
||||
|
||||
model = ModelState(cl_context, model_name, model_version)
|
||||
cloudlog.warning("models loaded, modeld starting")
|
||||
|
||||
# visionipc clients
|
||||
@@ -149,8 +179,8 @@ def main(demo=False):
|
||||
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
|
||||
|
||||
# messaging
|
||||
pm = PubMaster(["modelV2", "cameraOdometry"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
|
||||
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "frogpilotPlan"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
@@ -218,7 +248,9 @@ def main(demo=False):
|
||||
desire = DH.desire
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["roadCameraState"].frameId
|
||||
lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
|
||||
v_ego = max(sm["carState"].vEgo, 0.)
|
||||
if model.use_desired_curvature:
|
||||
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
|
||||
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
|
||||
@@ -249,7 +281,7 @@ def main(demo=False):
|
||||
inputs:dict[str, np.ndarray] = {
|
||||
'desire': vec_desire,
|
||||
'traffic_convention': traffic_convention,
|
||||
'lateral_control_params': lateral_control_params,
|
||||
**({'lateral_control_params': lateral_control_params} if model.use_desired_curvature else {}),
|
||||
}
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
@@ -259,24 +291,35 @@ def main(demo=False):
|
||||
|
||||
if model_output is not None:
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
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,
|
||||
meta_main.timestamp_eof, model_execution_time, live_calib_seen)
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
|
||||
planner_curves)
|
||||
|
||||
desire_state = modelv2_send.modelV2.meta.desireState
|
||||
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan'], frogpilot_toggles)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
modelv2_send.modelV2.meta.turnDirection = DH.turn_direction
|
||||
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
|
||||
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
|
||||
drivingdata_send.drivingModelData.meta.turnDirection = DH.turn_direction
|
||||
|
||||
fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
|
||||
pm.send('modelV2', modelv2_send)
|
||||
pm.send('drivingModelData', drivingdata_send)
|
||||
pm.send('cameraOdometry', posenet_send)
|
||||
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
|
||||
# Update FrogPilot parameters
|
||||
if sm['frogpilotPlan'].togglesUpdated:
|
||||
frogpilot_toggles = get_frogpilot_toggles()
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
|
||||
Reference in New Issue
Block a user