From 40ccab9e1cc5c70745834d6c3c79c29faafde90d Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 31 Jul 2024 17:58:33 -0700 Subject: [PATCH] FrogPilot community - Calculate road curvature Co-Authored-By: Jacob Pfeifer --- selfdrive/frogpilot/controls/frogpilot_planner.py | 5 ++++- selfdrive/frogpilot/controls/lib/frogpilot_functions.py | 8 ++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 72069b679..eb4b684dc 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, calculate_lane_width +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY GearShifter = car.CarState.GearShifter @@ -26,6 +26,7 @@ class FrogPilotPlanner: self.acceleration_jerk = 0 self.danger_jerk = 0 + self.road_curvature = 0 self.speed_jerk = 0 self.v_cruise = 0 @@ -50,6 +51,8 @@ class FrogPilotPlanner: self.lane_width_left = 0 self.lane_width_right = 0 + self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1 + 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) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py index b6b39153e..ae3f78e24 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py @@ -7,6 +7,7 @@ import os import shutil import socket import subprocess +import sys import threading import time import urllib.error @@ -62,6 +63,13 @@ def calculate_lane_width(lane, current_lane, road_edge): return float(min(distance_to_lane, distance_to_road_edge)) +# Credit goes to Pfeiferj! +def calculate_road_curvature(modelData, v_ego): + orientation_rate = np.abs(modelData.orientationRate.z) + velocity = modelData.velocity.x + max_pred_lat_acc = np.amax(orientation_rate * velocity) + return abs(float(max(max_pred_lat_acc / v_ego**2, sys.float_info.min))) + def backup_directory(backup, destination, success_msg, fail_msg): os.makedirs(destination, exist_ok=True) try: