zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

fix atc, long_mpc (#134)

Browse Source
This commit is contained in:
carrot 2025-02-18 15:59:28 +09:00 committed by GitHub
parent ab38ac3869
commit 257f81e7bd
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 45 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
selfdrive/carrot/carrot_functions.py
View File

@ -290,12 +290,12 @@ class CarrotPlanner:
self.activeCarrot = carrot_man.activeCarrot
self.xDistToTurn = carrot_man.xDistToTurn
self.atc_active = self.activeCarrot > 1 and 0 < self.xDistToTurn < 100
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
return v_cruise_kph, atc_active
def cruise_eco_control(self, v_ego_kph, v_cruise_kph):
v_cruise_kph_apply = v_cruise_kph
@ -353,7 +353,12 @@ class CarrotPlanner:
self.drivingModeDetector.update_data(v_ego_kph, vLead, carstate.aEgo, aLead, dRel)
v_cruise_kph = self.cruise_eco_control(v_ego_cluster_kph, v_cruise_kph)
v_cruise_kph = self._update_carrot_man(sm, v_ego_kph, v_cruise_kph)
v_cruise_kph, atc_active = self._update_carrot_man(sm, v_ego_kph, v_cruise_kph)
if atc_active and not self.atc_active and self.xState not in [XState.e2eStop, XState.e2eStopped, XState.lead]:
if self.atcType in ["turn left", "turn right", "atc left", "atc right"]:
self.xState = XState.e2ePrepare
self.atc_active = atc_active
v_cruise = v_cruise_kph * CV.KPH_TO_MS
if vCluRatio > 0.5:
@ -427,6 +432,9 @@ class CarrotPlanner:
elif self.xState == XState.e2ePrepare:
if lead_detected:
self.xState = XState.lead
elif self.atc_active:
if carstate.gasPressed:
self.xState = XState.e2eCruise
elif v_ego_kph < 5.0 and self.trafficState != TrafficState.green:
self.xState = XState.e2eStop
self.actual_stop_distance = 5.0 #2.0
@ -443,10 +451,7 @@ class CarrotPlanner:
else:
self.xState = XState.e2eCruise
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]:
if 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:

6
selfdrive/carrot/carrot_man.py
View File

@ -358,7 +358,8 @@ class CarrotMan:
speed = max(speed, self.carrot_serv.nRoadLimitSpeed)
speeds.append(speed)
distances.append(distance)
#print(f"curvatures= {[round(s, 4) for s in curvatures]}")
#print(f"speeds= {[round(s, 1) for s in speeds]}")
# Apply acceleration limits in reverse to adjust speeds
accel_limit = self.carrot_serv.autoNaviSpeedDecelRate # m/s^2
accel_limit_kmh = accel_limit * 3.6 # Convert to km/h per second
@ -393,6 +394,7 @@ class CarrotMan:
#distance_advance = self.sm['carState'].vEgo * 3.0 # Advance distance by 3.0 seconds
#out_speed = interp(distance_advance, distances, out_speeds)
out_speed = out_speeds[0]
#print(f"out_speeds= {[round(s, 1) for s in out_speeds]}")
else:
resampled_points = []
curvatures = []
@ -1456,7 +1458,7 @@ class CarrotServ:
source = "gas"
desired_speed = self.gas_override_speed
self.debugText = ""#f"desired={desired_speed:.1f},{source},g={self.gas_override_speed:.0f}"
self.debugText = f"route={route_speed:.1f}"#f"desired={desired_speed:.1f},{source},g={self.gas_override_speed:.0f}"
left_spd_sec = 100
left_tbt_sec = 100

2
selfdrive/controls/controlsd.py
View File

@ -134,7 +134,7 @@ class Controls:
turn_state = desire_state[1] + desire_state[2]
else:
turn_state = 0.0
desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay if turn_state < 0.1 else model_turn_delay)
desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay if turn_state < 0.01 else model_turn_delay)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
else:
if self.lanefull_mode_enabled:

10
selfdrive/controls/lib/desire_helper.py
View File

