sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tomb Raider 14 (#35620)

Browse Source 
* f7db6a09-43c5-4db9-b856-7fe1a1c231eb/400

* bd99d079-9afb-4af5-9f31-236d5c9ff15f/400

* aggressive tr: 7707a4ca-7d5e-47a2-8760-93b5004695cd/400

* bd99d079-9afb-4af5-9f31-236d5c9ff15f/400

* ae82d7a8-b74d-43b5-ab6d-d72e6040dab3/400

* revert stop distance

* comments
pull/35629/head
Harald Schäfer 3 weeks ago committed by GitHub
parent a5630eb7b7
commit 64fd3f9860
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
6 changed files with 33 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      selfdrive/controls/lib/longitudinal_planner.py
  2. 7
      selfdrive/modeld/fill_model_msg.py
  3. 14
      selfdrive/modeld/modeld.py
  4. 4
      selfdrive/modeld/models/driving_policy.onnx
  5. 4
      selfdrive/modeld/models/driving_vision.onnx
  6. 13
      selfdrive/modeld/parse_model_outputs.py

18
selfdrive/controls/lib/longitudinal_planner.py
Unescape View File

@ -52,6 +52,7 @@ class LongitudinalPlanner:
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
self.mpc = LongitudinalMpc(dt=dt)
# TODO remove mpc modes when TR released
self.mpc.mode = 'acc'
self.fcw = False
self.dt = dt
@ -90,7 +91,7 @@ class LongitudinalPlanner:
return x, v, a, j, throttle_prob
def update(self, sm):
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@ -113,7 +114,7 @@ class LongitudinalPlanner:
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if self.mpc.mode == 'acc':
if self.mode == 'acc':
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
@ -127,7 +128,7 @@ class LongitudinalPlanner:
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
# Compute model v_ego error
# TODO v_model_error is deprecated with TR
self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error)
# Don't clip at low speeds since throttle_prob doesn't account for creep
@ -160,8 +161,17 @@ class LongitudinalPlanner:
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
if self.mode == 'acc':
output_a_target = output_a_target_mpc
self.output_should_stop = output_should_stop_mpc
else:
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
for idx in range(2):
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

7
selfdrive/modeld/fill_model_msg.py
Unescape View File

@ -89,13 +89,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# temporal pose
temporal_pose = modelV2.temporalPose
temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
# poly path
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)

14
selfdrive/modeld/modeld.py
Unescape View File

@ -31,7 +31,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
from openpilot.common.transformations.model import get_warp_matrix
from openpilot.system import sentry
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
@ -46,8 +46,8 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
LAT_SMOOTH_SECONDS = 0.0
LONG_SMOOTH_SECONDS = 0.0
LAT_SMOOTH_SECONDS = 0.1
LONG_SMOOTH_SECONDS = 0.3
MIN_LAT_CONTROL_SPEED = 0.3
@ -60,7 +60,11 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
action_t=long_action_t)
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
desired_curvature = model_output['desired_curvature'][0, 0]
desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
plan[:,Plan.ORIENTATION_RATE][:,2],
ModelConstants.T_IDXS,
v_ego,
lat_action_t)
if v_ego > MIN_LAT_CONTROL_SPEED:
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
else:
@ -174,7 +178,7 @@ class ModelState:
# TODO model only uses last value now
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
if SEND_RAW_PRED:

4
selfdrive/modeld/models/driving_policy.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:5f714fb38bcd44b5d9f44e3a8e1595e1dfdf7558f0eec3485cf3f2dbb6dc7d8d
size 15971805
oid sha256:1741cad23f6f451782b5db6182218749ee12072e393d57eac36d8d5c55d9358a
size 15583374

4
selfdrive/modeld/models/driving_vision.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:3ac4867fbc618037e8d03143edbfeeae960f2025644b5dcf36c6665271b4f874
size 34883375
oid sha256:3d2bd82ba42341dba1bda5426e45c4c646db604c9ac422156eaa2b9ef26194f9
size 46265993

13
selfdrive/modeld/parse_model_outputs.py
Unescape View File

@ -88,6 +88,12 @@ class Parser:
self.parse_mdn('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('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
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))
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('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
self.parse_binary_crossentropy('meta', outs)
return outs
@ -95,17 +101,10 @@ class Parser:
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
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))
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('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_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))
if 'lat_planner_solution' in outs:
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
if 'desired_curvature' in outs:
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
return outs

Write Preview
Loading…
Cancel
Save