mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 00:05:59 +08:00
Code Issues Packages Projects Releases Wiki Activity

FrogPilot features - Lane detection

Browse Source
This commit is contained in:
FrogAi
2024-07-19 21:16:58 -07:00
parent 1e930ec10b
commit 751b99f588
2 changed files with 23 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/frogpilot/controls/frogpilot_planner.py
+11 -1
View File
@@ -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)
selfdrive/frogpilot/controls/lib/frogpilot_functions.py
+12
View File
@@ -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: