diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index b424d26..92704b8 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -120,6 +120,11 @@ class CarrotPlanner: self.jerk_factor = 1.0 self.jerk_factor_apply = 1.0 + self.activeCarrot = 0 + self.xDistToTurn = 0 + self.atcType = "" + self.atc_active = False + def _params_update(self): self.frame += 1 @@ -217,7 +222,6 @@ class CarrotPlanner: stop_x = self.xStopFilter2.process(stop_x) return stop_x - def check_model_stopping(self, v, v_ego, a_ego, model_x, y, d_rel): v_ego_kph = v_ego * CV.MS_TO_KPH model_v = self.vFilter.process(v[-1]) @@ -264,7 +268,7 @@ class CarrotPlanner: def _update_carrot_man(self, sm, v_ego_kph, v_cruise_kph): if sm.alive['carrotMan']: carrot_man = sm['carrotMan'] - atc_turn_left = carrot_man.atcType == "turn left" + atc_turn_left = carrot_man.atcType in ["turn left", "atc left"] trigger_start = self.carrot_staty_stop = False if atc_turn_left or sm['carState'].leftBlinker: if self.trafficState_carrot == 1 and carrot_man.trafficState == 3: # red -> left triggered @@ -284,6 +288,11 @@ class CarrotPlanner: self.xState = XState.e2eCruise self.traffic_starting_count = 10.0 / DT_MDL + self.activeCarrot = carrot_man.activeCarrot + self.xDistToTurn = carrot_man.xDistToTurn + self.atc_active = self.activeCarrot > 1 and 0 < self.xDistToTurn < 100 + self.atcType = carrot_man.atcType + v_cruise_kph = min(v_cruise_kph, carrot_man.desiredSpeed) return v_cruise_kph @@ -434,7 +443,10 @@ class CarrotPlanner: else: self.xState = XState.e2eCruise - if self.trafficState in [TrafficState.off, TrafficState.green] or self.xState not in [XState.e2eStop, XState.e2eStopped]: + if self.atc_active and self.atcType in ["turn left", "turn right", "atc left", "atc right"] and self.xState not in [XState.e2eStop, XState.e2eStopped, XState.lead]: + self.xState = XState.e2ePrepare + + elif self.trafficState in [TrafficState.off, TrafficState.green] or self.xState not in [XState.e2eStop, XState.e2eStopped]: stop_model_x = 1000.0 if self.user_stop_distance >= 0: diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index 6062cbc..726f3b4 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/selfdrive/carrot/carrot_man.py @@ -360,7 +360,7 @@ class CarrotMan: distances.append(distance) # Apply acceleration limits in reverse to adjust speeds - accel_limit = self.carrot_serv.autoNaviSpeedDecelRate * 0.9 # m/s^2, 설정된값의 90%를 사용하여, 좀더 낮은속도로 진입하도록 유도 + accel_limit = self.carrot_serv.autoNaviSpeedDecelRate # m/s^2 accel_limit_kmh = accel_limit * 3.6 # Convert to km/h per second out_speeds = [0] * len(speeds) out_speeds[-1] = speeds[-1] # Set the last speed as the initial value @@ -1300,7 +1300,7 @@ class CarrotServ: if check_steer: self.atc_activate_count = max(0, self.atc_activate_count + 1) if atc_type in ["turn left", "turn right"] and x_dist_to_turn > start_turn_dist: - atc_type = "fork left" if atc_type == "turn left" else "fork right" + atc_type = "atc left" if atc_type == "turn left" else "atc right" if self.autoTurnMapChange > 0 and check_steer: #print(f"x_dist_to_turn: {x_dist_to_turn}, atc_start_dist: {atc_start_dist}") @@ -1315,9 +1315,9 @@ class CarrotServ: if not self.atc_paused: steering_pressed = sm["carState"].steeringPressed steering_torque = sm["carState"].steeringTorque - if steering_pressed and steering_torque < 0 and atc_type == "fork left": + if steering_pressed and steering_torque < 0 and atc_type in ["fork left", "atc left"]: self.atc_paused = True - elif steering_pressed and steering_torque > 0 and atc_type == "fork right": + elif steering_pressed and steering_torque > 0 and atc_type in ["fork right", "atc right"]: self.atc_paused = True else: self.atc_paused = False diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 42e2ef7..cf38702 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -12,6 +12,11 @@ TurnDirection = log.Desire LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. +BLINKER_NONE = 0 +BLINKER_LEFT = 1 +BLINKER_RIGHT = 2 +BLINKER_BOTH = 3 + DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.Desire.none, @@ -92,7 +97,7 @@ class DesireHelper: self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.keep_pulse_timer = 0.0 - self.prev_one_blinker = False + self.prev_desire_enabled = False self.desire = log.Desire.none self.turn_direction = TurnDirection.none self.enable_turn_desires = True @@ -122,45 +127,9 @@ class DesireHelper: self.object_detected_count = 0 self.laneChangeNeedTorque = False + self.driver_blinker_state = BLINKER_NONE - def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState): - - self.laneChangeNeedTorque = self.params.get_bool("LaneChangeNeedTorque") - - - - v_ego = carstate.vEgo - #one_blinker = carstate.leftBlinker != carstate.rightBlinker - leftBlinker = carstate.leftBlinker - rightBlinker = carstate.rightBlinker - one_blinker = leftBlinker != rightBlinker - driver_one_blinker = one_blinker - - below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - - if not leftBlinker and not rightBlinker: - atc_type = carrotMan.atcType - if atc_type in ["turn left", "turn right"]: - if self.atc_active != 2: - below_lane_change_speed = True - self.lane_change_timer = 0.0 - leftBlinker = True if atc_type == "turn left" else False - rightBlinker = not leftBlinker - self.atc_active = 1 - self.blinker_ignore = False - elif atc_type in ["fork left", "fork right"]: - if self.atc_active != 2: - below_lane_change_speed = False - leftBlinker = True if atc_type == "fork left" else False - rightBlinker = not leftBlinker - self.atc_active = 1 - else: - self.atc_active = 0 - one_blinker = leftBlinker != rightBlinker - if not one_blinker: - self.blinker_ignore = False - one_blinker &= not self.blinker_ignore - + def check_lane_state(self, modeldata): self.lane_width_left, self.distance_to_road_edge_left, self.distance_to_road_edge_left_far, lane_prob_left = calculate_lane_width(modeldata.laneLines[0], modeldata.laneLineProbs[0], modeldata.laneLines[1], modeldata.roadEdges[0]) self.lane_width_right, self.distance_to_road_edge_right, self.distance_to_road_edge_right_far, lane_prob_right = calculate_lane_width(modeldata.laneLines[3], modeldata.laneLineProbs[3], @@ -177,13 +146,64 @@ class DesireHelper: self.available_right_lane = self.lane_width_right_count.counter > available_count self.available_left_edge = self.road_edge_left_count.counter > available_count and self.distance_to_road_edge_left_far > min_lane_width self.available_right_edge = self.road_edge_right_count.counter > available_count and self.distance_to_road_edge_right_far > min_lane_width + - if one_blinker: - lane_available = self.available_left_lane if leftBlinker else self.available_right_lane - edge_available = self.available_left_edge if leftBlinker else self.available_right_edge - lane_appeared = self.lane_exist_left_count.counter == int(0.2 / DT_MDL) if leftBlinker else self.lane_exist_right_count.counter == int(0.2 / DT_MDL) + def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState): - radar = radarState.leadLeft if leftBlinker else radarState.leadRight + self.laneChangeNeedTorque = self.params.get_bool("LaneChangeNeedTorque") + + + + v_ego = carstate.vEgo + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + #### check driver's blinker state + driver_blinker_state = carstate.leftBlinker * 1 + carstate.rightBlinker * 2 + driver_blinker_changed = driver_blinker_state != self.driver_blinker_state + self.driver_blinker_state = driver_blinker_state + driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT] + + ##### check ATC's blinker state + atc_type = carrotMan.atcType + atc_blinker_state = BLINKER_NONE + if atc_type in ["turn left", "turn right"]: + if self.atc_active != 2: + below_lane_change_speed = True + self.lane_change_timer = 0.0 + atc_blinker_state = BLINKER_LEFT if atc_type == "turn left" else BLINKER_RIGHT + self.atc_active = 1 + self.blinker_ignore = False + elif atc_type in ["fork left", "fork right", "atc left", "atc right"]: + if self.atc_active != 2: + below_lane_change_speed = False + atc_blinker_state = BLINKER_LEFT if atc_type in ["fork left", "atc left"] else BLINKER_RIGHT + self.atc_active = 1 + else: + self.atc_active = 0 + if driver_blinker_state != BLINKER_NONE and atc_blinker_state != BLINKER_NONE and driver_blinker_state != atc_blinker_state: + atc_blinker_state = BLINKER_NONE + self.atc_active = 2 + atc_desire_enabled = atc_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT] + + if driver_blinker_state == BLINKER_NONE: + self.blinker_ignore = False + if self.blinker_ignore: + driver_blinker_state = BLINKER_NONE + atc_blinker_state = BLINKER_NONE + driver_desire_enabled = False + + desire_enabled = driver_desire_enabled or atc_desire_enabled + blinker_state = driver_blinker_state if driver_desire_enabled else atc_blinker_state + + ##### check lane state + self.check_lane_state(modeldata) + + if desire_enabled: + lane_available = self.available_left_lane if blinker_state == BLINKER_LEFT else self.available_right_lane + edge_available = self.available_left_edge if blinker_state == BLINKER_LEFT else self.available_right_edge + lane_appeared = self.lane_exist_left_count.counter == int(0.2 / DT_MDL) if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter == int(0.2 / DT_MDL) + + radar = radarState.leadLeft if blinker_state == BLINKER_LEFT else radarState.leadRight side_object_dist = radar.dRel + radar.vLead * 4.0 if radar.status else 255 object_detected = side_object_dist < v_ego * 3.0 self.object_detected_count = max(1, self.object_detected_count + 1) if object_detected else min(-1, self.object_detected_count - 1) @@ -194,7 +214,7 @@ class DesireHelper: lane_appeared = False self.object_detected_count = 0 - auto_lane_change_blocked = leftBlinker + auto_lane_change_blocked = blinker_state == BLINKER_LEFT lane_availabled = not self.lane_available_last and lane_available edge_availabled = not self.edge_available_last and edge_available side_object_detected = self.object_detected_count > -0.3 / DT_MDL @@ -204,14 +224,14 @@ class DesireHelper: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.turn_direction = TurnDirection.none - elif one_blinker and below_lane_change_speed and not carstate.standstill and self.enable_turn_desires: + elif desire_enabled and below_lane_change_speed and not carstate.standstill and self.enable_turn_desires: self.lane_change_state = LaneChangeState.off - self.turn_direction = TurnDirection.turnLeft if leftBlinker else TurnDirection.turnRight + self.turn_direction = TurnDirection.turnLeft if blinker_state == BLINKER_LEFT else TurnDirection.turnRight self.lane_change_direction = self.turn_direction #LaneChangeDirection.none else: self.turn_direction = TurnDirection.none # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + if self.lane_change_state == LaneChangeState.off and desire_enabled and not self.prev_desire_enabled and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 @@ -219,7 +239,7 @@ class DesireHelper: elif self.lane_change_state == LaneChangeState.preLaneChange: # Set lane change direction self.lane_change_direction = LaneChangeDirection.left if \ - leftBlinker else LaneChangeDirection.right + blinker_state == BLINKER_LEFT else LaneChangeDirection.right torque_applied = carstate.steeringPressed and \ ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or @@ -228,7 +248,7 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - if not one_blinker or below_lane_change_speed: + if not desire_enabled or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none elif not blindspot_detected: @@ -236,7 +256,7 @@ class DesireHelper: if torque_applied: self.lane_change_state = LaneChangeState.laneChangeStarting # 운전자가 깜박이켠경우는 바로 차선변경 시작 - elif driver_one_blinker and lane_available: + elif driver_desire_enabled and lane_available: self.lane_change_state = LaneChangeState.laneChangeStarting # ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작 # lane_appeared: 차선이 생기는건 안함.. 위험. @@ -259,7 +279,7 @@ class DesireHelper: if self.lane_change_ll_prob > 0.99: self.lane_change_direction = LaneChangeDirection.none - if one_blinker: + if desire_enabled: self.lane_change_state = LaneChangeState.preLaneChange else: self.lane_change_state = LaneChangeState.off @@ -273,9 +293,9 @@ class DesireHelper: self.lane_available_last = lane_available self.edge_available_last = edge_available - self.prev_one_blinker = one_blinker + self.prev_desire_enabled = desire_enabled steering_pressed = carstate.steeringPressed and \ - ((carstate.steeringTorque < 0 and leftBlinker) or (carstate.steeringTorque > 0 and rightBlinker)) + ((carstate.steeringTorque < 0 and blinker_state == BLINKER_LEFT) or (carstate.steeringTorque > 0 and blinker_state == BLINKER_RIGHT)) if steering_pressed and self.lane_change_state != LaneChangeState.off: self.lane_change_direction = LaneChangeDirection.none self.lane_change_state = LaneChangeState.off @@ -287,6 +307,7 @@ class DesireHelper: else: self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + #print(f"desire = {self.desire}") #self.desireLog = f"desire = {self.desire}" self.desireLog = f"rlane={self.distance_to_road_edge_right:.1f},{self.distance_to_road_edge_right_far:.1f}" diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 2f4dcbe..8153d12 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -14,7 +14,6 @@ from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.lane_planner_2 import LanePlanner from collections import deque - TRAJECTORY_SIZE = 33 CAMERA_OFFSET = 0.04 @@ -80,7 +79,7 @@ class LateralPlanner: self.x0 = x0 self.lat_mpc.reset(x0=self.x0) - def update(self, sm): + def update(self, sm, carrot): global LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST self.readParams -= 1 if self.readParams <= 0: @@ -130,22 +129,19 @@ class LateralPlanner: # Turn off lanes during lane change #if self.DH.desire == log.Desire.laneChangeRight or self.DH.desire == log.Desire.laneChangeLeft: - activeCarrot = sm['carrotMan'].activeCarrot - xDistToTurn = sm['carrotMan'].xDistToTurn - atc_activate = activeCarrot > 1 and 0 < xDistToTurn < 250 - if md.meta.desire != log.Desire.none or atc_activate: + + if md.meta.desire != log.Desire.none or carrot.atc_active: self.LP.lane_change_multiplier = 0.0 #md.meta.laneChangeProb else: self.LP.lane_change_multiplier = 1.0 lateral_motion_cost = self.lateralMotionCost path_cost = self.lateralPathCost - atc_type = sm['carrotMan'].atcType - if atc_activate: - if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1): + if carrot.atc_active: + if carrot.atcType == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1): lateral_motion_cost = self.lateralMotionCostTurn path_cost = self.lateralPathCostTurn - elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1): + elif carrot.atcType == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1): lateral_motion_cost = self.lateralMotionCostTurn path_cost = self.lateralPathCostTurn @@ -202,7 +198,7 @@ class LateralPlanner: else: self.solution_invalid_cnt = 0 - def publish(self, sm, pm): + def publish(self, sm, pm, carrot): plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('lateralPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9a458c3..31908d1 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,7 +15,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.carrot.carrot_functions import CarrotPlanner LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -2.0 #-1.2 @@ -87,7 +86,6 @@ class LongitudinalPlanner: self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - self.carrot = CarrotPlanner() self.vCluRatio = 1.0 self.v_cruise_kph = 0.0 @@ -112,7 +110,7 @@ class LongitudinalPlanner: throttle_prob = 1.0 return x, v, a, j, throttle_prob - def update(self, sm): + def update(self, sm, carrot): self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: @@ -123,7 +121,8 @@ class LongitudinalPlanner: v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) - self.v_cruise_kph = self.carrot.update(sm, v_cruise_kph) + self.v_cruise_kph = carrot.update(sm, v_cruise_kph) + self.mpc.mode = carrot.mode v_cruise = self.v_cruise_kph * CV.KPH_TO_MS vCluRatio = sm['carState'].vCluRatio @@ -139,14 +138,14 @@ class LongitudinalPlanner: # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled # PCM cruise speed may be updated a few cycles later, check if initialized - reset_state = reset_state or not v_cruise_initialized or self.carrot.soft_hold_active + reset_state = reset_state or not v_cruise_initialized or carrot.soft_hold_active # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) if self.mpc.mode == 'acc': #accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] - accel_limits = [A_CRUISE_MIN, self.carrot.get_carrot_accel(v_ego)] + accel_limits = [A_CRUISE_MIN, carrot.get_carrot_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: @@ -180,10 +179,10 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = self.carrot.jerk_factor_apply) + self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(self.carrot, reset_state, sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) + self.mpc.update(carrot, reset_state, sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -199,7 +198,7 @@ class LongitudinalPlanner: self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - def publish(self, sm, pm): + def publish(self, sm, pm, carrot): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState']) @@ -229,12 +228,12 @@ class LongitudinalPlanner: longitudinalPlan.vTarget = float(v_target) longitudinalPlan.jTarget = float(j_target) - longitudinalPlan.xState = self.carrot.xState.value - longitudinalPlan.trafficState = self.carrot.trafficState.value + longitudinalPlan.xState = carrot.xState.value + longitudinalPlan.trafficState = carrot.trafficState.value longitudinalPlan.xTarget = self.v_cruise_kph longitudinalPlan.tFollow = float(self.mpc.t_follow) longitudinalPlan.desiredDistance = float(self.mpc.desired_distance) - longitudinalPlan.events = self.carrot.events.to_msg() - longitudinalPlan.myDrivingMode = self.carrot.myDrivingMode.value + longitudinalPlan.events = carrot.events.to_msg() + longitudinalPlan.myDrivingMode = carrot.myDrivingMode.value pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 8fb4abf..ccf0db2 100644 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging +from openpilot.selfdrive.carrot.carrot_functions import CarrotPlanner def main(): @@ -24,14 +25,15 @@ def main(): pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'lateralPlan']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'carrotMan'], poll='modelV2', ignore_avg_freq=['radarState']) + carrot = CarrotPlanner() while True: sm.update() if sm.updated['modelV2']: - lateral_planner.update(sm) - lateral_planner.publish(sm, pm) - longitudinal_planner.update(sm) - longitudinal_planner.publish(sm, pm) + lateral_planner.update(sm, carrot) + lateral_planner.publish(sm, pm, carrot) + longitudinal_planner.update(sm, carrot) + longitudinal_planner.publish(sm, pm, carrot) ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) msg = messaging.new_message('driverAssistance') diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index d7ad00f..67393b3 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -1254,8 +1254,8 @@ public: const auto car_state = sm["carState"].getCarState(); QString atc_type = QString::fromStdString(carrot_man.getAtcType()); - bool left_blinker = car_state.getLeftBlinker() || atc_type=="fork left" || atc_type =="turn left"; - bool right_blinker = car_state.getRightBlinker() || atc_type=="fork right" || atc_type =="turn right"; + bool left_blinker = car_state.getLeftBlinker() || atc_type=="fork left" || atc_type =="turn left" || atc_type == "atc left"; + bool right_blinker = car_state.getRightBlinker() || atc_type=="fork right" || atc_type =="turn right" || atc_type == "atc right"; _right_blinker = false; _left_blinker = false; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index b5a195a..c67e73f 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -172,7 +172,8 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { } else if (v_ego > 15) { wide_cam_requested = false; } - wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); + //wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); + wide_cam_requested = wide_cam_requested && s->scene.carrot_experimental_mode; } painter.beginNativePainting(); CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 67c6c05..4fa6d13 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -35,9 +35,10 @@ void ExperimentalButton::changeMode() { void ExperimentalButton::updateState(const UIState &s) { const auto cs = (*s.sm)["selfdriveState"].getSelfdriveState(); bool eng = cs.getEngageable() || cs.getEnabled(); - if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { + if ((s.scene.carrot_experimental_mode != experimental_mode) || (eng != engageable)) { engageable = eng; - experimental_mode = cs.getExperimentalMode(); + //experimental_mode = cs.getExperimentalMode(); + experimental_mode = s.scene.carrot_experimental_mode; update(); } } diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 4cb260d..a61aa7a 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -23,7 +23,8 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { } clip_region = surface_rect.adjusted(-CLIP_MARGIN, -CLIP_MARGIN, CLIP_MARGIN, CLIP_MARGIN); - experimental_mode = sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); + //experimental_mode = sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); + experimental_mode = s->scene.carrot_experimental_mode; longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); path_offset_z = sm["liveCalibration"].getLiveCalibration().getHeight()[0]; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 71416da..81c9f55 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -59,6 +59,11 @@ static void update_state(UIState *s) { } else if (!sm.allAliveAndValid({"wideRoadCameraState"})) { scene.light_sensor = -1; } + if (sm.updated("longitudinalPlan")) { + auto lp = sm["longitudinalPlan"].getLongitudinalPlan(); + scene.carrot_experimental_mode = lp.getXState() == 4; + } + scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index fb69ed3..884e6bd 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -69,6 +69,9 @@ typedef struct UIScene { int _display_time_count = 0; bool map_on_left; uint64_t started_frame; + + bool carrot_experimental_mode = false; + } UIScene; class UIState : public QObject { @@ -95,7 +98,6 @@ public: float show_brightness_ratio = 1.0; int show_brightness_timer = 20; - signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad);