From b4550a13bac8008edeb2f7d49fb925080bdbafc0 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 12 Jan 2024 22:39:30 -0700 Subject: [PATCH] Calculate average desired curvature using planned distance 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 --- cereal/custom.capnp | 1 + common/params.cc | 1 + selfdrive/controls/controlsd.py | 6 +++++- selfdrive/controls/lib/drive_helpers.py | 11 ++++++++--- selfdrive/controls/lib/lateral_planner.py | 5 ++++- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 ++ selfdrive/frogpilot/functions/frogpilot_planner.py | 13 ++++++++++++- selfdrive/frogpilot/ui/control_settings.cc | 3 ++- 8 files changed, 35 insertions(+), 7 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 4d71d8c..a861756 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -22,6 +22,7 @@ struct FrogPilotLateralPlan @0xda96579883444c35 { } struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 { + distances @3 :List(Float32); } struct FrogPilotNavigation @0xa5cd762cd951a455 { diff --git a/common/params.cc b/common/params.cc index b5886e9..51cd4dd 100644 --- a/common/params.cc +++ b/common/params.cc @@ -217,6 +217,7 @@ std::unordered_map keys = { {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, + {"AverageCurvature", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"LateralTune", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 737223a..db54921 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b728f11..f8cfcb4 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 27cc31b..b2e38d4 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -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]) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 64d0b62..ab44406 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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] diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 75d9e4d..a6f5a7d 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -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 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index f8645e6..f9ae4c5 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -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 = {};