diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 981a98717c..fdce578946 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -15,8 +15,11 @@ sunnypilot - 0.9.6.1 (2023-xx-xx) * C3X-specific changes * Altitude (ALT.) display on Developer UI * Current street name on top of driving screen when "OSM Debug UI" is enabled - * DISABLED: Map-based Turn Speed Control (M-TSC) - * Reimplementation in near future updates +* UPDATED: Map-based Turn Speed Control (M-TSC) implementation + * Only available in "staging-c3" and "dev-c3" branches. If you are using "release-c3" branch, navigate to "Software" panel, select the desired target branch, and check for update + * Refactored implementation thanks to pfeiferj! + * Based on the new OpenStreetMap implementation + * Improved predicted curvature calculations from OpenStreetMap data * UI updates * RE-ENABLED: Navigation: Full screen support * Display the map view in full screen diff --git a/common/params.cc b/common/params.cc index 5318eb0c14..06749a9987 100644 --- a/common/params.cc +++ b/common/params.cc @@ -270,6 +270,7 @@ std::unordered_map keys = { {"MadsCruiseMain", PERSISTENT}, {"MadsIconToggle", PERSISTENT}, {"MapboxFullScreen", PERSISTENT}, + {"MapTargetVelocities", PERSISTENT}, {"Map3DBuildings", PERSISTENT}, {"MaxTimeOffroad", PERSISTENT}, {"NNFF", PERSISTENT}, @@ -282,6 +283,7 @@ std::unordered_map keys = { {"OsmLocationTitle", PERSISTENT}, {"OsmLocationUrl", PERSISTENT}, {"OsmWayTest", PERSISTENT}, + {"OsmDownloadedDate", PERSISTENT}, {"PathOffset", PERSISTENT}, {"QuietDrive", PERSISTENT}, {"RoadEdge", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c030bf90b4..f8eb86325e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -229,9 +229,7 @@ class LongitudinalPlanner: longitudinalPlanSP.events = self.events.to_msg() longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state - longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.speed_limit) - longitudinalPlanSP.distToTurn = float(self.turn_speed_controller.distance) - longitudinalPlanSP.turnSign = int(self.turn_speed_controller.turn_sign) + longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target) longitudinalPlanSP.personality = self.personality @@ -244,7 +242,7 @@ class LongitudinalPlanner: self.vision_turn_controller.update(enabled, v_ego, v_cruise, sm) self.events = Events() self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.CP, self.events) - self.turn_speed_controller.update(enabled, v_ego, a_ego, sm) + self.turn_speed_controller.update(enabled, v_ego, sm, v_cruise) # Pick solution with the lowest velocity target. v_solutions = {'cruise': v_cruise} @@ -256,7 +254,7 @@ class LongitudinalPlanner: v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted if self.turn_speed_controller.is_active: - v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit + v_solutions['turnlimit'] = self.turn_speed_controller.v_target source = min(v_solutions, key=v_solutions.get) diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py index 7d1051d314..b2818c6c96 100644 --- a/selfdrive/controls/lib/turn_speed_controller.py +++ b/selfdrive/controls/lib/turn_speed_controller.py @@ -1,56 +1,63 @@ -import numpy as np +import json +import math import time -from common.params import Params from cereal import custom -from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_SPEED, LIMIT_MAX_MAP_DATA_AGE, \ - LIMIT_SPEED_OFFSET_TH, CONTROL_N, LIMIT_MIN_ACC, LIMIT_MAX_ACC -from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.common.params import Params +from openpilot.system.version import is_release_sp_branch + +R = 6373000.0 # approximate radius of earth in meters +TO_RADIANS = math.pi / 180 +TO_DEGREES = 180 / math.pi +TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some +TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit +TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps + # reach the target velocity when innacuracies in the distance modeling logic would cause overshoot. + # The value is multiplied against the target velocity to determine the additional distance. This is + # done to keep the distance calculations consistent but results in the offset actually being less + # time than specified depending on how much of a speed diffrential there is between v_ego and the + # target velocity. -_ACTIVE_LIMIT_MIN_ACC = -0.5 # m/s^2 Maximum deceleration allowed while active. -_ACTIVE_LIMIT_MAX_ACC = 0.5 # m/s^2 Maximum acelration allowed while active. +def calculate_accel(t, target_jerk, a_ego): + return a_ego + target_jerk * t -_DEBUG = False +def calculate_velocity(t, target_jerk, a_ego, v_ego): + return v_ego + a_ego * t + target_jerk/2 * (t ** 2) + + +def calculate_distance(t, target_jerk, a_ego, v_ego): + return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) + + +PARAMS_UPDATE_PERIOD = 5. TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState -def _debug(msg): - if not _DEBUG: - return - print(msg) +# 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 -def _description_for_state(turn_speed_control_state): - if turn_speed_control_state == TurnSpeedControlState.inactive: - return 'INACTIVE' - if turn_speed_control_state == TurnSpeedControlState.tempInactive: - return 'TEMP INACTIVE' - if turn_speed_control_state == TurnSpeedControlState.adapting: - return 'ADAPTING' - if turn_speed_control_state == TurnSpeedControlState.active: - return 'ACTIVE' - - -class TurnSpeedController(): +class TurnSpeedController: def __init__(self): - self._params = Params() - self._last_params_update = 0. - self._is_enabled = self._params.get_bool("TurnSpeedControl") + self.params = Params() + self.mem_params = Params("/dev/shm/params") + self.enabled = self.params.get_bool("TurnSpeedControl") and not is_release_sp_branch() + self.last_params_update = 0 self._op_enabled = False - self._v_ego = 0. - self._a_ego = 0. - self._v_cruise_setpoint = 0. - - self._v_offset = 0. - self._speed_limit = 0. - self._speed_limit_temp_inactive = 0. - self._distance = 0. - self._turn_sign = 0 + self._gas_pressed = False self._state = TurnSpeedControlState.inactive - - self._next_speed_limit_prev = 0. + self._v_cruise = 0 + self._min_v = 0 + self.target_lat = 0.0 + self.target_lon = 0.0 + self.target_v = 0.0 @property def state(self): @@ -58,18 +65,6 @@ class TurnSpeedController(): @state.setter def state(self, value): - if value != self._state: - _debug(f'Turn Speed Controller state: {_description_for_state(value)}') - - if value == TurnSpeedControlState.adapting: - _debug('TSC: Enteriing Adapting as speed offset is below threshold') - _debug(f'_v_offset: {self._v_offset * 3.6}\nspeed_limit: {self.speed_limit * 3.6}') - _debug(f'_v_ego: {self._v_ego * 3.6}\ndistance: {self.distance}') - - if value == TurnSpeedControlState.tempInactive: - # Track the speed limit value when controller was set to temp inactive. - self._speed_limit_temp_inactive = self._speed_limit - self._state = value @property @@ -77,140 +72,156 @@ class TurnSpeedController(): return self.state > TurnSpeedControlState.tempInactive @property - def speed_limit(self): - return max(self._speed_limit, LIMIT_MIN_SPEED) if self._speed_limit > 0. else 0. + def v_target(self): + return self._min_v - @property - def distance(self): - return max(self._distance, 0.) - - @property - def turn_sign(self): - return self._turn_sign - - def _get_limit_from_map_data(self, sm): - """Provides the speed limit, distance and turn sign to it for turns based on map data. - """ - # Ignore if no live map data - sock = 'liveMapDataSP' - if sm.logMonoTime[sock] is None: - _debug('TS: No map data for turn speed limit') - return 0., 0., 0 - - # Load map_data and initialize - map_data = sm[sock] - speed_limit = 0. - - # Calculate the age of the gps fix. Ignore if too old. - gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 - if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: - _debug(f'TS: Ignoring map data as is too old. Age: {gps_fix_age}') - return 0., 0., 0 - - # Load turn ahead sections info from map_data with distances corrected by gps_fix_age - distance_since_fix = self._v_ego * gps_fix_age - distances_to_sections_ahead = np.maximum(0., np.array(map_data.turnSpeedLimitsAheadDistances) - distance_since_fix) - speed_limit_in_sections_ahead = map_data.turnSpeedLimitsAhead - turn_signs_in_sections_ahead = map_data.turnSpeedLimitsAheadSigns - - # Ensure current speed limit is considered only if we are inside the section. - if map_data.turnSpeedLimitValid and self._v_ego > 0.: - speed_limit_end_time = (map_data.turnSpeedLimitEndDistance / self._v_ego) - gps_fix_age - if speed_limit_end_time > 0.: - speed_limit = map_data.turnSpeedLimit - - # When we have no ahead speed limit to consider or all are greater than current speed limit - # or car has stopped, then provide current value and reset tracking. - turn_sign = map_data.turnSpeedLimitSign if map_data.turnSpeedLimitValid else 0 - if len(speed_limit_in_sections_ahead) == 0 or self._v_ego <= 0. or \ - (speed_limit > 0 and np.amin(speed_limit_in_sections_ahead) > speed_limit): - self._next_speed_limit_prev = 0. - return speed_limit, 0., turn_sign - - # Calculated the time needed to adapt to the limits ahead and the corresponding distances. - adapt_times = (np.maximum(speed_limit_in_sections_ahead, LIMIT_MIN_SPEED) - self._v_ego) / LIMIT_ADAPT_ACC - adapt_distances = self._v_ego * adapt_times + 0.5 * LIMIT_ADAPT_ACC * adapt_times**2 - distance_gaps = distances_to_sections_ahead - adapt_distances - - # We select as next speed limit, the one that have the lowest distance gap. - next_idx = np.argmin(distance_gaps) - next_speed_limit = speed_limit_in_sections_ahead[next_idx] - distance_to_section_ahead = distances_to_sections_ahead[next_idx] - next_turn_sign = turn_signs_in_sections_ahead[next_idx] - distance_gap = distance_gaps[next_idx] - - # When we have a next_speed_limit value that has not changed from a provided next speed limit value - # in previous resolutions, we keep providing it along with the updated distance to it. - if next_speed_limit == self._next_speed_limit_prev: - return next_speed_limit, distance_to_section_ahead, next_turn_sign - - # Reset tracking - self._next_speed_limit_prev = 0. - - # When we detect we are close enough, we provide the next limit value and track it. - if distance_gap <= 0.: - self._next_speed_limit_prev = next_speed_limit - return next_speed_limit, distance_to_section_ahead, next_turn_sign - - # Otherwise we just provide the calculated speed_limit - return speed_limit, 0., turn_sign - - def _update_params(self): + def update_params(self): t = time.monotonic() - if t > self._last_params_update + 5.0: - self._is_enabled = self._params.get_bool("TurnSpeedControl") - self._last_params_update = t + if t > self.last_params_update + PARAMS_UPDATE_PERIOD: + self.enabled = self.params.get_bool("TurnSpeedControl") and not is_release_sp_branch() + self.last_params_update = t - def _update_calculations(self): - # Update current velocity offset (error) - self._v_offset = self.speed_limit - self._v_ego + def target_speed(self, v_ego, a_ego) -> float: + if not self.enabled: + return 0.0 - def _state_transition(self, sm): - # In any case, if op is disabled, or turn speed limit control is disabled - # or the reported speed limit is 0, deactivate. - if not self._op_enabled or not self._is_enabled or self.speed_limit == 0.: + lat = 0.0 + lon = 0.0 + try: + position = json.loads(self.mem_params.get("LastGPSPosition")) + lat = position["latitude"] + lon = position["longitude"] + except: return 0.0 + + try: + target_velocities = json.loads(self.mem_params.get("MapTargetVelocities")) + except: return 0.0 + + min_dist = 1000 + min_idx = 0 + distances = [] + + # find our location in the path + for i in range(len(target_velocities)): + target_velocity = target_velocities[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) + distances.append(d) + if d < min_dist: + min_dist = d + min_idx = i + + # only look at values from our current position forward + forward_points = target_velocities[min_idx:] + forward_distances = distances[min_idx:] + + # find velocities that we are within the distance we need to adjust for + valid_velocities = [] + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > v_ego: + continue + + d = forward_distances[i] + + a_diff = (a_ego - TARGET_ACCEL) + accel_t = abs(a_diff / TARGET_JERK) + min_accel_v = calculate_velocity(accel_t, TARGET_JERK, a_ego, v_ego) + + max_d = 0 + if tv > min_accel_v: + # calculate time needed based on target jerk + a = 0.5 * TARGET_JERK + b = a_ego + c = v_ego - tv + t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a + t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a + if not isinstance(t_a, complex) and t_a > 0: + t = t_a + else: + t = t_b + if isinstance(t, complex): + continue + + max_d = max_d + calculate_distance(t, TARGET_JERK, a_ego, v_ego) + else: + t = accel_t + max_d = calculate_distance(t, TARGET_JERK, a_ego, v_ego) + + # calculate additional time needed based on target accel + t = abs((min_accel_v - tv) / TARGET_ACCEL) + max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v) + + if d < max_d + tv * TARGET_OFFSET: + valid_velocities.append((float(tv), tlat, tlon)) + + # Find the smallest velocity we need to adjust for + min_v = 100.0 + target_lat = 0.0 + target_lon = 0.0 + for tv, lat, lon in valid_velocities: + if tv < min_v: + min_v = tv + target_lat = lat + target_lon = lon + + if self.target_v < min_v and not (self.target_lat == 0 and self.target_lon == 0): + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > v_ego: + continue + + if tlat == self.target_lat and tlon == self.target_lon and tv == self.target_v: + return float(self.target_v) + # not found so lets reset + self.target_v = 0.0 + self.target_lat = 0.0 + self.target_lon = 0.0 + + self.target_v = min_v + self.target_lat = target_lat + self.target_lon = target_lon + + return min_v + + def _state_transition(self): + if not self._op_enabled or not self.enabled: self.state = TurnSpeedControlState.inactive return - # In any case, we deactivate the speed limit controller temporarily - # if gas is pressed (to support gas override implementations). - if sm['carState'].gasPressed: + if self._gas_pressed: self.state = TurnSpeedControlState.tempInactive return - # inactive + # INACTIVE if self.state == TurnSpeedControlState.inactive: - # If the limit speed offset is negative (i.e. reduce speed) and lower than threshold and distanct to turn limit - # is positive (not in turn yet) we go to adapting state to reduce speed, otherwise we go directly to active - if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.: - self.state = TurnSpeedControlState.adapting - else: + if self._v_cruise > self._min_v != 0: self.state = TurnSpeedControlState.active - # tempInactive + + # TEMP INACTIVE elif self.state == TurnSpeedControlState.tempInactive: - # if the speed limit recorded when going to temp Inactive changes - # then set to inactive, activation will happen on next cycle - if self._speed_limit != self._speed_limit_temp_inactive: - self.state = TurnSpeedControlState.inactive - # adapting - elif self.state == TurnSpeedControlState.adapting: - # Go to active once the speed offset is over threshold or the distance to turn is now 0. - if self._v_offset >= LIMIT_SPEED_OFFSET_TH or self.distance == 0.: + if self._v_cruise > self._min_v != 0: self.state = TurnSpeedControlState.active - # active + else: + self.state = TurnSpeedControlState.inactive + + # ACTIVE elif self.state == TurnSpeedControlState.active: - # Go to adapting if the speed offset goes below threshold as long as the distance to turn is still positive. - if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.: - self.state = TurnSpeedControlState.adapting + if not (self._v_cruise > self._min_v != 0): + self.state = TurnSpeedControlState.inactive - def update(self, enabled, v_ego, a_ego, sm): - self._op_enabled = enabled - self._v_ego = v_ego - self._a_ego = a_ego + def update(self, op_enabled, v_ego, sm, v_cruise): + self.update_params() + self._op_enabled = op_enabled + self._gas_pressed = sm['carState'].gasPressed + self._v_cruise = v_cruise + self._min_v = self.target_speed(v_ego, sm['carState'].aEgo) - # Get the speed limit from Map Data - self._speed_limit, self._distance, self._turn_sign = self._get_limit_from_map_data(sm) - - self._update_params() - self._update_calculations() - self._state_transition(sm) + self._state_transition() diff --git a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc index 447c1e19bb..18a8541b2f 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc @@ -32,7 +32,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) { }, { "TurnSpeedControl", - tr("Enable Map Data Turn Speed Control (M-TSC)"), + tr("Enable Map Data Turn Speed Control (M-TSC) (Beta)"), tr("Use curvature information from map data to define speed limits to take turns ahead."), "../assets/offroad/icon_blank.png", }, @@ -413,6 +413,7 @@ void SunnypilotPanel::updateToggles() { QString nnff_loaded = tr("✅ NNLC Loaded"); auto _car_model = QString::fromStdString(params.get("NNFFCarModel")); + const bool is_release_sp = params.getBool("IsReleaseSPBranch"); auto cp_bytes = params.get("CarParamsPersistent"); if (!cp_bytes.empty()) { AlignedBuffer aligned_buf; @@ -445,6 +446,11 @@ void SunnypilotPanel::updateToggles() { } } + if (is_release_sp) { + params.remove("TurnSpeedControl"); + } + m_tsc->setVisible(!is_release_sp); + if (hasLongitudinalControl(CP) || custom_stock_long_param) { v_tsc->setEnabled(true); m_tsc->setEnabled(true); @@ -462,9 +468,10 @@ void SunnypilotPanel::updateToggles() { enforce_torque_lateral->refresh(); slc_toggle->refresh(); nnff_toggle->refresh(); + m_tsc->refresh(); } else { v_tsc->setEnabled(false); - m_tsc->setEnabled(false); + m_tsc->setVisible(false); // TODO: temporarily disable M-TSC until the reimplementation is in place. Remove this line to re-enable the toggle. reverse_acc->setEnabled(false); slc_toggle->setEnabled(false); slcSettings->setEnabled(false); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index b2999d050a..f648469149 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -722,7 +722,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { const int t_distance = int(lp_sp.getDistToTurn() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10; const QString t_distance_str(QString::number(t_distance) + (s.scene.is_metric ? "m" : "f")); - showTurnSpeedLimit = tsc_speed > 0.0 && (tsc_speed < speed || s.scene.show_debug_ui); + showTurnSpeedLimit = tsc_speed > 0.0 && std::round(tsc_speed) < 224 && (tsc_speed < speed || s.scene.show_debug_ui); turnSpeedLimit = QString::number(std::nearbyint(tsc_speed)); tscSubText = t_distance > 0 ? t_distance_str : QString(""); tscActive = tscState > cereal::LongitudinalPlanSP::SpeedLimitControlState::TEMP_INACTIVE; @@ -1097,8 +1097,8 @@ void AnnotatedCameraWidget::drawTrunSpeedSign(QPainter &p, QRect rc, const QStri const QColor text_color = QColor(0, 0, 0, is_active ? 255 : 85); const int x = rc.center().x(); - const int y = rc.center().y(); - const int width = rc.width(); + const int y = 184 * 2 + UI_BORDER_SIZE + 202; + const int width = 184; const float stroke_w = 15.0; const float cS = stroke_w / 2.0 + 4.5; // half width of the stroke on the corners of the triangle