brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Traffic Mode

Browse Source 
Added toggle to change the longitudinal behavior to be more focused on driving in traffic.
This commit is contained in:
FrogAi
2024-03-15 19:56:43 -07:00
parent 3e5c66abeb
commit bed144260f
13 changed files with 64 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -2,18 +2,20 @@ import cereal.messaging as messaging
import numpy as np
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, FrogPilotFunctions
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
TRAFFIC_MODE_T_FOLLOW = [.50, 1.]
class FrogPilotPlanner:
def __init__(self, CP, params, params_memory):
@@ -26,6 +28,7 @@ class FrogPilotPlanner:
self.mtsc = MapTurnSpeedController()
self.override_slc = False
self.traffic_mode_active = False
self.overridden_speed = 0
self.mtsc_target = 0
@@ -84,6 +87,11 @@ class FrogPilotPlanner:
# Update the current road curvature
self.road_curvature = self.fpf.road_curvature(modelData, v_ego)
# Update the current state of "Traffic Mode"
self.traffic_mode_active = self.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
if self.traffic_mode_active:
self.traffic_mode_t_follow = interp(v_ego, TRAFFIC_MODE_BP, TRAFFIC_MODE_T_FOLLOW)
# Update the desired stopping distance
self.stop_distance = STOP_DISTANCE
@@ -209,6 +217,7 @@ class FrogPilotPlanner:
self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration")
self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0
self.smoother_braking = longitudinal_tune and params.get_bool("SmoothBraking")
self.traffic_mode = longitudinal_tune and params.get_bool("TrafficMode")
self.map_turn_speed_controller = params.get_bool("MTSCEnabled")
if self.map_turn_speed_controller: