diff --git a/cereal b/cereal index 20b65eeb1f..c2adb4f7cf 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 20b65eeb1f6c580cdd7d63e53639f4fc48bc2f56 +Subproject commit c2adb4f7cf30e53735ee43cc3a8a3698c5410819 diff --git a/release/files_common b/release/files_common index 71c096716b..4784b369e7 100644 --- a/release/files_common +++ b/release/files_common @@ -142,7 +142,6 @@ selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_torque.py selfdrive/controls/lib/latcontrol_pid.py selfdrive/controls/lib/latcontrol.py -selfdrive/controls/lib/lateral_planner.py selfdrive/controls/lib/longcontrol.py selfdrive/controls/lib/longitudinal_planner.py selfdrive/controls/lib/pid.py diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index fa68313e86..c1a30094c4 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -207,3 +207,15 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): CP.fuzzyFingerprint = not exact_match return CarInterface(CP, CarController, CarState), CP + +def write_car_param(fingerprint="mock"): + params = Params() + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_non_essential_params(fingerprint) + params.put("CarParams", CP.to_bytes()) + +def get_demo_car_params(): + fingerprint="mock" + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_non_essential_params(fingerprint) + return CP diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 44b198a421..59657c5437 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -125,7 +125,6 @@ class CarInterfaceBase(ABC): @staticmethod def get_steer_feedforward_default(desired_angle, v_ego): # Proportional to realigning tire momentum: lateral acceleration. - # TODO: something with lateralPlan.curvatureRates return desired_angle * (v_ego**2) def get_steer_feedforward_function(self): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a25b23469f..2aae956090 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,8 +17,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_short_branch from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can -from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -32,6 +31,7 @@ from openpilot.system.hardware import HARDWARE SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 +CAMERA_OFFSET = 0.04 REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -41,9 +41,9 @@ IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState PandaType = log.PandaState.PandaType -Desire = log.LateralPlan.Desire -LaneChangeState = log.LateralPlan.LaneChangeState -LaneChangeDirection = log.LateralPlan.LaneChangeDirection +Desire = log.Desire +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel @@ -77,7 +77,7 @@ class Controls: if SIMULATION: ignore += ['driverCameraState', 'managerState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ]) @@ -288,8 +288,8 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: - direction = self.sm['lateralPlan'].laneChangeDirection + if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: + direction = self.sm['modelV2'].meta.laneChangeDirection if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) @@ -298,7 +298,7 @@ class Controls: self.events.add(EventName.preLaneChangeLeft) else: self.events.add(EventName.preLaneChangeRight) - elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting, + elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) @@ -370,8 +370,6 @@ class Controls: self.logged_comm_issue = None if not (self.CP.notCar and self.joystick_mode): - if not self.sm['lateralPlan'].mpcSolutionValid: - self.events.add(EventName.plannerError) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: @@ -572,8 +570,8 @@ class Controls: self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) - lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] + model_v2 = self.sm['modelV2'] CC = car.CarControl.new_message() CC.enabled = self.enabled @@ -588,9 +586,9 @@ class Controls: actuators.longControlState = self.LoC.long_control_state # Enable blinkers while lane changing - if self.sm['lateralPlan'].laneChangeState != LaneChangeState.off: - CC.leftBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.left - CC.rightBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.right + if model_v2.meta.laneChangeState != LaneChangeState.off: + CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left + CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame @@ -609,11 +607,11 @@ class Controls: actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures) + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) + actuators.curvature = self.desired_curvature actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited, self.desired_curvature, self.sm['liveLocationKalman']) - actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -651,7 +649,8 @@ class Controls: if undershooting and turning and good_speed and max_torque: lac_log.active and self.events.add(EventName.steerSaturated) elif lac_log.saturated: - dpath_points = lat_plan.dPathPoints + # TODO probably should not use dpath_points but curvature + dpath_points = model_v2.position.y if len(dpath_points): # Check if we deviated from the path # TODO use desired vs actual curvature @@ -777,7 +776,7 @@ class Controls: controlsState.alertSound = current_alert.audible_alert controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] - controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] controlsState.enabled = self.enabled controlsState.active = self.active controlsState.curvature = curvature diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d538035070..90b6858649 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -2,30 +2,30 @@ from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL -LaneChangeState = log.LateralPlan.LaneChangeState -LaneChangeDirection = log.LateralPlan.LaneChangeDirection +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. DESIRES = { LaneChangeDirection.none: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.none, + LaneChangeState.laneChangeFinishing: log.Desire.none, }, LaneChangeDirection.left: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft, }, LaneChangeDirection.right: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight, }, } @@ -38,7 +38,7 @@ class DesireHelper: self.lane_change_ll_prob = 1.0 self.keep_pulse_timer = 0.0 self.prev_one_blinker = False - self.desire = log.LateralPlan.Desire.none + self.desire = log.Desire.none def update(self, carstate, lateral_active, lane_change_prob): v_ego = carstate.vEgo @@ -110,5 +110,5 @@ class DesireHelper: self.keep_pulse_timer += DT_MDL if self.keep_pulse_timer > 1.0: self.keep_pulse_timer = 0.0 - elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): - self.desire = log.LateralPlan.Desire.none + elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): + self.desire = log.Desire.none diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9a5d1779fc..961b8069d4 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,7 +3,7 @@ import math from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.realtime import DT_MDL +from openpilot.common.realtime import DT_MDL, DT_CTRL from openpilot.selfdrive.modeld.constants import ModelConstants # WARNING: this value was determined based on the model's training distribution, @@ -163,21 +163,27 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures): +def clip_curvature(v_ego, prev_curvature, new_curvature): + v_ego = max(MIN_SPEED, v_ego) + max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + safe_desired_curvature = clip(new_curvature, + prev_curvature - max_curvature_rate * DT_CTRL, + prev_curvature + max_curvature_rate * DT_CTRL) + return safe_desired_curvature + + +def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N v_ego = max(MIN_SPEED, v_ego) - # TODO this needs more thought, use .2s extra for now to estimate other delays - delay = CP.steerActuatorDelay + .2 - # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature current_curvature_desired = curvatures[0] - psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis) - average_curvature_desired = psi / (v_ego * delay) + psi = interp(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * steer_delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired # This is the "desired rate of the setpoint" not an actual desired rate diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 79bead3ac8..7df41927cf 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -912,14 +912,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"), }, - # For planning the trajectory Model Predictive Control (MPC) is used. This is - # an optimization algorithm that is not guaranteed to find a feasible solution. - # If no solution is found or the solution has a very high cost this alert is thrown. - EventName.plannerError: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Planner Solution Error"), - ET.NO_ENTRY: NoEntryAlert("Planner Solution Error"), - }, - # When the relay in the harness box opens the CAN bus between the LKAS camera # and the rest of the car is separated. When messages from the LKAS camera # are received on the car side this usually means the relay hasn't opened correctly diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py deleted file mode 100644 index 2417eb3c68..0000000000 --- a/selfdrive/controls/lib/lateral_planner.py +++ /dev/null @@ -1,74 +0,0 @@ -import numpy as np -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error -from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -import cereal.messaging as messaging -from cereal import log - -TRAJECTORY_SIZE = 33 -CAMERA_OFFSET = 0.04 - -class LateralPlanner: - def __init__(self, CP, debug=False): - self.DH = DesireHelper() - - # Vehicle model parameters used to calculate lateral movement of car - self.factor1 = CP.wheelbase - CP.centerToFront - self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear) - self.last_cloudlog_t = 0 - self.solution_invalid_cnt = 0 - - self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) - self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3)) - self.v_plan = np.zeros((TRAJECTORY_SIZE,)) - self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32) - self.v_ego = MIN_SPEED - self.l_lane_change_prob = 0.0 - self.r_lane_change_prob = 0.0 - - self.debug_mode = debug - - def update(self, sm): - v_ego_car = sm['carState'].vEgo - - # Parse model predictions - md = sm['modelV2'] - if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE: - self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) - self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) - car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) - self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) - self.v_ego = self.v_plan[0] - self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate]) - - # Lane change logic - desire_state = md.meta.desireState - if len(desire_state): - self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft] - self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight] - lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob - self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) - - def publish(self, sm, pm): - plan_send = messaging.new_message('lateralPlan') - plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) - - lateralPlan = plan_send.lateralPlan - lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] - lateralPlan.dPathPoints = self.path_xyz[:,1].tolist() - lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist() - - lateralPlan.curvatures = (self.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist() - lateralPlan.curvatureRates = [float(0) for _ in range(CONTROL_N-1)] # TODO: unused - - lateralPlan.mpcSolutionValid = bool(1) - lateralPlan.solverExecutionTime = 0.0 - if self.debug_mode: - lateralPlan.solverState = log.LateralPlan.SolverState.new_message() - lateralPlan.solverState.x = self.x_sol.tolist() - - lateralPlan.desire = self.DH.desire - lateralPlan.useLaneLines = False - lateralPlan.laneChangeState = self.DH.lane_change_state - lateralPlan.laneChangeDirection = self.DH.lane_change_direction - - pm.send('lateralPlan', plan_send) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 46d10ef2f5..5ab4894424 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,29 +1,19 @@ #!/usr/bin/env python3 -import os -import numpy as np from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner -from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging -def cumtrapz(x, t): - return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))]) - -def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner): - plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS) - model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS) - +def publish_ui_plan(sm, pm, longitudinal_planner): ui_send = messaging.new_message('uiPlan') ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) uiPlan = ui_send.uiPlan uiPlan.frameId = sm['modelV2'].frameId - uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist() - uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist() - uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() + uiPlan.position.x = list(sm['modelV2'].position.x) + uiPlan.position.y = list(sm['modelV2'].position.y) + uiPlan.position.z = list(sm['modelV2'].position.z) uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist() pm.send('uiPlan', ui_send) @@ -36,12 +26,8 @@ def plannerd_thread(): CP = msg cloudlog.info("plannerd got CarParams: %s", CP.carName) - debug_mode = bool(int(os.getenv("DEBUG", "0"))) - longitudinal_planner = LongitudinalPlanner(CP) - lateral_planner = LateralPlanner(CP, debug=debug_mode) - - pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan']) + pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) @@ -49,11 +35,9 @@ def plannerd_thread(): sm.update() if sm.updated['modelV2']: - lateral_planner.update(sm) - lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner) + publish_ui_plan(sm, pm, longitudinal_planner) def main(): plannerd_thread() diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index fcecb3d9d3..42561f70f0 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -54,7 +54,7 @@ def cycle_alerts(duration=200, is_metric=False): CS = car.CarState.new_message() CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016") sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState'] + cameras) pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 21a5ebca5f..f2c0248afa 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -214,7 +214,7 @@ class TorqueEstimator(ParameterEstimator): return msg -def main(): +def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) @@ -242,4 +242,8 @@ def main(): params.put_nonblocking("LiveTorqueParameters", msg.to_bytes()) if __name__ == "__main__": - main() + import argparse + parser = argparse.ArgumentParser(description='Process the --demo argument.') + parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') + args = parser.parse_args() + main(demo=args.demo) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 7434e94287..4a47571be7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from typing import Dict from cereal import log +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -45,7 +46,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, timestamp_llk: int, model_execution_time: float, - nav_enabled: bool, valid: bool) -> None: + nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 msg.valid = valid @@ -72,9 +73,14 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) # lateral planning - solution = modelV2.lateralPlannerSolution - solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] - solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] + x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] + x_sol = np.column_stack([x, y, yaw, yawRate]) + v_ego = max(MIN_SPEED, v_ego) + psis = x_sol[0:CONTROL_N, 2].tolist() + curvatures = (x_sol[0:CONTROL_N, 3]/v_ego).tolist() + + action = modelV2.action + action.desiredCurvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures) # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index 14048ec9fd..e1cef4dcc3 100755 --- a/selfdrive/modeld/modeld +++ b/selfdrive/modeld/modeld @@ -7,4 +7,4 @@ if [ -f "$DIR/libthneed.so" ]; then export LD_PRELOAD="$DIR/libthneed.so" fi -exec "$DIR/modeld.py" +exec "$DIR/modeld.py" "$@" diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 9cc238530a..0b57228a7a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -4,6 +4,7 @@ import time import pickle import numpy as np import cereal.messaging as messaging +from cereal import car, log from pathlib import Path from typing import Dict, Optional from setproctitle import setproctitle @@ -17,6 +18,8 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive import sentry +from openpilot.selfdrive.car.car_helpers import get_demo_car_params +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState @@ -93,7 +96,6 @@ class ModelState: self.inputs['traffic_convention'][:] = inputs['traffic_convention'] self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] - # self.inputs['driving_style'][:] = inputs['driving_style'] # 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"))) @@ -113,7 +115,7 @@ class ModelState: return outputs -def main(): +def main(demo=False): sentry.set_tag("daemon", PROCESS_NAME) cloudlog.bind(daemon=PROCESS_NAME) setproctitle(PROCESS_NAME) @@ -148,7 +150,7 @@ def main(): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"]) + sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) publish_state = PublishState() params = Params() @@ -162,13 +164,23 @@ def main(): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() + + if demo: + CP = get_demo_car_params() + with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: + CP = msg + cloudlog.info("plannerd got CarParams: %s", CP.carName) + # TODO this needs more thought, use .2s extra for now to estimate other delays + steer_delay = CP.steerActuatorDelay + .2 + DH = DesireHelper() + + while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: @@ -205,7 +217,8 @@ def main(): # TODO: path planner timeout? sm.update(0) - desire = sm["lateralPlan"].desire.raw + desire = DH.desire + v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId if sm.updated["liveCalibration"]: @@ -261,7 +274,6 @@ def main(): inputs:Dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, - 'driving_style': driving_style, 'nav_features': nav_features, 'nav_instructions': nav_instructions} @@ -274,7 +286,15 @@ def main(): modelv2_send = messaging.new_message('modelV2') 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, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen) + meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen) + + 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) + modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state + modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_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) @@ -285,7 +305,11 @@ def main(): if __name__ == "__main__": try: - main() + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') + args = parser.parse_args() + main(demo=args.demo) except KeyboardInterrupt: cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") except Exception: diff --git a/selfdrive/modeld/tests/test_modeld.py b/selfdrive/modeld/tests/test_modeld.py index e010f6cfd6..257a9bc878 100755 --- a/selfdrive/modeld/tests/test_modeld.py +++ b/selfdrive/modeld/tests/test_modeld.py @@ -7,6 +7,7 @@ import cereal.messaging as messaging from cereal.visionipc import VisionIpcServer, VisionStreamType from openpilot.common.transformations.camera import tici_f_frame_size from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.car_helpers import write_car_param from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state @@ -22,9 +23,10 @@ class TestModeld(unittest.TestCase): self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) self.vipc_server.start_listener() + write_car_param() self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) - self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan']) + self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration']) managed_processes['modeld'].start() self.pm.wait_for_readers_to_update("roadCameraState", 10) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index a96dd18503..97b7c7c46c 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -105,11 +105,11 @@ def nav_model_replay(lr): def model_replay(lr, frs): # modeld is using frame pairs - modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx"}) - dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx"}) + modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}) + dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"}) if not SEND_EXTRA_INPUTS: - modeld_logs = [msg for msg in modeld_logs if msg.which() not in ["liveCalibration", "lateralPlan"]] - dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() not in ["liveCalibration", "lateralPlan"]] + modeld_logs = [msg for msg in modeld_logs if msg.which() not in ["liveCalibration",]] + dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() not in ["liveCalibration",]] # initial calibration cal_msg = next(msg for msg in lr if msg.which() == "liveCalibration").as_builder() cal_msg.logMonoTime = lr[0].logMonoTime diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 1be0aaf8c3..3f65f36e18 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -ad64b6f38c1362e9d184f3fc95299284eacb56d4 +0513d29764980f512710cc2ebd7c14f91ae0351d diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index b95c75ddf7..a6b2771668 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -461,7 +461,7 @@ CONFIGS = [ proc_name="controlsd", pubs=[ "can", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", - "longitudinalPlan", "lateralPlan", "liveLocationKalman", "liveParameters", "radarState", + "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope" ], @@ -486,8 +486,8 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"], - subs=["lateralPlan", "longitudinalPlan", "uiPlan"], - ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], + subs=["longitudinalPlan", "uiPlan"], + ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("modelV2"), tolerance=NUMPY_TOLERANCE, @@ -545,7 +545,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="modeld", - pubs=["lateralPlan", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], + pubs=["roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], subs=["modelV2", "cameraOdometry"], ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(), @@ -555,6 +555,7 @@ CONFIGS = [ main_pub_drained=False, vision_pubs=["roadCameraState", "wideRoadCameraState"], ignore_alive_pubs=["wideRoadCameraState"], + init_callback=get_car_params_callback, ), ProcessConfig( proc_name="dmonitoringmodeld", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 9163bcae75..a8d0976b57 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1b981ce7f817974d4a7a28b06f01f727a5a7ea7b \ No newline at end of file +0dffa4e5634108f41d140c74052c38059038abd0 diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index bc12234c9c..0161259356 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -82,7 +82,6 @@ TIMINGS = { "carState": [2.5, 0.35], "carControl": [2.5, 0.35], "controlsState": [2.5, 0.35], - "lateralPlan": [2.5, 0.5], "longitudinalPlan": [2.5, 0.5], "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], @@ -344,7 +343,7 @@ class TestOnroad(unittest.TestCase): result += "----------------- MPC Timing ------------------\n" result += "------------------------------------------------\n" - cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] + cfgs = [("longitudinalPlan", 0.05, 0.05),] for (s, instant_max, avg_max) in cfgs: ts = [getattr(m, s).solverExecutionTime for m in self.service_msgs[s]] self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index 337bbc9974..eed2ce231b 100755 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -10,6 +10,7 @@ from typing import List import cereal.messaging as messaging from cereal.services import SERVICE_LIST +from openpilot.selfdrive.car.car_helpers import write_car_param from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.tici.power_monitor import get_power from openpilot.selfdrive.manager.process_config import managed_processes @@ -51,6 +52,7 @@ class TestPowerDraw(unittest.TestCase): def setUp(self): HARDWARE.initialize_hardware() HARDWARE.set_power_save(False) + write_car_param() # wait a bit for power save to disable time.sleep(5) diff --git a/tools/latencylogger/README.md b/tools/latencylogger/README.md index c40ec1b9ed..a961f83619 100644 --- a/tools/latencylogger/README.md +++ b/tools/latencylogger/README.md @@ -53,9 +53,7 @@ Frame ID: 1202 modelV2.modelExecutionTime 23.62649142742157 modelV2.gpuExecutionTime 0.0 plannerd - lateralPlan published 66.915049 longitudinalPlan published 69.715999 - lateralPlan.solverExecutionTime 0.8170719956979156 longitudinalPlan.solverExecutionTime 0.5619999719783664 controlsd Data sampled 70.217763 diff --git a/tools/latencylogger/latency_logger.py b/tools/latencylogger/latency_logger.py index b561b65105..19c0a86bf4 100755 --- a/tools/latencylogger/latency_logger.py +++ b/tools/latencylogger/latency_logger.py @@ -13,13 +13,11 @@ from openpilot.tools.lib.logreader import LogReader DEMO_ROUTE = "9f583b1d93915c31|2022-05-18--10-49-51--0" SERVICES = ['camerad', 'modeld', 'plannerd', 'controlsd', 'boardd'] -# Retrieve controlsd frameId from lateralPlan, mismatch with longitudinalPlan will be ignored MONOTIME_KEYS = ['modelMonoTime', 'lateralPlanMonoTime'] MSGQ_TO_SERVICE = { 'roadCameraState': 'camerad', 'wideRoadCameraState': 'camerad', 'modelV2': 'modeld', - 'lateralPlan': 'plannerd', 'longitudinalPlan': 'plannerd', 'sendcan': 'controlsd', 'controlsState': 'controlsd' diff --git a/tools/replay/ui.py b/tools/replay/ui.py index e47aa0416e..7c95a75f8b 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -55,7 +55,7 @@ def ui_thread(addr): top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8) sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', - 'liveTracks', 'modelV2', 'liveParameters', 'lateralPlan'], addr=addr) + 'liveTracks', 'modelV2', 'liveParameters'], addr=addr) img = np.zeros((480, 640, 3), dtype='uint8') imgff = None diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index 06523497a5..f804b328de 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -51,8 +51,8 @@ class SteeringAccuracyTool: standstill = sm['carState'].standstill steer_limited = abs(sm['carControl'].actuators.steer - sm['carControl'].actuatorsOutput.steer) > 1e-2 overriding = sm['carState'].steeringPressed - changing_lanes = sm['lateralPlan'].laneChangeState != 0 - d_path_points = sm['lateralPlan'].dPathPoints + changing_lanes = sm['modelV2'].meta.laneChangeState != 0 + model_points = sm['modelV2'].position.y # must be engaged, not at standstill, not overriding steering, and not changing lanes if active and not standstill and not overriding and not changing_lanes: self.cnt += 1 @@ -75,8 +75,8 @@ class SteeringAccuracyTool: self.speed_group_stats[group][angle_abs]["cnt"] += 1 self.speed_group_stats[group][angle_abs]["err"] += angle_error self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) - if len(d_path_points): - self.speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0]) + if len(model_points): + self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0]) if steer_limited: self.speed_group_stats[group][angle_abs]["limited"] += 1 if control_state.saturated: @@ -138,10 +138,10 @@ if __name__ == "__main__": sm['carControl'] = msg.carControl elif msg.which() == 'controlsState': sm['controlsState'] = msg.controlsState - elif msg.which() == 'lateralPlan': - sm['lateralPlan'] = msg.lateralPlan + elif msg.which() == 'modelV2': + sm['modelV2'] = msg.modelV2 - if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'lateralPlan' in sm: + if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'modelV2' in sm: tool.update(sm) else: @@ -150,7 +150,7 @@ if __name__ == "__main__": messaging.context = messaging.Context() carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) - sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr) + sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'modelV2'], addr=args.addr) time.sleep(1) # Make sure all submaster data is available before going further print("waiting for messages...")