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

Calculate average desired curvature using planned distance

Browse Source 
Added toggle to calculate average desired curvature using planned distance instead of the car's speed.

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 8baf713117
commit b4550a13ba
8 changed files with 35 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
cereal/custom.capnp
View File

@@ -22,6 +22,7 @@ struct FrogPilotLateralPlan @0xda96579883444c35 {
}
struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 {
distances @3 :List(Float32);
}
struct FrogPilotNavigation @0xa5cd762cd951a455 {

1
common/params.cc
View File

@@ -217,6 +217,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"AlwaysOnLateral", PERSISTENT},
{"AlwaysOnLateralMain", PERSISTENT},
{"ApiCache_DriveStats", PERSISTENT},
{"AverageCurvature", PERSISTENT},
{"FrogPilotTogglesUpdated", PERSISTENT},
{"GasRegenCmd", PERSISTENT},
{"LateralTune", PERSISTENT},

6
selfdrive/controls/controlsd.py
View File

@@ -649,7 +649,9 @@ class Controls:
self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
lat_plan.curvatureRates,
frogpilot_long_plan.distances,
self.average_desired_curvature)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])
@@ -940,6 +942,8 @@ class Controls:
if hasattr(obj, 'update_frogpilot_params'):
obj.update_frogpilot_params(self.params)
self.average_desired_curvature = self.params.get_bool("AverageCurvature")
longitudinal_tune = self.params.get_bool("LongitudinalTune")
self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune

11
selfdrive/controls/lib/drive_helpers.py
View File

@@ -16,6 +16,7 @@ V_CRUISE_INITIAL = 40
V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
MIN_DIST = 0.001
MIN_SPEED = 1.0
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
@@ -163,11 +164,12 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
if len(psis) != CONTROL_N:
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature):
if len(psis) != CONTROL_N or len(distances) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
curvature_rates = [0.0]*CONTROL_N
distances = [0.0]*CONTROL_N
v_ego = max(MIN_SPEED, v_ego)
# TODO this needs more thought, use .2s extra for now to estimate other delays
@@ -178,7 +180,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
# psi to calculate a simple linearization of desired curvature
current_curvature_desired = curvatures[0]
psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
average_curvature_desired = psi / (v_ego * delay)
# Pfeiferj's #28118 PR - https://github.com/commaai/openpilot/pull/28118
distance = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances)
distance = max(MIN_DIST, distance)
average_curvature_desired = psi / distance if average_desired_curvature else psi / (v_ego * delay)
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
# This is the "desired rate of the setpoint" not an actual desired rate

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

@@ -35,7 +35,10 @@ class LateralPlanner:
if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
if frogpilot_planner.average_desired_curvature:
car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car)
else:
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])

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

@@ -230,6 +230,7 @@ class LongitudinalMpc:
# self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.solver.reset()
# self.solver.options_set('print_level', 2)
self.x_solution = np.zeros(N+1)
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.prev_a = np.array(self.a_solution)
@@ -442,6 +443,7 @@ class LongitudinalMpc:
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.x_solution = self.x_sol[:,0]
self.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]

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

@@ -1,9 +1,12 @@
import cereal.messaging as messaging
import numpy as np
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
from openpilot.selfdrive.modeld.constants import ModelConstants
# Acceleration profiles - Credit goes to the DragonPilot team!
# MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123]
@@ -34,6 +37,8 @@ class FrogPilotPlanner:
def __init__(self, params):
self.v_cruise = 0
self.x_desired_trajectory = np.zeros(CONTROL_N)
self.update_frogpilot_params(params)
def update(self, sm, mpc):
@@ -55,6 +60,9 @@ class FrogPilotPlanner:
self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego)
self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution)
self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N]
def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
return v_cruise - v_ego_diff
@@ -71,12 +79,15 @@ class FrogPilotPlanner:
frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan
frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist()
pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
def update_frogpilot_params(self, params):
self.is_metric = params.get_bool("IsMetric")
lateral_tune = params.get_bool("LateralTune")
self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune
longitudinal_tune = params.get_bool("LongitudinalTune")
self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0

3
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -6,6 +6,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
{"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"},
{"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
{"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""},
{"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
{"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""},
@@ -97,7 +98,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
conditionalExperimentalKeys = {};
fireTheBabysitterKeys = {};
laneChangeKeys = {};
lateralTuneKeys = {};
lateralTuneKeys = {"AverageCurvature"};
longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration"};
speedLimitControllerKeys = {};
visionTurnControlKeys = {};