mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-12 11:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Re implementing lateral_planner for sp specific stuff

Browse Source 
Deleting lateral_planner.py as done on master

Deprecating lateralPlanSP
This commit is contained in:
DevTekVE
2024-01-26 10:17:22 +01:00
parent 1253cb9819
commit fdf97f4ac7
7 changed files with 52 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: e14ebdc8f9...e7f607011a
selfdrive/controls/controlsd.py
+2 -2
View File
@@ -79,7 +79,7 @@ class Controls:
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick', 'longitudinalPlanSP', 'lateralPlanSP'] + self.camera_packets + self.sensor_packets,
'testJoystick', 'longitudinalPlanSP', 'lateralPlanSPDEPRECATED'] + self.camera_packets + self.sensor_packets,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ])
if CI is None:
@@ -294,7 +294,7 @@ class Controls:
lane_change_set_timer = int(self.params.get("AutoLaneChangeTimer", encoding="utf8"))
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
lc_prev = self.sm['lateralPlanSP'].laneChangePrev
lc_prev = self.sm['lateralPlanSPDEPRECATED'].laneChangePrev
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
selfdrive/controls/lib/lateral_planner.py
-86
View File
@@ -1,86 +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.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
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)
plan_sp_send = messaging.new_message('lateralPlanSP')
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
lateralPlanSP = plan_sp_send.lateralPlanSP
lateralPlanSP.laneChangePrev = self.DH.prev_lane_change
lateralPlanSP.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
lateralPlanSP.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
pm.send('lateralPlanSP', plan_sp_send)
selfdrive/controls/lib/lateral_planner_sp.py
+41
View File
@@ -0,0 +1,41 @@
import numpy as np
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 LateralPlannerSP:
"""Library is a stop-gap solution until we do a better assesment
whether we need or not this info published this way.
MR269."""
def __init__(self):
self.DH = DesireHelper()
self.l_lane_change_prob = 0.0
self.r_lane_change_prob = 0.0
self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
def update(self, sm):
md = sm['modelV2']
# 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_sp_send = messaging.new_message('lateralPlanSPDEPRECATED')
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
lateralPlanSPDEPRECATED = plan_sp_send.lateralPlanSPDEPRECATED
lateralPlanSPDEPRECATED.laneChangePrev = self.DH.prev_lane_change
lateralPlanSPDEPRECATED.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
lateralPlanSPDEPRECATED.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
pm.send('lateralPlanSPDEPRECATED', plan_sp_send)
selfdrive/controls/lib/vision_turn_controller.py
+1 -1
View File
@@ -153,7 +153,7 @@ class VisionTurnController():
# Get path polynomial approximation for curvature estimation from model data.
path_poly = None
model_data = sm['modelV2'] if sm.valid.get('modelV2', False) else None
lat_planner_data = sm['lateralPlanSP'] if sm.valid.get('lateralPlanSP', False) else None
lat_planner_data = sm['lateralPlanSPDEPRECATED'] if sm.valid.get('lateralPlanSPDEPRECATED', False) else None
# 1. When the probability of lanes is good enough, compute polynomial from lanes as they are way more stable
# on current mode than drving path.
selfdrive/controls/plannerd.py
+6 -2
View File
@@ -4,6 +4,7 @@ from openpilot.common.params import Params
from openpilot.common.realtime import Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.lib.lateral_planner_sp import LateralPlannerSP
import cereal.messaging as messaging
def publish_ui_plan(sm, pm, longitudinal_planner):
@@ -26,15 +27,18 @@ def plannerd_thread():
CP = msg
cloudlog.info("plannerd got CarParams: %s", CP.carName)
lateral_planner_sp = LateralPlannerSP()
longitudinal_planner = LongitudinalPlanner(CP)
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP', 'lateralPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlanSP', 'liveMapDataSP'],
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP', 'lateralPlanSPDEPRECATED'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlanSPDEPRECATED', 'liveMapDataSP'],
poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
while True:
sm.update()
if sm.updated['modelV2']:
lateral_planner_sp.update(sm)
lateral_planner_sp.publish(sm, pm)
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
publish_ui_plan(sm, pm, longitudinal_planner)
selfdrive/debug/cycle_alerts.py
+1 -1
View File
@@ -55,7 +55,7 @@ def cycle_alerts(duration=200, is_metric=False):
CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016")
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'longitudinalPlanSP', 'lateralPlanSP',
'managerState', 'longitudinalPlanSP', 'lateralPlanSPDEPRECATED',
'driverMonitoringStateSP'] + cameras)
pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])