mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-17 15:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

maybe>?

Browse Source
This commit is contained in:
Kumar
2024-09-28 15:01:41 -07:00
committed by GitHub
parent d4b79ff36a
commit 8ee80deb96
3 changed files with 21 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+2 -2
View File
@@ -255,8 +255,8 @@ class RadarD:
self.radar_state.radarErrors = list(radar_errors)
self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
if len(sm['modelV2'].temporalPose.trans):
model_v_ego = sm['modelV2'].temporalPose.trans[0]
if len(sm['modelV2'].velocity.x):
model_v_ego = sm['modelV2'].velocity.x[0]
else:
model_v_ego = self.v_ego
leads_v3 = sm['modelV2'].leadsV3
selfdrive/modeld/fill_model_msg.py
+19 -19
View File
@@ -99,6 +99,25 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
orientation_rate = modelV2.orientationRate
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# temporal pose
temporal_pose = modelV2.temporalPose
if custom_model.valid:
if custom_model.capabilities & ModelCapabilities.PlanTemporalPose:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
else:
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
else:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
# poly path
poly_path = driving_model_data.path
fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
@@ -185,25 +204,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
meta.hardBrakePredicted = hard_brake_predicted.item()
# temporal pose
temporal_pose = modelV2.temporalPose
if custom_model.valid:
if custom_model.capabilities & ModelCapabilities.PlanTemporalPose:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
else:
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
else:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
# confidence
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
# any disengage prob
selfdrive/modeld/parse_model_outputs.py
-2
View File
@@ -91,8 +91,6 @@ class Parser:
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
if 'sim_pose' in outs:
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))