mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-27 17:42:04 +08:00
Delete lat planner (#31089)
* Initial commit * Fixup * typo * ignore lateral plan * Update cereal * Remove lateralPlan * Fix release build * Fix release build * give car params * Add carParams to include_all_types * Write car param in powerdraw test * add demo mode * Update model regf * proc replay ref commit * Try * Move enum definition * Update cereal * typo * Write car param for modeld test * Update ref * Update model ref again --------- Co-authored-by: Kacper Rączy <gfw.kra@gmail.com> old-commit-hash: e6c97c384671b448f307a7ed91416886f2186d80
This commit is contained in:
Harald Schäfer
+1
-1
Submodule cereal updated: 20b65eeb1f...c2adb4f7cf
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
|
||||
@@ -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'])
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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" "$@"
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1 +1 @@
|
||||
ad64b6f38c1362e9d184f3fc95299284eacb56d4
|
||||
0513d29764980f512710cc2ebd7c14f91ae0351d
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -1 +1 @@
|
||||
1b981ce7f817974d4a7a28b06f01f727a5a7ea7b
|
||||
0dffa4e5634108f41d140c74052c38059038abd0
|
||||
|
||||
@@ -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)}")
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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'
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
@@ -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...")
|
||||
|
||||
Reference in New Issue
Block a user