mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-06 05:52:12 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update modeld action logic (#35032)

Browse Source 
* add action logic

* magic numbers

* unused

* plot accel

* more changes
This commit is contained in:
Harald Schäfer
2025-04-17 17:59:15 -07:00
committed by GitHub
parent 4f913f0cfb
commit b42ec33a63
5 changed files with 62 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/drive_helpers.py
+17 -1
View File
@@ -1,7 +1,7 @@
import numpy as np
from cereal import log
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.realtime import DT_CTRL
from openpilot.common.realtime import DT_CTRL, DT_MDL
MIN_SPEED = 1.0
CONTROL_N = 17
@@ -43,3 +43,19 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
return float(vel_err)
return 0.0
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, t_idxs, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
a_target = 0.0
should_stop = (v_target < vEgoStopping and
v_target_1sec < vEgoStopping)
return a_target, should_stop
selfdrive/controls/lib/longitudinal_planner.py
+2 -19
View File
@@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error, get_accel_from_plan
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
@@ -48,23 +48,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return [a_target[0], min(a_target[1], a_x_allowed)]
def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
if len(speeds) == CONTROL_N:
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
a_target = 0.0
should_stop = (v_target < vEgoStopping and
v_target_1sec < vEgoStopping)
return a_target, should_stop
class LongitudinalPlanner:
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
@@ -176,7 +159,7 @@ 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,
output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
for idx in range(2):
selfdrive/modeld/fill_model_msg.py
+5 -4
View File
@@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
builder.rightProb = lane_line_probs[2]
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
valid: bool) -> None:
@@ -71,7 +71,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
driving_model_data.frameIdExtra = vipc_frame_id_extra
driving_model_data.frameDropPerc = frame_drop_perc
driving_model_data.modelExecutionTime = model_execution_time
driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
driving_model_data.action = action
modelV2 = extended_msg.modelV2
modelV2.frameId = vipc_frame_id
@@ -98,8 +99,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
# poly path
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
# lateral planning
modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
# action
modelV2.action = action
# times at X_IDXS of edges and lines aren't used
LINE_T_IDXS: list[float] = []
selfdrive/modeld/modeld.py
+37 -5
View File
@@ -21,17 +21,20 @@ from opendbc.car.car_helpers import get_demo_car_params
from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import config_realtime_process
from openpilot.common.realtime import config_realtime_process, DT_MDL
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
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
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
PROCESS_NAME = "selfdrive.modeld.modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -40,6 +43,30 @@ 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
def smooth_value(val, prev_val, tau):
alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1
return alpha * val + (1 - alpha) * prev_val
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action:
plan = model_output['plan'][0]
desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
plan[:,Plan.ACCELERATION][:,0],
ModelConstants.T_IDXS,
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 = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
shouldStop=bool(should_stop))
class FrameMeta:
frame_id: int = 0
timestamp_sof: int = 0
@@ -221,7 +248,10 @@ def main(demo=False):
cloudlog.info("modeld got CarParams: %s", CP.brand)
# TODO this needs more thought, use .2s extra for now to estimate other delays
steer_delay = CP.steerActuatorDelay + .2
# TODO Move smooth seconds to action function
lat_delay = CP.steerActuatorDelay + .2 + LAT_SMOOTH_SECONDS
long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
@@ -263,7 +293,7 @@ def main(demo=False):
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.)
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
lateral_control_params = np.array([v_ego, lat_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))]
@@ -306,7 +336,9 @@ def main(demo=False):
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL)
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
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)
selfdrive/test/process_replay/model_replay.py
+1
View File
@@ -64,6 +64,7 @@ def generate_report(proposed, master, tmp, commit):
ModelV2_Plots = zl([
(lambda x: get_idx_if_non_empty(x.velocity.x, 0), "velocity.x"),
(lambda x: get_idx_if_non_empty(x.action.desiredCurvature), "desiredCurvature"),
(lambda x: get_idx_if_non_empty(x.action.desiredAcceleration), "desiredAcceleration"),
(lambda x: get_idx_if_non_empty(x.leadsV3[0].x, 0), "leadsV3.x"),
(lambda x: get_idx_if_non_empty(x.laneLines[1].y, 0), "laneLines.y"),
(lambda x: get_idx_if_non_empty(x.meta.desireState, 3), "desireState.laneChangeLeft"),