From be006371ba2f86d78a3f2bc6549d7bd78a578b7c Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 31 Jul 2024 19:10:52 -0700 Subject: [PATCH] Controls - Speed Limit Controller Automatically adjust the max speed to match the current speed limit using 'Open Street Maps', 'Navigate On openpilot', or your car's dashboard (Toyotas/Lexus/HKG only). Credit goes to Pfeiferj! https: //github.com/pfeiferj Co-Authored-By: Jacob Pfeifer --- selfdrive/controls/controlsd.py | 2 +- .../frogpilot/controls/frogpilot_planner.py | 14 +- .../lib/conditional_experimental_mode.py | 5 + .../controls/lib/speed_limit_controller.py | 126 ++++++++++++++++++ selfdrive/navd/navd.py | 7 + selfdrive/ui/qt/onroad/annotated_camera.cc | 46 +++++-- selfdrive/ui/qt/onroad/annotated_camera.h | 4 + selfdrive/ui/qt/onroad/onroad_home.cc | 7 + selfdrive/ui/ui.cc | 7 + selfdrive/ui/ui.h | 6 + 10 files changed, 212 insertions(+), 12 deletions(-) create mode 100644 selfdrive/frogpilot/controls/lib/speed_limit_controller.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3fcbc7255..ffe14afe3 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -880,7 +880,7 @@ class Controls: while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.frogpilot_toggles.speed_limit_controller and SpeedLimitController.experimental_mode self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index ef500a52b..6c3a7bf54 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -17,6 +17,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im 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 from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController GearShifter = car.CarState.GearShifter @@ -58,6 +59,7 @@ class FrogPilotPlanner: self.model_length = 0 self.mtsc_target = 0 self.road_curvature = 0 + self.slc_target = 0 self.speed_jerk = 0 self.tracked_model_length = 0 self.v_cruise = 0 @@ -232,6 +234,13 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 + # Pfeiferj's Speed Limit Controller + if frogpilot_toggles.speed_limit_controller: + SpeedLimitController.update(controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles) + self.slc_target = SpeedLimitController.desired_speed_limit + else: + self.slc_target = 0 + if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled: self.forcing_stop = True self.v_cruise = -1 @@ -251,7 +260,7 @@ class FrogPilotPlanner: self.forcing_stop = False self.tracked_model_length = 0 - targets = [self.mtsc_target] + targets = [self.mtsc_target, self.slc_target - v_ego_diff] self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) def publish(self, sm, pm, frogpilot_toggles): @@ -278,6 +287,9 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = float(self.max_accel) frogpilotPlan.minAcceleration = float(self.min_accel) + frogpilotPlan.slcSpeedLimit = self.slc_target + frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset + frogpilotPlan.vCruise = self.v_cruise pm.send('frogpilotPlan', frogpilot_plan_send) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 49f75099f..e5e304670 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -3,6 +3,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController MODEL_LENGTH = ModelConstants.IDX_N PLANNER_TIME = ModelConstants.T_IDXS[MODEL_LENGTH - 1] @@ -62,6 +63,10 @@ class ConditionalExperimentalMode: self.status_value = 15 if not self.frogpilot_planner.forcing_stop else 16 return True + if SpeedLimitController.experimental_mode: + self.status_value = 17 + return True + return False def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles): diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py new file mode 100644 index 000000000..8d6b08040 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -0,0 +1,126 @@ +# PFEIFER - SLC - Modified by FrogAi for FrogPilot +import json +import math + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + +R = 6373000.0 # approximate radius of earth in meters +TO_RADIANS = math.pi / 180 + +# points should be in radians +# output is meters +def distance_to_point(ax, ay, bx, by): + a = math.sin((bx - ax) / 2) * math.sin((bx - ax) / 2) + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2) * math.sin((by - ay) / 2) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) + return R * c # in meters + +class SpeedLimitController: + def __init__(self): + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.map_speed_limit = 0 # m/s + self.max_speed_limit = 0 # m/s + self.nav_speed_limit = 0 # m/s + self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit") + + def get_param_memory(self, key, is_json=False): + param_value = self.params_memory.get(key) + if param_value is None: + return {} if is_json else 0.0 + return json.loads(param_value) if is_json else float(param_value) + + def update_previous_limit(self, speed_limit): + if self.prv_speed_limit != speed_limit: + self.params.put_float_nonblocking("PreviousSpeedLimit", speed_limit) + self.prv_speed_limit = speed_limit + + def update(self, enabled, navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles): + self.write_map_state(v_ego) + self.nav_speed_limit = navigationSpeedLimit + + self.max_speed_limit = v_cruise if enabled else 0 + + self.frogpilot_toggles = frogpilot_toggles + + def write_map_state(self, v_ego): + self.map_speed_limit = self.get_param_memory("MapSpeedLimit") + + next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True) + next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0) + next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0) + next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0) + + position = self.get_param_memory("LastGPSPosition", is_json=True) + lat = position.get("latitude", 0) + lon = position.get("longitude", 0) + + if next_map_speed_limit_value > 1: + d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS) + + if self.prv_speed_limit < next_map_speed_limit_value: + max_d = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego + else: + max_d = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego + + if d < max_d: + self.map_speed_limit = next_map_speed_limit_value + + @property + def experimental_mode(self): + return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode + + @property + def desired_speed_limit(self): + if self.speed_limit > 1: + self.update_previous_limit(self.speed_limit) + return self.speed_limit + self.offset + return 0 + + @property + def offset(self): + if self.speed_limit < 13.5: + return self.frogpilot_toggles.offset1 + if self.speed_limit < 24: + return self.frogpilot_toggles.offset2 + if self.speed_limit < 29: + return self.frogpilot_toggles.offset3 + return self.frogpilot_toggles.offset4 + + @property + def speed_limit(self): + limits = [self.map_speed_limit, self.nav_speed_limit] + filtered_limits = [float(limit) for limit in limits if limit > 1] + + if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: + return max(filtered_limits) + if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits: + return min(filtered_limits) + + speed_limits = { + "Offline Maps": self.map_speed_limit, + "Navigation": self.nav_speed_limit, + } + + for priority in [ + self.frogpilot_toggles.speed_limit_priority1, + self.frogpilot_toggles.speed_limit_priority2, + self.frogpilot_toggles.speed_limit_priority3, + ]: + if speed_limits.get(priority, 0) in filtered_limits: + return speed_limits[priority] + + if self.frogpilot_toggles.use_previous_limit: + return self.prv_speed_limit + + if self.frogpilot_toggles.use_set_speed: + return self.max_speed_limit + + return 0 + +SpeedLimitController = SpeedLimitController() diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 84fa8337e..0ef74d9f4 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -74,6 +74,8 @@ class RouteEngine: self.approaching_turn = False self.update_toggles = False + self.nav_speed_limit = 0 + def update(self): self.sm.update(0) @@ -276,6 +278,7 @@ class RouteEngine: if self.step_idx is None: msg.valid = False + self.nav_speed_limit = 0 self.pm.send('navInstruction', msg) return @@ -350,6 +353,9 @@ class RouteEngine: if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + self.nav_speed_limit = closest.annotations['maxspeed'] + if not self.localizer_valid or ('maxspeed' not in closest.annotations): + self.nav_speed_limit = 0 # Speed limit sign type if 'speedLimitSign' in step: @@ -405,6 +411,7 @@ class RouteEngine: frogpilotNavigation.approachingIntersection = self.approaching_intersection frogpilotNavigation.approachingTurn = self.approaching_turn + frogpilotNavigation.navigationSpeedLimit = self.nav_speed_limit self.pm.send('frogpilotNavigation', frogpilot_plan_send) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index ec50f8c31..e1497e511 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -54,11 +54,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; auto speed_limit_sign = nav_instruction.getSpeedLimitSign(); - speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0; + speedLimit = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + if (speedLimitController) { + speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0); + } - has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); - has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); + has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign); + has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) && !(speedLimitController && !useViennaSLCSign) || (speedLimitController && useViennaSLCSign); is_metric = s.scene.is_metric; speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph"); hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); @@ -91,6 +94,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg); QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–"; + QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : ""); QString speedStr = QString::number(std::nearbyint(speed)); QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–"; @@ -166,11 +170,20 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(blackColor(), 6)); p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16); - p.setFont(InterFont(28, QFont::DemiBold)); - p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); - p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); - p.setFont(InterFont(70, QFont::Bold)); - p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + if (speedLimitController && showSLCOffset) { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + p.setFont(InterFont(50, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + } } // EU (Vienna style) sign @@ -181,9 +194,16 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(Qt::red, 20)); p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16)); - p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); p.setPen(blackColor()); - p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); + if (showSLCOffset) { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr); + p.setFont(InterFont(40, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); + } } // current speed @@ -549,6 +569,11 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce reverseCruise = scene.reverse_cruise; + speedLimitController = scene.speed_limit_controller; + showSLCOffset = speedLimitController && scene.show_slc_offset; + slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); + useViennaSLCSign = scene.use_vienna_slc_sign; + trafficModeActive = scene.traffic_mode_active; } @@ -590,6 +615,7 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { {14, tr("Experimental Mode activated for slower lead")}, {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))}, {16, tr("Experimental Mode forced on for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))}, + {17, tr("Experimental Mode activated due to no speed limit")}, }; if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 00a7aa4c4..9387872d8 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -61,11 +61,15 @@ private: bool reverseCruise; bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; + bool showSLCOffset; + bool speedLimitController; bool trafficModeActive; + bool useViennaSLCSign; float accelerationConversion; float cruiseAdjustment; float distanceConversion; + float slcSpeedLimitOffset; float speedConversion; int alertSize; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 413bb8324..dd4274709 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -98,6 +98,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { QPoint pos = e->pos(); QRect maxSpeedRect(7, 25, 225, 225); + QRect speedLimitRect(7, 250, 225, 225); if (maxSpeedRect.contains(pos) && scene.reverse_cruise_ui) { scene.reverse_cruise = !scene.reverse_cruise; @@ -106,6 +107,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { return; } + if (speedLimitRect.contains(pos) && scene.show_slc_offset_ui) { + scene.show_slc_offset = !scene.show_slc_offset; + params.putBoolNonBlocking("ShowSLCOffset", scene.show_slc_offset); + return; + } + if (scene.experimental_mode_via_screen && pos != timeoutPoint) { if (clickTimer.isActive()) { clickTimer.stop(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index ee745bd29..d8f2788fa 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -237,6 +237,8 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); + scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -306,6 +308,11 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.reverse_cruise = quality_of_life_controls && params.getBool("ReverseCruise"); scene.reverse_cruise_ui = params.getBool("ReverseCruiseUI"); + scene.speed_limit_controller = scene.longitudinal_control && params.getBool("SpeedLimitController"); + scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset"); + scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI"); + scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna"); + scene.tethering_config = params.getInt("TetheringEnabled"); if (scene.tethering_config == 2) { WifiManager(s).setTetheringEnabled(true); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 694707512..4cb639241 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -139,13 +139,19 @@ typedef struct UIScene { bool right_hand_drive; bool show_aol_status_bar; bool show_cem_status_bar; + bool show_slc_offset; + bool show_slc_offset_ui; + bool speed_limit_controller; bool tethering_enabled; bool traffic_mode; bool traffic_mode_active; bool use_kaofui_icons; + bool use_vienna_slc_sign; float adjusted_cruise; float lead_detection_threshold; + float speed_limit; + float speed_limit_offset; int alert_size; int conditional_speed;