atc, e2e long test
This commit is contained in:
ajouatom
parent
5076d75a3a
commit
ba75acc217
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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}"
|
||||
|
||||
|
||||
@ -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'])
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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')
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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];
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user