Skip to content

Commit 4d82b5c

Browse files
haraschaxgreatgitsby
authored andcommitted
Revert "Revert TR again (commaai#35179)"
This reverts commit e9cea3a.
1 parent 350a235 commit 4d82b5c

File tree

9 files changed

+37
-25
lines changed

9 files changed

+37
-25
lines changed

selfdrive/controls/lib/longitudinal_planner.py

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ def parse_model(model_msg, model_error):
9090
return x, v, a, j, throttle_prob
9191

9292
def update(self, sm):
93-
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
93+
self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
9494

9595
if len(sm['carControl'].orientationNED) == 3:
9696
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -113,7 +113,7 @@ def update(self, sm):
113113
# No change cost when user is controlling the speed, or when standstill
114114
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
115115

116-
if self.mpc.mode == 'acc':
116+
if self.mode == 'acc':
117117
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
118118
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
119119
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
@@ -160,8 +160,17 @@ def update(self, sm):
160160
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
161161

162162
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
163-
output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
163+
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
164164
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
165+
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
166+
output_should_stop_e2e = sm['modelV2'].action.shouldStop
167+
168+
if self.mode == 'acc':
169+
output_a_target = output_a_target_mpc
170+
self.output_should_stop = output_should_stop_mpc
171+
else:
172+
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
173+
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
165174

166175
for idx in range(2):
167176
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

selfdrive/modeld/fill_model_msg.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -90,11 +90,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
9090
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
9191

9292
# temporal pose
93-
temporal_pose = modelV2.temporalPose
94-
temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
95-
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
96-
temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
97-
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
93+
#temporal_pose = modelV2.temporalPose
94+
#temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
95+
#temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
96+
#temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
97+
#temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
9898

9999
# poly path
100100
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)

selfdrive/modeld/modeld.py

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@
3131
from openpilot.common.transformations.model import get_warp_matrix
3232
from openpilot.system import sentry
3333
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
34-
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
34+
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
3535
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
3636
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
3737
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
@@ -46,8 +46,8 @@
4646
VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
4747
POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
4848

49-
LAT_SMOOTH_SECONDS = 0.0
50-
LONG_SMOOTH_SECONDS = 0.0
49+
LAT_SMOOTH_SECONDS = 0.1
50+
LONG_SMOOTH_SECONDS = 0.3
5151
MIN_LAT_CONTROL_SPEED = 0.3
5252

5353

@@ -60,7 +60,11 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
6060
action_t=long_action_t)
6161
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
6262

63-
desired_curvature = model_output['desired_curvature'][0, 0]
63+
desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
64+
plan[:,Plan.ORIENTATION_RATE][:,2],
65+
ModelConstants.T_IDXS,
66+
v_ego,
67+
lat_action_t)
6468
if v_ego > MIN_LAT_CONTROL_SPEED:
6569
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
6670
else:
@@ -177,7 +181,7 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_
177181
# TODO model only uses last value now
178182
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
179183
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
180-
self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
184+
self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
181185

182186
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
183187
if SEND_RAW_PRED:
Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
version https://git-lfs.github.com/spec/v1
2-
oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36
3-
size 15966721
2+
oid sha256:19e30484236efff72d519938c3e26461dbeb89c11d81fa7ecbff8e0263333c18
3+
size 15588463
Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
version https://git-lfs.github.com/spec/v1
2-
oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624
3-
size 34882971
2+
oid sha256:dad289ae367cefcb862ef1d707fb4919d008f0eeaa1ebaf18df58d8de5a7d96e
3+
size 46265585

selfdrive/modeld/parse_model_outputs.py

Lines changed: 6 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -88,24 +88,23 @@ def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndar
8888
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
8989
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
9090
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
91+
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
92+
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))
93+
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
94+
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
95+
for k in ['lead_prob', 'lane_lines_prob']:
96+
self.parse_binary_crossentropy(k, outs)
9197
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
9298
self.parse_binary_crossentropy('meta', outs)
9399
return outs
94100

95101
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
96102
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
97103
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
98-
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
99-
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))
100-
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
101-
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
102-
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
103104
if 'lat_planner_solution' in outs:
104105
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
105106
if 'desired_curvature' in outs:
106107
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
107-
for k in ['lead_prob', 'lane_lines_prob']:
108-
self.parse_binary_crossentropy(k, outs)
109108
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
110109
return outs
111110

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
8e57f774d0e8302e1bf1834f84720bb1f9211ae3
1+
b31b7c5c29e6d30ccee2fa5105af778810fcd02e

selfdrive/ui/_spinner

2.03 MB
Binary file not shown.

selfdrive/ui/_text

2.04 MB
Binary file not shown.

0 commit comments

Comments
 (0)