diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 61c95518b..72069b679 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -13,7 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel -from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY GearShifter = car.CarState.GearShifter @@ -43,6 +43,13 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel stopping_distance = STOP_DISTANCE + if v_ego >= frogpilot_toggles.minimum_lane_change_speed: + self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]) + self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles) self.set_follow_values(controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) self.set_lead_status(lead_distance, stopping_distance, v_ego) @@ -101,6 +108,9 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk) frogpilotPlan.tFollow = float(self.t_follow) + frogpilotPlan.laneWidthLeft = self.lane_width_left + frogpilotPlan.laneWidthRight = self.lane_width_right + frogpilotPlan.maxAcceleration = float(self.max_accel) frogpilotPlan.minAcceleration = float(self.min_accel) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py index d81649894..ff7230463 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py @@ -2,6 +2,7 @@ import datetime import filecmp import glob import http.client +import numpy as np import os import shutil import socket @@ -50,6 +51,17 @@ def run_cmd(cmd, success_msg, fail_msg): except Exception as e: print(f"Unexpected error occurred: {e}") +def calculate_lane_width(lane, current_lane, road_edge): + current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) + + lane_y_interp = interp(current_x, np.array(lane.x), np.array(lane.y)) + road_edge_y_interp = interp(current_x, np.array(road_edge.x), np.array(road_edge.y)) + + distance_to_lane = np.mean(abs(current_y - lane_y_interp)) + distance_to_road_edge = np.mean(abs(current_y - road_edge_y_interp)) + + return float(min(distance_to_lane, distance_to_road_edge)) + def backup_directory(backup, destination, success_msg, fail_msg): os.makedirs(destination, exist_ok=True) try: