diff --git a/cereal b/cereal index e14ebdc8f9..e7f607011a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e14ebdc8f9f708228730714929d29e18289ce212 +Subproject commit e7f607011a5b449a0d020a0056e48d9761a285fd diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a09acad832..cdf5a57342 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py deleted file mode 100644 index 6927312bd8..0000000000 --- a/selfdrive/controls/lib/lateral_planner.py +++ /dev/null @@ -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) diff --git a/selfdrive/controls/lib/lateral_planner_sp.py b/selfdrive/controls/lib/lateral_planner_sp.py new file mode 100644 index 0000000000..f69d37dc5f --- /dev/null +++ b/selfdrive/controls/lib/lateral_planner_sp.py @@ -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) diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index 96561ee10f..ee36d0a684 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/selfdrive/controls/lib/vision_turn_controller.py @@ -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. diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 28f2121264..9924537e67 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -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) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index d66d4033fb..a6cd6ba7ab 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -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'])