diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index 92704b8..dee52cb 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -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: diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index bdf4ee3..bc6fd7d 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/selfdrive/carrot/carrot_man.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 145fad9..d432e1b 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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: diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index cf38702..82d6f8d 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 8153d12..51119ef 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 145b36e..ac8f321 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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 diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index 67393b3..0c755b3 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -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; }