From 10f8f07fdc3b5db8194cab2e873799adf2623de6 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 22 Jun 2024 14:27:21 -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 | 123 ++++++++++++++++++ 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, 209 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 9a5c70aee..2e04eb3c7 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -879,7 +879,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 45aae4f16..ebaa4b008 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, TRAJECTORY_SIZE from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController A_CRUISE_MIN_ECO = A_CRUISE_MIN / 5 A_CRUISE_MIN_SPORT = A_CRUISE_MIN / 2 @@ -51,6 +52,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 @@ -211,6 +213,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(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles) + self.slc_target = SpeedLimitController.desired_speed_limit + else: + self.slc_target = 0 + if (frogpilot_toggles.force_standstill or frogpilot_toggles.force_stops) and v_ego < 1 and not self.override_force_stop: if carState.gasPressed: self.override_force_stop = True @@ -228,7 +237,7 @@ class FrogPilotPlanner: self.v_cruise = self.tracked_model_length / ModelConstants.T_IDXS[TRAJECTORY_SIZE - 1] else: - targets = [self.mtsc_target] + targets = [self.mtsc_target, self.slc_target - v_ego_diff] self.v_cruise = min([target if target > CRUISING_SPEED else v_cruise for target in targets]) def publish(self, sm, pm, frogpilot_toggles): @@ -253,6 +262,9 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.minAcceleration = self.min_accel + frogpilotPlan.slcSpeedLimit = self.slc_target + frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset + frogpilotPlan.vCruise = float(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 0ee526ab3..fc4c0f34d 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, CRUISING_SPEED, PROBABILITY, TRAJECTORY_SIZE +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController class ConditionalExperimentalMode: def __init__(self): @@ -54,6 +55,10 @@ class ConditionalExperimentalMode: self.status_value = 15 return True + if SpeedLimitController.experimental_mode: + self.status_value = 16 + return True + return False def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, 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..7e53683b1 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -0,0 +1,123 @@ +# 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.nav_speed_limit = 0 # m/s + self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit") + + def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles): + self.write_map_state(v_ego) + self.nav_speed_limit = navigationSpeedLimit + + self.frogpilot_toggles = frogpilot_toggles + + def get_param_memory(self, key, is_json=False, default=None): + try: + data = self.params_memory.get(key) + if data is None: + return default + return json.loads(data) if is_json else float(data.decode('utf-8')) + except (TypeError, ValueError): + return default + + 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, default={}) + 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, default={}) + 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 = v_ego * self.frogpilot_toggles.map_speed_lookahead_higher + else: + max_d = v_ego * self.frogpilot_toggles.map_speed_lookahead_lower + + if d < max_d: + self.map_speed_limit = next_map_speed_limit_value + + @property + def speed_limit(self): + limits = [self.map_speed_limit, self.nav_speed_limit] + filtered_limits = [limit for limit in limits if limit is not None and limit > 1] + + if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: + return float(max(filtered_limits)) + if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits: + return float(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 float(speed_limits[priority]) + + if self.frogpilot_toggles.use_previous_limit: + return float(self.prv_speed_limit) + + 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 desired_speed_limit(self): + if self.speed_limit > 1: + self.update_previous_limit(self.speed_limit) + return self.speed_limit + self.offset + return 0 + + def update_previous_limit(self, speed_limit): + if self.prv_speed_limit != speed_limit: + self.params.put_float("PreviousSpeedLimit", speed_limit) + self.prv_speed_limit = speed_limit + + @property + def experimental_mode(self): + return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode + +SpeedLimitController = SpeedLimitController() diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 7440c7fa2..97f467ad7 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 e4af7345e..9dcae0548 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 @@ -545,6 +565,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; } @@ -585,6 +610,7 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { {13, tr("Experimental Mode activated for slower lead")}, {14, tr("Experimental Mode activated for stopped lead")}, {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))}, + {16, 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 7f9c9696c..ea3f98f16 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(); @@ -302,6 +304,11 @@ void ui_update_frogpilot_params(UIState *s) { 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 3862c6f53..8cffb4e51 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -137,13 +137,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;