@ -128,6 +128,7 @@ class DesireHelper:
self.laneChangeNeedTorque = False
self.driver_blinker_state = BLINKER_NONE
self.atc_type = ""
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],
@ -192,6 +193,11 @@ class DesireHelper:
atc_blinker_state = BLINKER_NONE
driver_desire_enabled = False
if self.atc_type != atc_type:
atc_desire_enabled = False
self.atc_type = atc_type
desire_enabled = driver_desire_enabled or atc_desire_enabled
blinker_state = driver_blinker_state if driver_desire_enabled else atc_blinker_state
@ -214,10 +220,11 @@ class DesireHelper:
lane_appeared = False
self.object_detected_count = 0
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
auto_lane_change_blocked = (atc_blinker_state == BLINKER_LEFT) and (driver_blinker_state != BLINKER_LEFT)
auto_lane_change_available = not auto_lane_change_blocked and lane_availabled and edge_availabled and not side_object_detected
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
@ -228,6 +235,7 @@ class DesireHelper:
self.lane_change_state = LaneChangeState.off
self.turn_direction = TurnDirection.turnLeft if blinker_state == BLINKER_LEFT else TurnDirection.turnRight
self.lane_change_direction = self.turn_direction #LaneChangeDirection.none
desire_enabled = False
else:
self.turn_direction = TurnDirection.none
# LaneChangeState.off

4
selfdrive/controls/lib/lateral_planner.py
View File

@ -138,10 +138,10 @@ class LateralPlanner:
lateral_motion_cost = self.lateralMotionCost
path_cost = self.lateralPathCost
if carrot.atc_active:
if carrot.atcType == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1):
if carrot.atcType == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.01):
lateral_motion_cost = self.lateralMotionCostTurn
path_cost = self.lateralPathCostTurn
elif carrot.atcType == "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.01):
lateral_motion_cost = self.lateralMotionCostTurn
path_cost = self.lateralPathCostTurn

30
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@ -91,10 +91,10 @@ def get_safe_obstacle_distance(v_ego, t_follow=None, comfort_brake=COMFORT_BRAKE
t_follow = get_T_FOLLOW()
return (v_ego**2) / (2 * comfort_brake) + t_follow * v_ego + stop_distance
def desired_follow_distance(v_ego, v_lead, t_follow=None):
def desired_follow_distance(v_ego, v_lead, comfort_brake, stop_distance, t_follow=None):
if t_follow is None:
t_follow = get_T_FOLLOW()
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
return get_safe_obstacle_distance(v_ego, t_follow, comfort_brake, stop_distance) - get_stopped_equivalence_factor(v_lead)
def gen_long_model():
@ -355,24 +355,24 @@ class LongitudinalMpc:
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
mode = self.mode
comfort_brake = carrot.comfort_brake
stop_distance = carrot.stop_distance
if mode == 'blended':
stop_x = 1000.0
else:
v_cruise, stop_x, mode = carrot.v_cruise, carrot.stop_dist, carrot.mode
desired_distance = desired_follow_distance(v_ego, radarstate.leadOne.vLead, comfort_brake, stop_distance, t_follow)
t_follow = carrot.dynamic_t_follow(t_follow, radarstate.leadOne, desired_distance)
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
mode = self.mode
comfort_brake = COMFORT_BRAKE
stop_distance = STOP_DISTANCE
desired_distance = desired_follow_distance(v_ego, radarstate.leadOne.vLead, t_follow)
if mode == 'blended':
stop_x = 1000.0
else:
v_cruise, stop_x, mode = carrot.v_cruise, carrot.stop_dist, carrot.mode
comfort_brake, stop_distance = carrot.comfort_brake, carrot.stop_distance
t_follow = carrot.dynamic_t_follow(t_follow, radarstate.leadOne, desired_distance)
self.desired_distance = desired_follow_distance(v_ego, radarstate.leadOne.vLead, t_follow)
self.desired_distance = desired_follow_distance(v_ego, radarstate.leadOne.vLead, comfort_brake, stop_distance, t_follow)
self.params[:,0] = ACCEL_MIN if not reset_state else a_ego
# negative accel constraint causes problems because negative speed is not allowed

4
selfdrive/ui/carrot.cc
View File

@ -703,8 +703,8 @@ public:
#endif
}
else if (xState == 4) { //XState.e2ePrepare
ui_draw_text(s, x, disp_y, "신호출발중", disp_size, COLOR_WHITE, BOLD);
}
ui_draw_text(s, x, disp_y, "E2E주행중", disp_size, COLOR_WHITE, BOLD);
}
else if (xState == 0 || xState == 1 || xState == 2) { //XState.lead
draw_dist = true;
}