diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 712b9911af..9fe9b1bd15 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -84,7 +84,7 @@ function launch { # start manager cd selfdrive/manager - ./custom_dep.py && ./build.py && ./manager.py + ./build.py && ./manager.py # if broken, keep on screen error while true; do sleep 1; done diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 393f53e4b7..12784558e8 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -37,14 +37,6 @@ CRUISE_INTERVAL_SIGN = { ButtonType.decelCruise: -1, } -# Constants for Limit controllers. -LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. -LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. -LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. -LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. -LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. -LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. - class VCruiseHelper: def __init__(self, CP): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f988cadb05..7a8dcf0581 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,9 +15,6 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error -from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController -from openpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver -from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController from openpilot.selfdrive.controls.lib.events import Events from openpilot.system.swaglog import cloudlog @@ -69,11 +66,7 @@ class LongitudinalPlanner: self.read_param() self.personality = log.LongitudinalPersonality.standard - self.cruise_source = 'cruise' - self.vision_turn_controller = VisionTurnController(CP) - self.speed_limit_controller = SpeedLimitController() self.events = Events() - self.turn_speed_controller = TurnSpeedController() def read_param(self): try: @@ -135,21 +128,15 @@ class LongitudinalPlanner: if force_slow_decel: v_cruise = 0.0 - - # Get acceleration and active solutions for custom long mpc. - self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions( - not reset_state and self.CP.openpilotLongitudinalControl, self.v_desired_filter.x, - self.a_desired, v_cruise, sm) - # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol) + accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_weights(prev_accel_constraint, personality=self.personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) @@ -195,48 +182,6 @@ class LongitudinalPlanner: longitudinalPlanSP = plan_sp_send.longitudinalPlanSP - longitudinalPlanSP.longitudinalPlanSource = self.mpc.source if self.mpc.source != 'cruise' else self.cruise_source - - longitudinalPlanSP.visionTurnControllerState = self.vision_turn_controller.state - longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_turn) - - longitudinalPlanSP.speedLimitControlState = self.speed_limit_controller.state - longitudinalPlanSP.speedLimit = float(self.speed_limit_controller.speed_limit) - longitudinalPlanSP.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset) - longitudinalPlanSP.distToSpeedLimit = float(self.speed_limit_controller.distance) - longitudinalPlanSP.isMapSpeedLimit = bool(self.speed_limit_controller.source == SpeedLimitResolver.Source.map_data) 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) - pm.send('longitudinalPlanSP', plan_sp_send) - - def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm): - # Update controllers - self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm) - self.events = Events() - self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.events) - self.turn_speed_controller.update(enabled, v_ego, a_ego, sm) - - # Pick solution with the lowest velocity target. - a_solutions = {'cruise': float("inf")} - v_solutions = {'cruise': v_cruise} - - if self.vision_turn_controller.is_active: - a_solutions['turn'] = self.vision_turn_controller.a_target - v_solutions['turn'] = self.vision_turn_controller.v_turn - - if self.speed_limit_controller.is_active: - a_solutions['limit'] = self.speed_limit_controller.a_target - v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted - - if self.turn_speed_controller.is_active: - a_solutions['turnlimit'] = self.turn_speed_controller.a_target - v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit - - source = min(v_solutions, key=v_solutions.get) - - return source, a_solutions[source], v_solutions[source] diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py deleted file mode 100644 index cc5769017d..0000000000 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ /dev/null @@ -1,377 +0,0 @@ -import numpy as np -import time -from common.numpy_fast import interp -from enum import IntEnum -from cereal import custom, car -from common.params import Params -from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \ - LIMIT_MAX_MAP_DATA_AGE, CONTROL_N -from selfdrive.controls.lib.events import Events -from selfdrive.modeld.constants import ModelConstants - - -_PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates. -_TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal. - -# Lookup table for speed limit percent offset depending on speed. -_LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h -_LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h - -SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState -EventName = car.CarEvent.EventName - -_DEBUG = False - - -def _debug(msg): - if not _DEBUG: - return - print(msg) - - -def _description_for_state(speed_limit_control_state): - if speed_limit_control_state == SpeedLimitControlState.inactive: - return 'INACTIVE' - if speed_limit_control_state == SpeedLimitControlState.tempInactive: - return 'TEMP_INACTIVE' - if speed_limit_control_state == SpeedLimitControlState.adapting: - return 'ADAPTING' - if speed_limit_control_state == SpeedLimitControlState.active: - return 'ACTIVE' - - -class SpeedLimitResolver(): - class Source(IntEnum): - none = 0 - car_state = 1 - map_data = 2 - - class Policy(IntEnum): - car_state_only = 0 - map_data_only = 1 - car_state_priority = 2 - map_data_priority = 3 - combined = 4 - - def __init__(self, policy=Policy.map_data_priority): - self._limit_solutions = {} # Store for speed limit solutions from different sources - self._distance_solutions = {} # Store for distance to current speed limit start for different sources - self._v_ego = 0. - self._current_speed_limit = 0. - self._policy = policy - self._next_speed_limit_prev = 0. - self.speed_limit = 0. - self.distance = 0. - self.source = SpeedLimitResolver.Source.none - - def resolve(self, v_ego, current_speed_limit, sm): - self._v_ego = v_ego - self._current_speed_limit = current_speed_limit - self._sm = sm - - self._get_from_car_state() - self._get_from_map_data() - self._consolidate() - - return self.speed_limit, self.distance, self.source - - def _get_from_car_state(self): - self._limit_solutions[SpeedLimitResolver.Source.car_state] = self._sm['carState'].cruiseState.speedLimit - self._distance_solutions[SpeedLimitResolver.Source.car_state] = 0. - - def _get_from_map_data(self): - # Ignore if no live map data - sock = 'liveMapDataSP' - if self._sm.logMonoTime[sock] is None: - self._limit_solutions[SpeedLimitResolver.Source.map_data] = 0. - self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0. - _debug('SL: No map data for speed limit') - return - - # Load limits from map_data - map_data = self._sm[sock] - speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. - next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 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: - self._limit_solutions[SpeedLimitResolver.Source.map_data] = 0. - self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0. - _debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') - return - - # When we have no ahead speed limit to consider or it is greater than current speed limit - # or car has stopped, then provide current value and reset tracking. - if next_speed_limit == 0. or self._v_ego <= 0. or next_speed_limit > self._current_speed_limit: - self._limit_solutions[SpeedLimitResolver.Source.map_data] = speed_limit - self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0. - self._next_speed_limit_prev = 0. - return - - # Calculate the actual distance to the speed limit ahead corrected by gps_fix_age - distance_since_fix = self._v_ego * gps_fix_age - distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) - - # 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. - if next_speed_limit == self._next_speed_limit_prev: - self._limit_solutions[SpeedLimitResolver.Source.map_data] = next_speed_limit - self._distance_solutions[SpeedLimitResolver.Source.map_data] = distance_to_speed_limit_ahead - return - - # Reset tracking - self._next_speed_limit_prev = 0. - - # Calculated the time needed to adapt to the new limit and the corresponding distance. - adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC - adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time**2 - - # When we detect we are close enough, we provide the next limit value and track it. - if distance_to_speed_limit_ahead <= adapt_distance: - self._limit_solutions[SpeedLimitResolver.Source.map_data] = next_speed_limit - self._distance_solutions[SpeedLimitResolver.Source.map_data] = distance_to_speed_limit_ahead - self._next_speed_limit_prev = next_speed_limit - return - - # Otherwise we just provide the map data speed limit. - self.distance_to_map_speed_limit = 0. - self._limit_solutions[SpeedLimitResolver.Source.map_data] = speed_limit - self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0. - - def _consolidate(self): - limits = np.array([], dtype=float) - distances = np.array([], dtype=float) - sources = np.array([], dtype=int) - - if self._policy == SpeedLimitResolver.Policy.car_state_only or \ - self._policy == SpeedLimitResolver.Policy.car_state_priority or \ - self._policy == SpeedLimitResolver.Policy.combined: - limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.car_state]) - distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.car_state]) - sources = np.append(sources, SpeedLimitResolver.Source.car_state.value) - - if self._policy == SpeedLimitResolver.Policy.map_data_only or \ - self._policy == SpeedLimitResolver.Policy.map_data_priority or \ - self._policy == SpeedLimitResolver.Policy.combined: - limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.map_data]) - distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.map_data]) - sources = np.append(sources, SpeedLimitResolver.Source.map_data.value) - - if np.amax(limits) == 0.: - if self._policy == SpeedLimitResolver.Policy.car_state_priority: - limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.map_data]) - distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.map_data]) - sources = np.append(sources, SpeedLimitResolver.Source.map_data.value) - - elif self._policy == SpeedLimitResolver.Policy.map_data_priority: - limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.car_state]) - distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.car_state]) - sources = np.append(sources, SpeedLimitResolver.Source.car_state.value) - - # Get all non-zero values and set the minimum if any, otherwise 0. - mask = limits > 0. - limits = limits[mask] - distances = distances[mask] - sources = sources[mask] - - if len(limits) > 0: - min_idx = np.argmin(limits) - self.speed_limit = limits[min_idx] - self.distance = distances[min_idx] - self.source = SpeedLimitResolver.Source(sources[min_idx]) - else: - self.speed_limit = 0. - self.distance = 0. - self.source = SpeedLimitResolver.Source.none - - _debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}') - - -class SpeedLimitController(): - def __init__(self): - self._params = Params() - self._resolver = SpeedLimitResolver() - self._last_params_update = 0.0 - self._last_op_enabled_time = 0.0 - self._is_metric = self._params.get_bool("IsMetric") - self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset") - self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator") - self._op_enabled = False - self._op_enabled_prev = False - self._v_ego = 0. - self._a_ego = 0. - self._v_offset = 0. - self._v_cruise_setpoint = 0. - self._v_cruise_setpoint_prev = 0. - self._v_cruise_setpoint_changed = False - self._speed_limit = 0. - self._speed_limit_prev = 0. - self._speed_limit_changed = False - self._distance = 0. - self._source = SpeedLimitResolver.Source.none - self._state = SpeedLimitControlState.inactive - self._state_prev = SpeedLimitControlState.inactive - self._gas_pressed = False - self._a_target = 0. - - @property - def a_target(self): - return self._a_target if self.is_active else self._a_ego - - @property - def state(self): - return self._state - - @state.setter - def state(self, value): - if value != self._state: - _debug(f'Speed Limit Controller state: {_description_for_state(value)}') - - if value == SpeedLimitControlState.tempInactive: - # Reset previous speed limit to current value as to prevent going out of tempInactive in - # a single cycle when the speed limit changes at the same time the user has temporarily deactivate it. - self._speed_limit_prev = self._speed_limit - - self._state = value - - @property - def is_active(self): - return self.state > SpeedLimitControlState.tempInactive - - @property - def speed_limit_offseted(self): - return self._speed_limit + self.speed_limit_offset - - @property - def speed_limit_offset(self): - if self._offset_enabled: - return interp(self._speed_limit, _LIMIT_PERC_OFFSET_BP, _LIMIT_PERC_OFFSET_V) * self._speed_limit - return 0. - - @property - def speed_limit(self): - return self._speed_limit - - @property - def distance(self): - return self._distance - - @property - def source(self): - return self._source - - def _update_params(self): - t = time.monotonic() - if t > self._last_params_update + _PARAMS_UPDATE_PERIOD: - self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset") - _debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_enabled}') - self._last_params_update = t - - def _update_calculations(self): - # Update current velocity offset (error) - self._v_offset = self.speed_limit_offseted - self._v_ego - - # Track the time op becomes active to prevent going to tempInactive right away after - # op enabling since controlsd will change the cruise speed every time on enabling and this will - # cause a temp inactive transition if the controller is updated before controlsd sets actual cruise - # speed. - if not self._op_enabled_prev and self._op_enabled: - self._last_op_enabled_time = time.monotonic() - - # Update change tracking variables - self._speed_limit_changed = self._speed_limit != self._speed_limit_prev - self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev - self._speed_limit_prev = self._speed_limit - self._v_cruise_setpoint_prev = self._v_cruise_setpoint - self._op_enabled_prev = self._op_enabled - - def _state_transition(self): - self._state_prev = self._state - - # In any case, if op is disabled, or speed limit control is disabled - # or the reported speed limit is 0 or gas is pressed, deactivate. - if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or (self._gas_pressed and self._disengage_on_accelerator): - self.state = SpeedLimitControlState.inactive - return - - # In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed. - # Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations - # due to controlsd logic changing cruise setpoint when going active. - if self._v_cruise_setpoint_changed and \ - time.monotonic() > (self._last_op_enabled_time + _TEMP_INACTIVE_GUARD_PERIOD): - self.state = SpeedLimitControlState.tempInactive - return - - # inactive - if self.state == SpeedLimitControlState.inactive: - # If the limit speed offset is negative (i.e. reduce speed) and lower than threshold - # we go to adapting state to quickly reduce speed, otherwise we go directly to active - if self._v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting - else: - self.state = SpeedLimitControlState.active - # tempInactive - elif self.state == SpeedLimitControlState.tempInactive: - # if speed limit changes, transition to inactive, - # proper active state will be set on next iteration. - if self._speed_limit_changed: - self.state = SpeedLimitControlState.inactive - # adapting - elif self.state == SpeedLimitControlState.adapting: - # Go to active once the speed offset is over threshold. - if self._v_offset >= LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.active - # active - elif self.state == SpeedLimitControlState.active: - # Go to adapting if the speed offset goes below threshold. - if self._v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting - - def _update_solution(self): - # inactive or tempInactive state - if self.state <= SpeedLimitControlState.tempInactive: - # Preserve current values - a_target = self._a_ego - # adapting - elif self.state == SpeedLimitControlState.adapting: - # When adapting we target to achieve the speed limit on the distance if not there yet, - # otherwise try to keep the speed constant around the control time horizon. - if self.distance > 0: - a_target = (self.speed_limit_offseted**2 - self._v_ego**2) / (2. * self.distance) - else: - a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N] - # active - elif self.state == SpeedLimitControlState.active: - # When active we are trying to keep the speed constant around the control time horizon. - a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N] - - # Keep solution limited. - self._a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC) - - def _update_events(self, events): - if not self.is_active: - # no event while inactive - return - - if self._state_prev <= SpeedLimitControlState.tempInactive: - events.add(EventName.speedLimitActive) - elif self._speed_limit_changed != 0: - events.add(EventName.speedLimitValueChange) - - def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, events=Events()): - self._op_enabled = enabled - self._v_ego = v_ego - self._a_ego = a_ego - self._v_cruise_setpoint = v_cruise_setpoint - self._gas_pressed = sm['carState'].gasPressed - - self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm) - - self._update_params() - self._update_calculations() - self._state_transition() - self._update_solution() - self._update_events(events) diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py deleted file mode 100644 index 28caa105ae..0000000000 --- a/selfdrive/controls/lib/turn_speed_controller.py +++ /dev/null @@ -1,243 +0,0 @@ -import numpy as np -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 selfdrive.modeld.constants import ModelConstants - - -_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. - - -_DEBUG = False - -TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState - - -def _debug(msg): - if not _DEBUG: - return - print(msg) - - -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(): - def __init__(self): - self._params = Params() - self._last_params_update = 0. - self._is_enabled = self._params.get_bool("TurnSpeedControl") - 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._state = TurnSpeedControlState.inactive - - self._next_speed_limit_prev = 0. - - self._a_target = 0. - - @property - def a_target(self): - return self._a_target if self.is_active else self._a_ego - - @property - def state(self): - return self._state - - @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 - def is_active(self): - 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. - - @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): - t = time.monotonic() - if t > self._last_params_update + 5.0: - self._is_enabled = self._params.get_bool("TurnSpeedControl") - self._last_params_update = t - - def _update_calculations(self): - # Update current velocity offset (error) - self._v_offset = self.speed_limit - self._v_ego - - 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.: - 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: - self.state = TurnSpeedControlState.tempInactive - return - - # 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: - self.state = TurnSpeedControlState.active - # tempInactive - 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.: - self.state = TurnSpeedControlState.active - # 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 - - def _update_solution(self): - # inactive or tempInactive state - if self.state <= TurnSpeedControlState.tempInactive: - # Preserve current values - a_target = self._a_ego - # adapting - elif self.state == TurnSpeedControlState.adapting: - # When adapting we target to achieve the speed limit on the distance. - a_target = (self.speed_limit**2 - self._v_ego**2) / (2. * self.distance) - a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC) - # active - elif self.state == TurnSpeedControlState.active: - # When active we are trying to keep the speed constant around the control time horizon. - # but under constrained acceleration limits since we are in a turn. - a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N] - a_target = np.clip(a_target, _ACTIVE_LIMIT_MIN_ACC, _ACTIVE_LIMIT_MAX_ACC) - - # update solution values. - self._a_target = a_target - - def update(self, enabled, v_ego, a_ego, sm): - self._op_enabled = enabled - self._v_ego = v_ego - self._a_ego = a_ego - - # 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._update_solution() diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py deleted file mode 100644 index 96561ee10f..0000000000 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ /dev/null @@ -1,293 +0,0 @@ -import numpy as np -import math -import time -from cereal import custom -from common.numpy_fast import interp -from common.params import Params -from common.conversions import Conversions as CV -from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX - - -_MIN_V = 5.6 # Do not operate under 20km/h - -_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state. -_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops. - -_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning turn state. - -_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state. -_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger end of turn cycle. - -_EVAL_STEP = 5. # mts. Resolution of the curvature evaluation. -_EVAL_START = 20. # mts. Distance ahead where to start evaluating vision curvature. -_EVAL_LENGHT = 150. # mts. Distance ahead where to stop evaluating vision curvature. -_EVAL_RANGE = np.arange(_EVAL_START, _EVAL_LENGHT, _EVAL_STEP) - -_A_LAT_REG_MAX = 2. # Maximum lateral acceleration - -_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting. - -# Lookup table for the minimum smooth deceleration during the ENTERING state -# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. -_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state -_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead - -# Lookup table for the acceleration for the TURNING state -# depending on the current lateral acceleration of the vehicle. -_TURNING_ACC_V = [0.5, 0., -0.4] # acc value -_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc - -_LEAVING_ACC = 0.5 # Confortble acceleration to regain speed while leaving a turn. - -_MIN_LANE_PROB = 0.6 # Minimum lanes probability to allow curvature prediction based on lanes. - -_DEBUG = False - - -def _debug(msg): - if not _DEBUG: - return - print(msg) - - -VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState - - -def eval_curvature(poly, x_vals): - """ - This function returns a vector with the curvature based on path defined by `poly` - evaluated on distance vector `x_vals` - """ - # https://en.wikipedia.org/wiki/Curvature# Local_expressions - def curvature(x): - a = abs(2 * poly[1] + 6 * poly[0] * x) / (1 + (3 * poly[0] * x**2 + 2 * poly[1] * x + poly[2])**2)**(1.5) - return a - - return np.vectorize(curvature)(x_vals) - - -def eval_lat_acc(v_ego, x_curv): - """ - This function returns a vector with the lateral acceleration based - for the provided speed `v_ego` evaluated over curvature vector `x_curv` - """ - - def lat_acc(curv): - a = v_ego**2 * curv - return a - - return np.vectorize(lat_acc)(x_curv) - - -def _description_for_state(turn_controller_state): - if turn_controller_state == VisionTurnControllerState.disabled: - return 'DISABLED' - if turn_controller_state == VisionTurnControllerState.entering: - return 'ENTERING' - if turn_controller_state == VisionTurnControllerState.turning: - return 'TURNING' - if turn_controller_state == VisionTurnControllerState.leaving: - return 'LEAVING' - - -class VisionTurnController(): - def __init__(self, CP): - self._params = Params() - self._CP = CP - self._op_enabled = False - self._gas_pressed = False - self._is_enabled = self._params.get_bool("TurnVisionControl") - self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator") - self._last_params_update = 0. - self._v_cruise_setpoint = 0. - self._v_ego = 0. - self._a_ego = 0. - self._a_target = 0. - self._v_overshoot = 0. - self._state = VisionTurnControllerState.disabled - - self._reset() - - @property - def state(self): - return self._state - - @state.setter - def state(self, value): - if value != self._state: - _debug(f'TVC: TurnVisionController state: {_description_for_state(value)}') - if value == VisionTurnControllerState.disabled: - self._reset() - self._state = value - - @property - def a_target(self): - return self._a_target if self.is_active else self._a_ego - - @property - def v_turn(self): - if not self.is_active: - return self._v_cruise_setpoint - return self._v_overshoot if self._lat_acc_overshoot_ahead \ - else self._v_ego + self._a_target * _NO_OVERSHOOT_TIME_HORIZON - - @property - def is_active(self): - return self._state != VisionTurnControllerState.disabled - - def _reset(self): - self._current_lat_acc = 0. - self._max_v_for_current_curvature = 0. - self._max_pred_lat_acc = 0. - self._v_overshoot_distance = 200. - self._lat_acc_overshoot_ahead = False - - def _update_params(self): - t = time.monotonic() - if t > self._last_params_update + 5.0: - self._is_enabled = self._params.get_bool("TurnVisionControl") - self._last_params_update = t - - def _update_calculations(self, sm): - # Get path polynomial approximation for curvature estimation from model data. - path_poly = None - model_data = sm['modelV2'] if sm.valid.get('modelV2', False) else None - lat_planner_data = sm['lateralPlanSP'] if sm.valid.get('lateralPlanSP', False) else None - - # 1. When the probability of lanes is good enough, compute polynomial from lanes as they are way more stable - # on current mode than drving path. - if model_data is not None and len(model_data.laneLines) == 4 and len(model_data.laneLines[0].t) == TRAJECTORY_SIZE: - ll_x = model_data.laneLines[1].x # left and right ll x is the same - lll_y = np.array(model_data.laneLines[1].y) - rll_y = np.array(model_data.laneLines[2].y) - l_prob = model_data.laneLineProbs[1] - r_prob = model_data.laneLineProbs[2] - lll_std = model_data.laneLineStds[1] - rll_std = model_data.laneLineStds[2] - - # Reduce reliance on lanelines that are too far apart or will be in a few seconds - width_pts = rll_y - lll_y - prob_mods = [] - for t_check in [0.0, 1.5, 3.0]: - width_at_t = interp(t_check * (self._v_ego + 7), ll_x, width_pts) - prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) - mod = min(prob_mods) - l_prob *= mod - r_prob *= mod - - # Reduce reliance on uncertain lanelines - l_std_mod = interp(lll_std, [.15, .3], [1.0, 0.0]) - r_std_mod = interp(rll_std, [.15, .3], [1.0, 0.0]) - l_prob *= l_std_mod - r_prob *= r_std_mod - - # Find path from lanes as the average center lane only if min probability on both lanes is above threshold. - if l_prob > _MIN_LANE_PROB and r_prob > _MIN_LANE_PROB: - c_y = width_pts / 2 + lll_y - path_poly = np.polyfit(ll_x, c_y, 3) - - # 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as - # provided by `lateralPlanner`. - if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \ - and lat_planner_data.dPathWLinesX[0] > 0: - path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3) - - # 3. If no polynomial derived from lanes or driving path, then provide a straight line poly. - if path_poly is None: - path_poly = np.array([0., 0., 0., 0.]) - - current_curvature = abs( - sm['carState'].steeringAngleDeg * CV.DEG_TO_RAD / (self._CP.steerRatio * self._CP.wheelbase)) - self._current_lat_acc = current_curvature * self._v_ego**2 - self._max_v_for_current_curvature = math.sqrt(_A_LAT_REG_MAX / current_curvature) if current_curvature > 0 \ - else V_CRUISE_MAX * CV.KPH_TO_MS - - pred_curvatures = eval_curvature(path_poly, _EVAL_RANGE) - max_pred_curvature = np.amax(pred_curvatures) - self._max_pred_lat_acc = self._v_ego**2 * max_pred_curvature - - max_curvature_for_vego = _A_LAT_REG_MAX / max(self._v_ego, 0.1)**2 - lat_acc_overshoot_idxs = np.nonzero(pred_curvatures >= max_curvature_for_vego)[0] - self._lat_acc_overshoot_ahead = len(lat_acc_overshoot_idxs) > 0 - - if self._lat_acc_overshoot_ahead: - self._v_overshoot = min(math.sqrt(_A_LAT_REG_MAX / max_pred_curvature), self._v_cruise_setpoint) - self._v_overshoot_distance = max(lat_acc_overshoot_idxs[0] * _EVAL_STEP + _EVAL_START, _EVAL_STEP) - _debug(f'TVC: High LatAcc. Dist: {self._v_overshoot_distance:.2f}, v: {self._v_overshoot * CV.MS_TO_KPH:.2f}') - - def _state_transition(self): - # In any case, if system is disabled or the feature is disabeld or gas is pressed, disable. - if not self._op_enabled or not self._is_enabled or (self._gas_pressed and self._disengage_on_accelerator): - self.state = VisionTurnControllerState.disabled - return - - # DISABLED - if self.state == VisionTurnControllerState.disabled: - # Do not enter a turn control cycle if speed is low. - if self._v_ego <= _MIN_V: - pass - # If substantial lateral acceleration is predicted ahead, then move to Entering turn state. - elif self._max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: - self.state = VisionTurnControllerState.entering - # ENTERING - elif self.state == VisionTurnControllerState.entering: - # Transition to Turning if current lateral acceleration is over the threshold. - if self._current_lat_acc >= _TURNING_LAT_ACC_TH: - self.state = VisionTurnControllerState.turning - # Abort if the predicted lateral acceleration drops - elif self._max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH: - self.state = VisionTurnControllerState.disabled - # TURNING - elif self.state == VisionTurnControllerState.turning: - # Transition to Leaving if current lateral acceleration drops drops below threshold. - if self._current_lat_acc <= _LEAVING_LAT_ACC_TH: - self.state = VisionTurnControllerState.leaving - # LEAVING - elif self.state == VisionTurnControllerState.leaving: - # Transition back to Turning if current lateral acceleration goes back over the threshold. - if self._current_lat_acc >= _TURNING_LAT_ACC_TH: - self.state = VisionTurnControllerState.turning - # Finish if current lateral acceleration goes below threshold. - elif self._current_lat_acc < _FINISH_LAT_ACC_TH: - self.state = VisionTurnControllerState.disabled - - def _update_solution(self): - # DISABLED - if self.state == VisionTurnControllerState.disabled: - # when not overshooting, calculate v_turn as the speed at the prediction horizon when following - # the smooth deceleration. - a_target = self._a_ego - # ENTERING - elif self.state == VisionTurnControllerState.entering: - # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come. - a_target = interp(self._max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) - if self._lat_acc_overshoot_ahead: - # when overshooting, target the acceleration needed to achieve the overshoot speed at - # the required distance - a_target = min((self._v_overshoot**2 - self._v_ego**2) / (2 * self._v_overshoot_distance), a_target) - _debug(f'TVC Entering: Overshooting: {self._lat_acc_overshoot_ahead}') - _debug(f' Decel: {a_target:.2f}, target v: {self.v_turn * CV.MS_TO_KPH}') - # TURNING - elif self.state == VisionTurnControllerState.turning: - # When turning we provide a target acceleration that is comfortable for the lateral accelearation felt. - a_target = interp(self._current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) - # LEAVING - elif self.state == VisionTurnControllerState.leaving: - # When leaving we provide a comfortable acceleration to regain speed. - a_target = _LEAVING_ACC - - # update solution values. - self._a_target = a_target - - def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm): - self._op_enabled = enabled - self._gas_pressed = sm['carState'].gasPressed - self._v_ego = v_ego - self._a_ego = a_ego - self._v_cruise_setpoint = v_cruise_setpoint - - self._update_params() - self._update_calculations(sm) - self._state_transition() - self._update_solution() diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 863396ac6b..4b355842f7 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -42,7 +42,7 @@ def plannerd_thread(): lateral_planner = LateralPlanner(CP, debug=debug_mode) pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan', 'longitudinalPlanSP', 'lateralPlanSP']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlanSP', 'liveMapDataSP'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) while True: diff --git a/selfdrive/manager/custom_dep.py b/selfdrive/manager/custom_dep.py deleted file mode 100755 index 3e32d80000..0000000000 --- a/selfdrive/manager/custom_dep.py +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import errno -import shutil -import time -from common.basedir import BASEDIR -from urllib.request import urlopen -from glob import glob -import subprocess -import importlib.util - -# NOTE: Do NOT import anything here that needs be built (e.g. params) -from common.spinner import Spinner - - -sys.path.append(os.path.join(BASEDIR, "third_party/mapd")) -OPSPLINE_SPEC = importlib.util.find_spec('scipy') -OVERPY_SPEC = importlib.util.find_spec('overpy') -MAX_BUILD_PROGRESS = 100 -TMP_DIR = '/data/tmp' -THIRD_PARTY_DIR = '/data/openpilot/third_party/mapd' -THIRD_PARTY_DIR_SP = '/data/third_party_community' - - -def wait_for_internet_connection(return_on_failure=False): - retries = 0 - while True: - try: - _ = urlopen('https://www.google.com/', timeout=10) - return True - except Exception as e: - print(f'Wait for internet failed: {e}') - if return_on_failure and retries == 15: - return False - retries += 1 - time.sleep(2) # Wait for 2 seconds before retrying - - -def install_dep(spinner): - wait_for_internet_connection() - - TOTAL_PIP_STEPS = 2986 - - try: - os.makedirs(TMP_DIR) - except OSError as e: - if e.errno != errno.EEXIST: - raise - my_env = os.environ.copy() - my_env['TMPDIR'] = TMP_DIR - - pip_target = [f'--target={THIRD_PARTY_DIR}'] - packages = [] - if OPSPLINE_SPEC is None: - packages.append('scipy==1.11.1') - if OVERPY_SPEC is None: - packages.append('overpy==0.6') - - pip = subprocess.Popen([sys.executable, "-m", "pip", "install", "-v"] + pip_target + packages, - stdout=subprocess.PIPE, env=my_env) - - # Read progress from pip and update spinner - steps = 0 - while True: - output = pip.stdout.readline() - if pip.poll() is not None: - break - if output: - steps += 1 - spinner.update_progress(MAX_BUILD_PROGRESS * min(1., steps / TOTAL_PIP_STEPS), 100.) - print(output.decode('utf8', 'replace')) - - shutil.rmtree(TMP_DIR) - os.unsetenv('TMPDIR') - - # remove numpy installed to THIRD_PARTY_DIR since numpy is already present in the AGNOS image - if OPSPLINE_SPEC is None: - for directory in glob(f'{THIRD_PARTY_DIR}/numpy*'): - shutil.rmtree(directory) - if os.path.exists(f'{THIRD_PARTY_DIR}/bin'): - shutil.rmtree(f'{THIRD_PARTY_DIR}/bin') - - dup = f'cp -rf {THIRD_PARTY_DIR} {THIRD_PARTY_DIR_SP}' - process_dup = subprocess.Popen(dup, stdout=subprocess.PIPE, shell=True) - - -if __name__ == "__main__" and (OPSPLINE_SPEC is None or OVERPY_SPEC is None): - spinner = Spinner() - if os.path.exists(THIRD_PARTY_DIR_SP): - spinner.update("Loading dependencies") - command = f'rm -rf {THIRD_PARTY_DIR}; cp -rf {THIRD_PARTY_DIR_SP} {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - print(f"SP_LOG: Removed directory {THIRD_PARTY_DIR}") - print(f"SP_LOG: Copied {THIRD_PARTY_DIR_SP} to {THIRD_PARTY_DIR}") - else: - spinner.update("Waiting for internet") - install_dep(spinner) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 81677133f5..ed84d9380d 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -25,9 +25,6 @@ from openpilot.system.version import is_dirty, get_commit, get_version, get_orig is_tested_branch, is_release_branch -sys.path.append(os.path.join(BASEDIR, "third_party/mapd")) - - def manager_init() -> None: # update system time from panda set_time(cloudlog) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index f8ae579d5b..4313ace969 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -84,7 +84,6 @@ procs = [ PythonProcess("gpxd", "selfdrive.gpxd.gpxd", only_onroad), PythonProcess("gpxd_uploader", "selfdrive.gpxd.gpx_uploader", always_run), - PythonProcess("mapd", "selfdrive.mapd.mapd", only_onroad), PythonProcess("fleet_manager", "system.fleetmanager.fleet_manager", only_offroad), # debug procs diff --git a/selfdrive/mapd/README.md b/selfdrive/mapd/README.md deleted file mode 100644 index bd3a3c9497..0000000000 --- a/selfdrive/mapd/README.md +++ /dev/null @@ -1,8 +0,0 @@ -# MapD -The OpenStreetMap-based speed logical by the [Move Fast team](https://github.com/move-fast), [dragonpilot team](https://github.com/dragonpilot-community/dragonpilot), and additional improvements by [sunnypilot](https://github.com/sunnyhaibin/sunnypilot). - -The comma three uses regular SciPy. To have a better experience with `mapd`, please go to [OpenStreetMap](https://openstreetmap.org) to update and improve your area's data (i.e., Speed Limit, Stop Signs, Traffic Lights). - -To use `mapd`, you consent to `mapd` uploading the traces. You may opt out of uploading traces at any time. - -© OpenStreetMap contributors diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py deleted file mode 100644 index 1ad834acd6..0000000000 --- a/selfdrive/mapd/config.py +++ /dev/null @@ -1,7 +0,0 @@ -# Map query config - -QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. -MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query. -FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. -LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg. -LANE_WIDTH = 3.7 # Lane width estimate. Used for detecting departures from way. diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py deleted file mode 100644 index a4cbf06b5c..0000000000 --- a/selfdrive/mapd/lib/NodesData.py +++ /dev/null @@ -1,430 +0,0 @@ -import numpy as np -from enum import Enum -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors - -from scipy.interpolate import splev, splprep - - -_TURN_CURVATURE_THRESHOLD = 0.002 # 1/mts. A curvature over this value will generate a speed limit section. -_MAX_LAT_ACC = 2. # Maximum lateral acceleration in turns. -_SPLINE_EVAL_STEP = 5 # mts for spline evaluation for curvature calculation -_MIN_SPEED_SECTION_LENGTH = 100. # mts. Sections below this value will not be split in smaller sections. -_MAX_CURV_DEVIATION_FOR_SPLIT = 2. # Split a speed section if the max curvature deviates from mean by this factor. -_MAX_CURV_SPLIT_ARC_ANGLE = 90. # degrees. Arc section to split into new speed section around max curvature. -_MIN_NODE_DISTANCE = 50. # mts. Minimum distance between nodes for spline evaluation. Data is enhanced if not met. -_ADDED_NODES_DIST = 15. # mts. Distance between added nodes when data is enhanced for spline evaluation. -_DIVERTION_SEARCH_RANGE = [-200., 50.] # mt. Range of distance to current location for diversion search. - - -def nodes_raw_data_array_for_wr(wr, drop_last=False): - """Provides an array of raw node data (id, lat, lon, speed_limit, advisory_speed_limit) for all nodes in way relation - """ - sl = wr.speed_limit - asl = wr.advisory_speed_limit - data = np.array([(n.id, n.lat, n.lon, sl, asl) for n in wr.way.nodes], dtype=float) - - # reverse the order if way direction is backwards - if wr.direction == DIRECTION.BACKWARD: - data = np.flip(data, axis=0) - - # drop last if requested - return data[:-1] if drop_last else data - - -def node_calculations(points): - """Provides node calculations based on an array of (lat, lon) points in radians. - points is a (N x 1) array where N >= 3 - """ - if len(points) < 3: - raise(IndexError) - - # Get the vector representation of node points in cartesian plane. - # (N-1, 2) array. Not including (0., 0.) - v = vectors(points) * R - - # Calculate the vector magnitudes (or distance) - # (N-1, 1) array. No distance for v[-1] - d = np.linalg.norm(v, axis=1) - - # Calculate the bearing (from true north clockwise) for every node. - # (N-1, 1) array. No bearing for v[-1] - b = np.arctan2(v[:, 0], v[:, 1]) - - # Add origin to vector space. (i.e first node in list) - v = np.concatenate(([[0., 0.]], v)) - - # Provide distance to previous node and distance to next node - dp = np.concatenate(([0.], d)) - dn = np.concatenate((d, [0.])) - - # Provide cumulative distance on route - dr = np.cumsum(dp, axis=0) - - # Bearing of last node should keep bearing from previous. - b = np.concatenate((b, [b[-1]])) - - return v, dp, dn, dr, b - - -def spline_curvature_calculations(vect, dist_prev): - """Provides an array of curvatures and its distances by applying a spline interpolation - to the path described by the nodes data. - """ - # We need to artificially enhance the data before applying spline interpolation to avoid getting - # inexistent curvature values close to irregularities on the road when the resolution of nodes data - # approaching the irregularity is low. - - # - Find indexes where dist_prev is greater than threshold - too_far_idxs = np.nonzero(dist_prev >= _MIN_NODE_DISTANCE)[0] - - # - Traversing in reverse order, enhance data by adding points at the found indexes. - for idx in too_far_idxs[::-1]: - dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors. - n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added. - new_v = vect[idx, :] / n # new relative vector to insert. - vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors. - vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors - - # Data is now enhanced, we can proceed with curvature evaluation. - # - Create cumulative arrays for distance traveled and vector (x, y) - ds = np.cumsum(dist_prev, axis=0) - vs = np.cumsum(vect, axis=0) - - # - spline interpolation - tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=unbalanced-tuple-unpacking - - # - evaluate every _SPLINE_EVAL_STEP mts. - n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) - unew = np.arange(0, n + 1) / n - - # - get derivatives - d1 = splev(unew, tck, der=1) - d2 = splev(unew, tck, der=2) - - # - calculate curvatures - num = d1[0] * d2[1] - d1[1] * d2[0] - den = (d1[0]**2 + d1[1]**2)**(1.5) - curv = num / den - curv_ds = unew * ds[-1] - - return curv, curv_ds - - -def speed_section(curv_sec): - """Map curvature section data into turn speed sections data. - Returns: [section start distance, section end distance, speed limit based on max curvature, sing of curvature] - """ - max_curv_idx = np.argmax(curv_sec[:, 0]) - start = np.amin(curv_sec[:, 2]) - end = np.amax(curv_sec[:, 2]) - - return np.array([start, end, np.sqrt(_MAX_LAT_ACC / curv_sec[max_curv_idx, 0]), curv_sec[max_curv_idx, 1]]) - - -def split_speed_section_by_sign(curv_sec): - """Will split the given curvature section in subsections if there is a change of sign on the curvature value - in the section. - """ - # Find the indexes where the curvatures change signs (if any). - c_idx = np.nonzero(np.diff(curv_sec[:, 1]))[0] + 1 - - # Split section base on change of sign. - return np.split(curv_sec, c_idx) - - -def split_speed_section_by_curv_degree(curv_sec): - """Will split the given curvature section in subsections as to isolate peaks of turn with substantially - higher curvature values. This will aid on preventing having very long turn sections with low speed limit - that is only really necessary for a small region of the section. - """ - # Only consider splitting a section if long enough. - length = curv_sec[-1, 2] - curv_sec[0, 2] - if length <= _MIN_SPEED_SECTION_LENGTH: - return [curv_sec] - - # Only split if max curvature deviates substantially from mean curvature. - max_curv_idx = np.argmax(curv_sec[:, 0]) - max_curv = curv_sec[max_curv_idx, 0] - mean_curv = np.mean(curv_sec[:, 0]) - if max_curv / mean_curv <= _MAX_CURV_DEVIATION_FOR_SPLIT: - return [curv_sec] - - # Calculate where to split as to isolate a curve section around the max curvature peak. - arc_side = (np.radians(_MAX_CURV_SPLIT_ARC_ANGLE) / max_curv) / 2. - arc_side_idx_lenght = int(np.ceil(arc_side / _SPLINE_EVAL_STEP)) - split_idxs = [max_curv_idx - arc_side_idx_lenght, max_curv_idx + arc_side_idx_lenght] - split_idxs = list(filter(lambda idx: idx > 0 and idx < len(curv_sec) - 1, split_idxs)) - - # If the arc section to split extendes outside the section, then no need to split. - if len(split_idxs) == 0: - return [curv_sec] - - # Create the splits and split the resulting sections recursevly. - splits = [split_speed_section_by_curv_degree(cs) for cs in np.split(curv_sec, split_idxs)] - - # Flatten the results and return the new list of curvature sections. - curv_secs = [cs for split in splits for cs in split] - return curv_secs - - -def speed_limits_for_curvatures_data(curv, dist): - """Provides the calculations for the speed limits from the curvatures array and distances, - by providing distances to curvature sections and corresponding speed limit values as well as - curvature direction/sign. - """ - # Prepare a data array for processing with absolute curvature values, curvature sign and distances. - curv_abs = np.abs(curv) - data = np.column_stack((curv_abs, np.sign(curv), dist)) - - # Find where curvatures overshoot turn curvature threshold and define as section - is_section = curv_abs >= _TURN_CURVATURE_THRESHOLD - - # Find the indexes where the sections start and end. i.e. change indexes. - c_idx = np.nonzero(np.diff(is_section))[0] + 1 - - # Create independent arrays for each split section base on change indexes. - splits = np.array(np.split(data, c_idx), dtype=object) - - # Filter the splits to keep only the curvature section arrays by getting the odd or even split arrays depending - # on whether the first split is a curvature split or not. - curv_sec_idxs = np.arange(0 if is_section[0] else 1, len(splits), 2, dtype=int) - curv_secs = splits[curv_sec_idxs] - - # Further split the curv sections by sign change - sub_secs = [split_speed_section_by_sign(cs) for cs in curv_secs] - curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec] - - # Further split the curv sections by degree of curvature - sub_secs = [split_speed_section_by_curv_degree(cs) for cs in curv_secs] - curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec] - - # Return an array where each row represents a turn speed limit section. - # [start, end, speed_limit, curvature_sign] - return np.array([speed_section(cs) for cs in curv_secs]) - -def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids): - """ - Evaluates if the way relation `wr` is a valid diversion from node with id `node_id`. - A valid diversion is a way relation with an edge node with the given `node_id` that is not already included - in the list of way relations in the route (`wr_ids`) and that can be travaled in the direction as if starting - from node with id `node_id` - """ - if wr.id in wr_ids: - return False - wr.update_direction_from_starting_node(node_id) - return not wr.is_prohibited - - -class SpeedLimitSection(): - """And object representing a speed limited road section ahead. - provides the start and end distance and the speed limit value - """ - def __init__(self, start, end, value): - self.start = start - self.end = end - self.value = value - - def __repr__(self): - return f'from: {self.start}, to: {self.end}, limit: {self.value}' - - -class TurnSpeedLimitSection(SpeedLimitSection): - def __init__(self, start, end, value, sign): - super().__init__(start, end, value) - self.curv_sign = sign - - def __repr__(self): - return f'{super().__repr__()}, sign: {self.curv_sign}' - - -class NodeDataIdx(Enum): - """Column index for data elements on NodesData underlying data store. - """ - node_id = 0 - lat = 1 - lon = 2 - speed_limit = 3 - advisory_speed_limit = 4 - x = 5 # x value of cartesian vector representing the section between last node and this node. - y = 6 # y value of cartesian vector representing the section between last node and this node. - dist_prev = 7 # distance to previous node. - dist_next = 8 # distance to next node - dist_route = 9 # cumulative distance on route - bearing = 10 # bearing of the vector departing from this node. - - -class NodesData: - """Container for the list of node data from a ordered list of way relations to be used in a Route - """ - def __init__(self, way_relations, wr_index): - self._nodes_data = np.array([]) - self._divertions = [[]] - self._curvature_speed_sections_data = np.array([]) - - way_count = len(way_relations) - if way_count == 0: - return - - # We want all the nodes from the last way section - nodes_data = nodes_raw_data_array_for_wr(way_relations[-1]) - - # For the ways before the last in the route we want all the nodes but the last, as that one is the first on - # the next section. Collect them, append last way node data and concatenate the numpy arrays. - if way_count > 1: - wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]]) - wrs_data += (nodes_data,) - nodes_data = np.concatenate(wrs_data) - - # Get a subarray with lat, lon to compute the remaining node values. - lat_lon_array = nodes_data[:, [1, 2]] - points = np.radians(lat_lon_array) - # Ensure we have more than 3 points, if not calculations are not possible. - if len(points) <= 3: - return - vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points) - - # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, advisory_speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] - self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing)) - - # Build route diversion options data from the wr_index. - wr_ids = [wr.id for wr in way_relations] - self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id) - if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)] - for node_id in nodes_data[:, 0]] - - # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process. - # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign] - if len(vect) > 3: - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds) - - @property - def count(self): - return len(self._nodes_data) - - def get(self, node_data_idx): - """Returns the array containing all the elements of a specific NodeDataIdx type. - """ - if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]: - return np.array([]) - - return self._nodes_data[:, node_data_idx.value] - - def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) - dist = np.cumsum(dist, axis=0) - sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:] - sl_next = np.concatenate((sl[1:], [0.])) - - # Create a boolean mask where speed limit changes and filter values - sl_change = sl != sl_next - distances = dist[sl_change] - speed_limits = sl[sl_change] - - # Create speed limits sections combining all continuous nodes that have same speed limit value. - start = 0. - limits_ahead = [] - for idx, end in enumerate(distances): - limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) - start = end - - return limits_ahead - - - def advisory_speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) - dist = np.cumsum(dist, axis=0) - sl = self.get(NodeDataIdx.advisory_speed_limit)[ahead_idx - 1:] - sl_next = np.concatenate((sl[1:], [0.])) - - # Create a boolean mask where speed limit changes and filter values - sl_change = sl != sl_next - distances = dist[sl_change] - speed_limits = sl[sl_change] - - # Create speed limits sections combining all continuous nodes that have same speed limit value. - start = 0. - limits_ahead = [] - for idx, end in enumerate(distances): - if speed_limits[idx] != None and speed_limits[idx] > 0: - limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) - start = end - - return limits_ahead - - - def distance_to_end(self, ahead_idx, distance_to_node_ahead): - if len(self._nodes_data) == 0 or ahead_idx is None: - return None - - return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))) - - def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location for - speed limit sections due to curvatures in the road. - """ - if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None: - return [] - - # Find the current distance traveled so far on the route. - dist_curr = self.get(NodeDataIdx.dist_route)[ahead_idx] - distance_to_node_ahead - - # Filter the sections to get only those where the stop distance is ahead of current. - sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr - data = self._curvature_speed_sections_data[sec_filter] - - # Offset distances to current distance. - data[:, [0, 1]] -= dist_curr - - # Create speed limits sections - limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data] - - advisory_speed_limits_ahead = self.advisory_speed_limits_ahead(ahead_idx, distance_to_node_ahead) - for advisory_limit in advisory_speed_limits_ahead: - for limit in limits_ahead: - if limit.start >= advisory_limit.start and limit.end <= advisory_limit.end: - limit.value = advisory_limit.value - - - return limits_ahead - - def possible_divertions(self, ahead_idx, distance_to_node_ahead): - """ Returns and array with the way relations the route could possible divert to by finding - the alternative way diversions on the nodes in the vicinity of the current location. - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - dist_route = self.get(NodeDataIdx.dist_route) - rel_dist = dist_route - dist_route[ahead_idx] + distance_to_node_ahead - valid_idxs = np.nonzero(np.logical_and(rel_dist >= _DIVERTION_SEARCH_RANGE[0], - rel_dist <= _DIVERTION_SEARCH_RANGE[1]))[0] - valid_divertions = [self._divertions[i] for i in valid_idxs] - - return [wr for wrs in valid_divertions for wr in wrs] # flatten. - - def distance_to_node(self, node_id, ahead_idx, distance_to_node_ahead): - """ - Provides the distance to a specific node in the route identified by `node_id` in reference to the node ahead - (`ahead_idx`) and the distance from current location to the node ahead (`distance_to_node_ahead`). - """ - node_ids = self.get(NodeDataIdx.node_id) - node_idxs = np.nonzero(node_ids == node_id)[0] - if len(self._nodes_data) == 0 or ahead_idx is None or len(node_idxs) == 0: - return None - - return self.get(NodeDataIdx.dist_route)[node_idxs[0]] - self.get(NodeDataIdx.dist_route)[ahead_idx] + \ - distance_to_node_ahead diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py deleted file mode 100644 index 6689e36ff7..0000000000 --- a/selfdrive/mapd/lib/Route.py +++ /dev/null @@ -1,340 +0,0 @@ -from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx -from selfdrive.mapd.config import QUERY_RADIUS -from selfdrive.mapd.lib.geo import ref_vectors, R, distance_to_points -from itertools import compress -import numpy as np - - -_ACCEPTABLE_BEARING_DELTA_COSINE = -0.7 # Continuation paths with a bearing of 180 +/- 45 degrees. -_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees. -_MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius. - - -class Route(): - """A set of consecutive way relations forming a default driving route. - """ - def __init__(self, current, wr_index, way_collection_id, query_center): - """Create a Route object from a given `wr_index` (Way relation index) - - Args: - current (WayRelation): The Way Relation that is currently located. It must be active. - wr_index (WayRelationIndex): The indexes of WayRelations by node id. - way_collection_id (UUID): The id of the Way Collection that created this Route. - query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query. - """ - self.way_collection_id = way_collection_id - self._ordered_way_relations = [] - self._nodes_data = None - self._reset() - - # An active current way is needed to be able to build a route - if not current.active: - return - - # Build the route by finding iteratavely the best matching ways continuing after the end of the - # current (last_wr) way. Use the index to find the continuation possibilities on each iteration. - last_wr = current - ordered_way_ids = [] - split_wrs = [] - while True: - # - Append current element to the route list of ordered way relations. - self._ordered_way_relations.append(last_wr) - ordered_way_ids.append(last_wr.id) - - # - Get the id of the node at the end of the way and then fetch the way relations that share the end node id. - last_node_id = last_wr.last_node.id - way_relations = wr_index.way_relations_with_edge_node_id(last_node_id) - - # - Add split way relations when necessary and remove parent way relations. - split_wrs_to_add = [wr for wr in split_wrs if last_node_id in wr.edge_nodes_ids] - way_relations.extend(split_wrs_to_add) - parent_ids = [wr.parent_wr_id for wr in split_wrs_to_add] - way_relations = [wr for wr in way_relations if wr.id not in parent_ids] - - # - If no more way_relations than last_wr, we have to check if we join another wr on an internal node, and - # if we do, we replace such way relation with the split of it and continue. - if len(way_relations) == 1: - way_relations = wr_index.way_relations_with_node_id(last_node_id) - # If no more way_relations than last_wr or its parent, we got to the end. - if len(way_relations) == 1: - break - - # If last_wr is a split, replace its parent with last_wr - way_relations = [last_wr if wr is last_wr.parent else wr for wr in way_relations] - - # If we join a wr on an internal node, then we artificially split the wr in two and pass both wrs as - # candidates to the wr selection code below. - wr_to_split = [wr for wr in way_relations if wr is not last_wr][0] - next_split_way_id = -len(split_wrs) - 1 # Keep split wrs ids unique on Route - new_wrs = wr_to_split.split(last_node_id, [next_split_way_id, next_split_way_id - 1]) - # If it could not be splited, we are done. - if len(new_wrs) != 2: - break - - # Replace the original way relation for the split version on way_relations and track splited wrs. - split_wrs.extend(new_wrs) - way_relations.remove(wr_to_split) - way_relations.extend(new_wrs) - - # - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node - # on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way. - ref_point = last_wr.last_node_coordinates - points = np.array([wr.node_before_edge_coordinates(last_node_id) for wr in way_relations]) - v = ref_vectors(ref_point, points) * R - - # - Calculate the bearing (from true north clockwise) for every end section of each way. - b = np.arctan2(v[:, 0], v[:, 1]) - - # - Find index of las_wr section and calculate deltas of bearings to the other sections. - last_wr_idx = way_relations.index(last_wr) - b_ref = b[last_wr_idx] - delta = b - b_ref - - # - Update the direction of the possible route continuation ways as starting from last_node_id. - # Make sure to exclude any ways already included in the ordered list as to not modify direction when there - # are looping roads (like roundabouts). A way will never be included twice in a route anyway. - for wr in way_relations: - if wr.id not in ordered_way_ids: - wr.update_direction_from_starting_node(last_node_id) - - # - Filter the possible route continuation way relations: - # - exclude any way already added to the ordered list. - # - exclude all way relations that are prohibited due to traffic direction. - mask = [wr.id not in ordered_way_ids and not wr.is_prohibited for wr in way_relations] - way_relations = list(compress(way_relations, mask)) - delta = delta[mask] - - # if no options left, we got to the end. - if len(way_relations) == 0: - break - - # - The cosine of the bearing delta will aid us in choosing the way that continues. The cosine is - # minimum (-1) for a perfect straight continuation as delta would be pi or -pi. - cos_delta = np.cos(delta) - - def pick_best_idx(cos_delta): - """Selects the best index on `cos_delta` array for a way that continues the route. - In principle we want to choose the way that continues as straight as possible. - Bue we need to make sure that if there are 2 or more ways continuing relatively straight, then we - need to disambiguate, either by matching the `ref` or `name` value of the continuing way with the - last way selected. - This can prevent cases where the chosen route could be for instance an exit ramp of a way due to the fact - that the ramp has a better match on bearing to previous way. We choose to stay on the road with the same `ref` - or `name` value if available. - If there is no ambiguity or there are no `name` or `ref` values to disambiguate, then we pick the one with - the straightest following direction. - """ - # Find the indexes of the cosine of the deltas that are considered straight enough to continue. - idxs = np.nonzero(cos_delta < _ACCEPTABLE_BEARING_DELTA_COSINE)[0] - - # If no amiguity or no way to break it, just return the straightest line. - if len(idxs) <= 1 or (last_wr.ref is None and last_wr.name is None): - # The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent - # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi - # and -pi. - return np.argmin(cos_delta) - - wrs = [way_relations[idx] for idx in idxs] - - # If we find a continuation way with the same reference we just choose it. - refs = list(map(lambda wr: wr.ref, wrs)) - if last_wr.ref is not None: - idx = next((idx for idx, ref in enumerate(refs) if ref == last_wr.ref), None) - if idx is not None: - return idxs[idx] - - # If we find a continuation way with the same name we just choose it. - names = list(map(lambda wr: wr.name, wrs)) - if last_wr.name is not None: - idx = next((idx for idx, name in enumerate(names) if name == last_wr.name), None) - if idx is not None: - return idxs[idx] - - # We did not manage to disambiguate, choose straightest path. - return np.argmin(cos_delta) - - # Get the index of the continuation way. - best_idx = pick_best_idx(cos_delta) - - # - Make sure to not select as route continuation a way that turns too much if we are close to the border of - # map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the - # data for the way that actually continues straight. - if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE: - dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0] - if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE: - break - - # - Select next way. - last_wr = way_relations[best_idx] - - # Build the node data from the ordered list of way relations - self._nodes_data = NodesData(self._ordered_way_relations, wr_index) - - # Locate where we are in the route node list. - self._locate() - - def __repr__(self): - count = self._nodes_data.count if self._nodes_data is not None else None - return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}' - - def _reset(self): - self._limits_ahead = None - self._cuvature_limits_ahead = None - self._curvatures_ahead = None - self._ahead_idx = None - self._distance_to_node_ahead = None - - @property - def located(self): - return self._ahead_idx is not None - - def _locate(self): - """Will resolve the index in the nodes_data list for the node ahead of the current location. - It updates as well the distance from the current location to the node ahead. - """ - current = self.current_wr - if current is None: - return - - node_ahead_id = current.node_ahead.id - self._distance_to_node_ahead = current.distance_to_node_ahead - start_idx = self._ahead_idx if self._ahead_idx is not None else 1 - self._ahead_idx = None - - ids = self._nodes_data.get(NodeDataIdx.node_id) - for idx in range(start_idx, len(ids)): - if ids[idx] == node_ahead_id: - self._ahead_idx = idx - break - - @property - def current_wr(self): - return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None - - def update(self, location_rad, bearing_rad, location_stdev): - """Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the - route on the original direction. If direction has changed or active point on the route can not be found, the route - will become invalid. - """ - if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None: - return - - # Skip if no update on location or bearing. - if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad: - return - - # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route - # with the way relations remaining ahead. - for idx, wr in enumerate(self._ordered_way_relations): - active_direction = wr.direction - wr.update(location_rad, bearing_rad, location_stdev) - - if not wr.active: - continue - - if wr.direction != active_direction: - # Driving direction on the route has changed. stop. - break - - # We have now the current wr. Repopulate from here till the end and locate - self._ordered_way_relations = self._ordered_way_relations[idx:] - self._reset() - self._locate() - - # If the active way is diverting, check whether there are possibilities to divert from the route in the - # vecinity of the current location. If there are possibilities, then stop here to loose the route as we are - # most likely driving away. If there are no possibilities, then stick to the route as the diversion is probably - # just a matter of GPS accuracy. (It can happen after driving under a bridge) - if wr.diverting and len(self._nodes_data.possible_divertions(self._ahead_idx, self._distance_to_node_ahead)) > 0: - break - - # The current location in route is valid, return. - return - - # if we got here, there is no new active way relation or driving direction has changed. Reset. - self._reset() - - @property - def speed_limits_ahead(self): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if self._limits_ahead is not None: - return self._limits_ahead - - if self._nodes_data is None or self._ahead_idx is None: - return [] - - self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead) - return self._limits_ahead - - @property - def curvature_speed_limits_ahead(self): - """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location due - to curvatures - """ - if self._cuvature_limits_ahead is not None: - return self._cuvature_limits_ahead - - if self._nodes_data is None or self._ahead_idx is None: - return [] - - self._cuvature_limits_ahead = self._nodes_data. \ - curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead) - - return self._cuvature_limits_ahead - - @property - def current_speed_limit(self): - if not self.located: - return None - - limits_ahead = self.speed_limits_ahead - if len(limits_ahead) == 0 or limits_ahead[0].start != 0: - return None - - return limits_ahead[0].value - - @property - def current_curvature_speed_limit_section(self): - if not self.located: - return None - - limits_ahead = self.curvature_speed_limits_ahead - if len(limits_ahead) == 0 or limits_ahead[0].start != 0: - return None - - return limits_ahead[0] - - @property - def next_speed_limit_section(self): - if not self.located: - return None - - limits_ahead = self.speed_limits_ahead - if len(limits_ahead) == 0: - return None - - # Find the first section that does not start in 0. i.e. the next section - for section in limits_ahead: - if section.start > 0: - return section - - return None - - def next_curvature_speed_limit_sections(self, horizon_mts): - if not self.located: - return [] - - # Provide the curvature speed sections that start ahead (> 0) and up to horizon - return list(filter(lambda la: la.start > 0 and la.start <= horizon_mts, self.curvature_speed_limits_ahead)) - - @property - def distance_to_end(self): - if not self.located: - return None - - return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead) - - @property - def current_road_name(self): - return self.current_wr.road_name if self.located else None diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py deleted file mode 100644 index fda2ae7fb3..0000000000 --- a/selfdrive/mapd/lib/WayCollection.py +++ /dev/null @@ -1,85 +0,0 @@ -from selfdrive.mapd.lib.WayRelation import WayRelation -from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex -from selfdrive.mapd.lib.Route import Route -from selfdrive.mapd.config import LANE_WIDTH -import uuid - - -_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acceptable bearing delta - - -class WayCollection(): - """A collection of WayRelations to use for maps data analysis. - """ - def __init__(self, ways, query_center): - """Creates a WayCollection with a set of OSM way objects. - - Args: - ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center` - query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query. - """ - self.id = uuid.uuid4() - self.way_relations = [WayRelation(way) for way in ways] - self.query_center = query_center - - self.wr_index = WayRelationIndex(self.way_relations) - - def get_route(self, location_rad, bearing_rad, location_stdev): - """Provides the best route found in the way collection based on current location and bearing. - """ - if location_rad is None or bearing_rad is None or location_stdev is None: - return None - - # Update all way relations in collection to the provided location and bearing. - for wr in self.way_relations: - wr.update(location_rad, bearing_rad, location_stdev) - - # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of - # travel is valid. - valid_way_relations = [wr for wr in self.way_relations if wr.active and not wr.is_prohibited] - - # If no active, then we could not find a current way to build a route. - if len(valid_way_relations) == 0: - return None - - # If only one valid, then pick it as current. - if len(valid_way_relations) == 1: - current = valid_way_relations[0] - - # If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high. - else: - wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, - valid_way_relations)) - - # If delta bearing indicator is too high for all, then use as current the one that has the shorter one. - if len(wr_acceptable_bearing) == 0: - valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta) - current = valid_way_relations[0] - - # If only one with acceptable bearing, use it. - elif len(wr_acceptable_bearing) == 1: - current = wr_acceptable_bearing[0] - - else: - # If more than one with acceptable bearing, filter the ones with distance to way lower than 2 standard - # deviation from GPS accuracy (95%) + half the road width estimate. - wr_accurate_distance = [wr for wr in wr_acceptable_bearing - if wr.distance_to_way <= 2. * location_stdev + wr.lanes * LANE_WIDTH / 2.] - - # If none with accurate distance to way, then select the closest to the way - if len(wr_accurate_distance) == 0: - wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way) - current = wr_acceptable_bearing[0] - - # If only one with distance under accuracy, select this one. - elif len(wr_accurate_distance) == 1: - current = wr_accurate_distance[0] - - # If more than one with distance under accuracy. Then select the one with lowest highway rank. - # i.e. preferred motorways over other roads and so on. This is to prevent selecting a small parallel - # road to a main road when the accuracy is poor. - else: - wr_accurate_distance.sort(key=lambda wr: wr.highway_rank) - current = wr_accurate_distance[0] - - return Route(current, self.wr_index, self.id, self.query_center) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py deleted file mode 100644 index 0ba49b1013..0000000000 --- a/selfdrive/mapd/lib/WayRelation.py +++ /dev/null @@ -1,435 +0,0 @@ -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points, point_on_line -from selfdrive.mapd.lib.osm import create_way -from common.conversions import Conversions as CV -from selfdrive.mapd.config import LANE_WIDTH -from common.basedir import BASEDIR -from datetime import datetime as dt -import numpy as np -import re -import json - - -_WAY_BBOX_PADING = 80. / R # 80 mts of padding to bounding box. (expressed in radians) - - -with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds.json", "rb") as f: - _COUNTRY_LIMITS = json.loads(f.read()) - - -_WD = { - 'Mo': 0, - 'Tu': 1, - 'We': 2, - 'Th': 3, - 'Fr': 4, - 'Sa': 5, - 'Su': 6 -} - -_HIGHWAY_RANK = { - 'motorway': 0, - 'motorway_link': 1, - 'trunk': 10, - 'trunk_link': 11, - 'primary': 20, - 'primary_link': 21, - 'secondary': 30, - 'secondary_link': 31, - 'tertiary': 40, - 'tertiary_link': 41, - 'unclassified': 50, - 'residential': 60, - 'living_street': 61 -} - - -def is_osm_time_condition_active(condition_string): - """ - Will indicate if a time condition for a restriction as described - @ https://wiki.openstreetmap.org/wiki/Conditional_restrictions - is active for the current date and time of day. - """ - now = dt.now().astimezone() - today = now.date() - week_days = [] - - # Look for days of week matched and validate if today matches criteria. - dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string) - - if len(dr) == 1: - week_days = [_WD[dr[0]]] - # If two or more matches condider it a range of days between 1st and 2nd element. - elif len(dr) > 1: - week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1)) - - # If valid week days list is not empty and today day is not in the list, then the time-date range is not active. - if len(week_days) > 0 and now.weekday() not in week_days: - return False - - # Look for time ranges on the day. No time range, means all day - tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string) - - # if no time range but there were week days set, consider it active during the whole day - if len(tr) == 0: - return len(dr) > 0 - - # Search among time ranges matched, one where now time belongs too. If found range is active. - for times_tup in tr: - times = list(map(lambda tt: dt. - combine(today, dt.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup)) - if now >= times[0] and now <= times[1]: - return True - - return False - - -def speed_limit_value_for_limit_string(limit_string): - # Look for matches of speed by default in kph, or in mph when explicitly noted. - v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string) - if v is None: - return None - conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS - return conv * float(v[1]) - - -def speed_limit_for_osm_tag_limit_string(limit_string): - # https://wiki.openstreetmap.org/wiki/Key:maxspeed - if limit_string is None: - # When limit is set to 0. is considered not existing. - return 0. - - # Attempt to parse limit as simple numeric value considering units. - limit = speed_limit_value_for_limit_string(limit_string) - if limit is not None: - return limit - - # Look for matches of speed with country implicit values. - v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string) - if v is None: - return 0. - - if v[2] == "zone" and v[3] is not None: - conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS - limit = conv * float(v[3]) - elif f'{v[1]}:{v[2]}' in _COUNTRY_LIMITS: - limit = speed_limit_value_for_limit_string(_COUNTRY_LIMITS[f'{v[1]}:{v[2]}']) - - return limit if limit is not None else 0. - - -def conditional_speed_limit_for_osm_tag_limit_string(limit_string): - if limit_string is None: - # When limit is set to 0. is considered not existing. - return 0. - - # Look for matches of the ` @ ()` format - v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string) - if v is None: - return 0. # No valid format match - - value = speed_limit_for_osm_tag_limit_string(v[1]) - if value == 0.: - return 0. # Invalid speed limit value - - # Look for date-time conditions separated by semicolon - v = re.findall(r'(?:;|^)([^;]*)', v[2]) - for datetime_condition in v: - if is_osm_time_condition_active(datetime_condition): - return value - - # If we get here, no current date-time condition is active. - return 0. - - -class WayRelation(): - """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle. - """ - def __init__(self, way, parent=None): - self.way = way - self.parent = parent - self.parent_wr_id = parent.id if parent is not None else None # For WRs created as splits of other WRs - self.reset_location_variables() - self.direction = DIRECTION.NONE - self._speed_limit = None - self._advisory_speed_limit = None - self._one_way = way.tags.get("oneway") - self.name = way.tags.get('name') - self.ref = way.tags.get('ref') - self.highway_type = way.tags.get("highway") - self.highway_rank = _HIGHWAY_RANK.get(self.highway_type, 1000) - try: - self.lanes = int(way.tags.get('lanes')) - except Exception: - self.lanes = 2 - - # Create numpy arrays with nodes data to support calculations. - self._nodes_np = np.radians(np.array([[node.lat, node.lon] for node in way.nodes], dtype=float)) - self._nodes_ids = np.array([node.id for node in way .nodes], dtype=int) - - # Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2) - v = vectors(self._nodes_np) * R - - # Calculate the vector magnitudes (or distance) between nodes. (N-1) - self._way_distances = np.linalg.norm(v, axis=1) - - # Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1) - self._way_bearings = np.arctan2(v[:, 0], v[:, 1]) - - # Define bounding box to ease the process of locating a node in a way. - # [[min_lat, min_lon], [max_lat, max_lon]] - self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING, - np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING)) - - # Get the edge nodes ids. - self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id] - - def __repr__(self): - return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})' - - def __eq__(self, other): - if isinstance(other, WayRelation): - return self.id == other.id - return False - - def reset_location_variables(self): - self.distance_to_node_ahead = 0. - self.location_rad = None - self.bearing_rad = None - self.active = False - self.diverting = False - self.ahead_idx = None - self.behind_idx = None - self._active_bearing_delta = None - self._distance_to_way = None - - @property - def id(self): - return self.way.id - - @property - def road_name(self): - if self.name is not None: - return self.name - return self.ref - - def update(self, location_rad, bearing_rad, location_stdev): - """Will update and validate the associated way with a given `location_rad` and `bearing_rad`. - Specifically it will find the nodes behind and ahead of the current location and bearing. - If no proper fit to the way geometry, the way relation is marked as invalid. - """ - self.reset_location_variables() - - # Ignore if location not in way bounding box - if not self.is_location_in_bbox(location_rad): - return - - # - Get the distance and bearings from location to all nodes. (N) - bearings = bearing_to_points(location_rad, self._nodes_np) - - # - Get absolute bearing delta to current driving bearing. (N) - delta = np.abs(bearing_rad - bearings) - - # - Nodes are ahead if the cosine of the delta is positive (N) - is_ahead = np.cos(delta) >= 0. - - # - Possible locations on the way are those where adjacent nodes change from ahead to behind or vice-versa. - possible_idxs = np.nonzero(np.diff(is_ahead))[0] - - # - when no possible locations found, then the location is not in this way. - if len(possible_idxs) == 0: - return - - projections = point_on_line(self._nodes_np[:-1], self._nodes_np[1:], location_rad) - h = distance_to_points(location_rad, projections) - - # - Calculate the delta between driving bearing and way bearings. (N-1) - bw_delta = self._way_bearings - bearing_rad - - # - The absolute value of the sin of `bw_delta` indicates how close the bearings match independent of direction. - # We will use this value along the distance to the way to aid on way selection. (N-1) - abs_sin_bw_delta = np.abs(np.sin(bw_delta)) - - # - Get the delta to way bearing indicators and the distance to the way for the possible locations. - abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs] - h_possible = h[possible_idxs] - - # - Get the index where the distance to the way is minimum. That is the chosen location. - min_h_possible_idx = np.argmin(h_possible) - min_delta_idx = possible_idxs[min_h_possible_idx] - projection = projections[min_delta_idx] - - # - If the distance to the way is over 4 standard deviations of the gps accuracy + the maximum road width - # estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore) - # In theory the osm path is centered on the road which means half the road width would cover the whole road. - # however, often times the osm path is not perfectly centered so we'll make the possible route more lenient by using - # the full road width. - road_width_estimate = self.lanes * LANE_WIDTH - half_road_width_estimate = road_width_estimate / 2. - if h_possible[min_h_possible_idx] > 4. * location_stdev + road_width_estimate: - return - - # If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road - # width estimate + 1 lane width then we are most likely diverting from this route. Adding a lane width to give - # leniency to not perfectly centered osm paths - diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate + LANE_WIDTH - - # Populate location variables with result - if is_ahead[min_delta_idx]: - self.direction = DIRECTION.BACKWARD - self.ahead_idx = min_delta_idx - self.behind_idx = min_delta_idx + 1 - else: - self.direction = DIRECTION.FORWARD - self.ahead_idx = min_delta_idx + 1 - self.behind_idx = min_delta_idx - - self._distance_to_way = h[min_delta_idx] - self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx] - - # find the distance to the next node by projecting our location onto the line and finding the delta between that - # point and the next point on the route - self.distance_to_node_ahead = distance_to_points(projection, np.array([self._nodes_np[self.ahead_idx]]))[0] - self.active = True - self.diverting = diverting - self.location_rad = location_rad - self.bearing_rad = bearing_rad - self._speed_limit = None - self._advisory_speed_limit = None - - def update_direction_from_starting_node(self, start_node_id): - self._speed_limit = None - self._advisory_speed_limit = None - if self.edge_nodes_ids[0] == start_node_id: - self.direction = DIRECTION.FORWARD - elif self.edge_nodes_ids[-1] == start_node_id: - self.direction = DIRECTION.BACKWARD - else: - self.direction = DIRECTION.NONE - - def is_location_in_bbox(self, location_rad): - """Indicates if a given location is contained in the bounding box surrounding the way. - self.bbox = [[min_lat, min_lon], [max_lat, max_lon]] - """ - is_g = np.greater_equal(location_rad, self.bbox[0, :]) - is_l = np.less_equal(location_rad, self.bbox[1, :]) - - return np.all(np.concatenate((is_g, is_l))) - - @property - def speed_limit(self): - if self._speed_limit is not None: - return self._speed_limit - - # Get string from corresponding tag, consider conditional limits first. - limit_string = self.way.tags.get("maxspeed:conditional") - if limit_string is None: - if self.direction == DIRECTION.FORWARD: - limit_string = self.way.tags.get("maxspeed:forward:conditional") - elif self.direction == DIRECTION.BACKWARD: - limit_string = self.way.tags.get("maxspeed:backward:conditional") - - limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string) - - # When no conditional limit set, attempt to get from regular speed limit tags. - if limit == 0.: - limit_string = self.way.tags.get("maxspeed") - if limit_string is None: - if self.direction == DIRECTION.FORWARD: - limit_string = self.way.tags.get("maxspeed:forward") - elif self.direction == DIRECTION.BACKWARD: - limit_string = self.way.tags.get("maxspeed:backward") - - limit = speed_limit_for_osm_tag_limit_string(limit_string) - - self._speed_limit = limit - return self._speed_limit - - - @property - def advisory_speed_limit(self): - if self._advisory_speed_limit is not None: - return self._advisory_speed_limit - - limit_string = self.way.tags.get("maxspeed:advisory") - limit = speed_limit_for_osm_tag_limit_string(limit_string) - - self._advisory_speed_limit = limit - return self._advisory_speed_limit - - - @property - def active_bearing_delta(self): - """Returns the sine of the delta between the current location bearing and the exact - bearing of the portion of way we are currentluy located at. - """ - return self._active_bearing_delta - - @property - def is_one_way(self): - return self._one_way in ['yes'] or self.highway_type in ["motorway"] - - @property - def is_prohibited(self): - # Direction must be defined to asses this property. Default to `True` if not. - if self.direction == DIRECTION.NONE: - return True - return self.is_one_way and self.direction == DIRECTION.BACKWARD - - @property - def distance_to_way(self): - """Returns the perpendicular (i.e. minimum) distance between current location and the way - """ - return self._distance_to_way - - @property - def node_ahead(self): - return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None - - @property - def last_node(self): - """Returns the last node on the way considering the traveling direction - """ - if self.direction == DIRECTION.FORWARD: - return self.way.nodes[-1] - if self.direction == DIRECTION.BACKWARD: - return self.way.nodes[0] - return None - - @property - def last_node_coordinates(self): - """Returns the coordinates for the last node on the way considering the traveling direction. (in radians) - """ - if self.direction == DIRECTION.FORWARD: - return self._nodes_np[-1] - if self.direction == DIRECTION.BACKWARD: - return self._nodes_np[0] - return None - - def node_before_edge_coordinates(self, node_id): - """Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians) - """ - if self.edge_nodes_ids[0] == node_id: - return self._nodes_np[1] - - if self.edge_nodes_ids[-1] == node_id: - return self._nodes_np[-2] - - return np.array([0., 0.]) - - def split(self, node_id, way_ids=None): - """ Returns and array with the way relations resulting from splitting the current way relation at node_id - """ - idxs = np.nonzero(self._nodes_ids == node_id)[0] - if len(idxs) == 0: - return [] - - idx = idxs[0] - if idx == 0 or idx == len(self._nodes_ids) - 1: - return [self] - - if not isinstance(way_ids, list): - way_ids = [-1, -2] # Default id values. - - ways = [create_way(way_ids[0], node_ids=self._nodes_ids[:idx + 1], from_way=self.way), - create_way(way_ids[1], node_ids=self._nodes_ids[idx:], from_way=self.way)] - return [WayRelation(way, parent=self) for way in ways] diff --git a/selfdrive/mapd/lib/WayRelationIndex.py b/selfdrive/mapd/lib/WayRelationIndex.py deleted file mode 100644 index e941dcd5b7..0000000000 --- a/selfdrive/mapd/lib/WayRelationIndex.py +++ /dev/null @@ -1,34 +0,0 @@ - - -class WayRelationIndex(): - """ - A class containing an index of WayRelations by node ids of internal nodes and edge nodes. - """ - def __init__(self, way_relations): - self._edge_nodes_index_dict = {} - self._full_nodes_index_dict = {} - - for wr in way_relations: - self.add(wr) - - def add(self, way_relation): - for node in way_relation.way.nodes: - node_id = node.id - self._full_nodes_index_dict[node_id] = self._full_nodes_index_dict.get(node_id, []) + [way_relation] - if node_id in way_relation.edge_nodes_ids: - self._edge_nodes_index_dict[node_id] = self._edge_nodes_index_dict.get(node_id, []) + [way_relation] - - def remove(self, way_relation): - for node in way_relation.way.nodes: - node_id = node.id - self._full_nodes_index_dict[node_id] = [wr for wr in self._full_nodes_index_dict.get(node_id, []) - if wr is not way_relation] - if node_id in way_relation.edge_nodes_ids: - self._edge_nodes_index_dict[node_id] = [wr for wr in self._edge_nodes_index_dict.get(node_id, []) - if wr is not way_relation] - - def way_relations_with_edge_node_id(self, node_id): - return self._edge_nodes_index_dict.get(node_id, []) - - def way_relations_with_node_id(self, node_id): - return self._full_nodes_index_dict.get(node_id, []) diff --git a/selfdrive/mapd/lib/default_speeds.json b/selfdrive/mapd/lib/default_speeds.json deleted file mode 100644 index a8db608807..0000000000 --- a/selfdrive/mapd/lib/default_speeds.json +++ /dev/null @@ -1,111 +0,0 @@ -{ - "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", - "AR:urban": "40", - "AR:urban:primary": "60", - "AR:urban:secondary": "60", - "AR:rural": "110", - "AT:urban": "50", - "AT:rural": "100", - "AT:trunk": "100", - "AT:motorway": "130", - "BE:urban": "50", - "BE-VLG:rural": "70", - "BE-WAL:rural": "90", - "BE:trunk": "120", - "BE:motorway": "120", - "CH:urban[1]": "50", - "CH:rural": "80", - "CH:trunk": "100", - "CH:motorway": "120", - "CZ:pedestrian_zone": "20", - "CZ:living_street": "20", - "CZ:urban": "50", - "CZ:urban_trunk": "80", - "CZ:urban_motorway": "80", - "CZ:rural": "90", - "CZ:trunk": "110", - "CZ:motorway": "130", - "DK:urban": "50", - "DK:rural": "80", - "DK:motorway": "130", - "DE:living_street": "7", - "DE:residential": "30", - "DE:urban": "50", - "DE:rural": "100", - "DE:trunk": "none", - "DE:motorway": "none", - "FI:urban": "50", - "FI:rural": "80", - "FI:trunk": "100", - "FI:motorway": "120", - "FR:urban": "50", - "FR:rural": "80", - "FR:trunk": "110", - "FR:motorway": "130", - "GR:urban": "50", - "GR:rural": "90", - "GR:trunk": "110", - "GR:motorway": "130", - "HU:urban": "50", - "HU:rural": "90", - "HU:trunk": "110", - "HU:motorway": "130", - "IT:urban": "50", - "IT:rural": "90", - "IT:trunk": "110", - "IT:motorway": "130", - "JP:national": "60", - "JP:motorway": "100", - "LT:living_street": "20", - "LT:urban": "50", - "LT:rural": "90", - "LT:trunk": "120", - "LT:motorway": "130", - "PL:living_street": "20", - "PL:urban": "50", - "PL:rural": "90", - "PL:trunk": "100", - "PL:motorway": "140", - "RO:urban": "50", - "RO:rural": "90", - "RO:trunk": "100", - "RO:motorway": "130", - "RU:living_street": "20", - "RU:urban": "60", - "RU:rural": "90", - "RU:motorway": "110", - "SK:urban": "50", - "SK:rural": "90", - "SK:trunk": "90", - "SK:motorway": "90", - "SI:urban": "50", - "SI:rural": "90", - "SI:trunk": "110", - "SI:motorway": "130", - "ES:living_street": "20", - "ES:urban": "50", - "ES:rural": "50", - "ES:trunk": "90", - "ES:motorway": "120", - "SE:urban": "50", - "SE:rural": "70", - "SE:trunk": "90", - "SE:motorway": "110", - "GB:nsl_restricted": "30 mph", - "GB:nsl_single": "60 mph", - "GB:nsl_dual": "70 mph", - "GB:motorway": "70 mph", - "UA:urban": "50", - "UA:rural": "90", - "UA:trunk": "110", - "UA:motorway": "130", - "UZ:living_street": "30", - "UZ:urban": "70", - "UZ:rural": "100", - "UZ:motorway": "110", - "ZA:trunk": "120", - "ZA:residential": "60", - "ZA:rural": "100", - "ZA:urban": "60", - "ZA:motorway": "120" -} diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py deleted file mode 100644 index 51947481ae..0000000000 --- a/selfdrive/mapd/lib/geo.py +++ /dev/null @@ -1,78 +0,0 @@ -from enum import Enum -import numpy as np - - -R = 6373000.0 # approximate radius of earth in mts - - -def vectors(points): - """Provides a array of vectors on cartesian space (x, y). - Each vector represents the path from a point in `points` to the next. - `points` must by a (N, 2) array of [lat, lon] pairs in radians. - """ - latA = points[:-1, 0] - latB = points[1:, 0] - delta = np.diff(points, axis=0) - dlon = delta[:, 1] - - x = np.sin(dlon) * np.cos(latB) - y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) - - return np.column_stack((x, y)) - - -def ref_vectors(ref, points): - """Provides a array of vectors on cartesian space (x, y). - Each vector represents the path from ref to a point in `points`. - `points` must by a (N, 2) array of [lat, lon] pairs in radians. - """ - latA = ref[0] - latB = points[:, 0] - delta = points - ref - dlon = delta[:, 1] - - x = np.sin(dlon) * np.cos(latB) - y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) - - return np.column_stack((x, y)) - - -def bearing_to_points(point, points): - """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each - one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud, - longitude pair in radians. - """ - delta = points - point - x = np.sin(delta[:, 1]) * np.cos(points[:, 0]) - y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1])) - return np.arctan2(x, y) - -def point_on_line(start_points, end_points, point, extend_line = False): - """project a single point onto each line for an np array of start points and end points - ref: https://stackoverflow.com/a/61342198 - """ - ap = np.subtract(point, start_points) - ab = np.subtract(end_points, start_points) - t = np.array([np.dot(ap[i], ab[i]) / np.dot(ab[i], ab[i]) for i in range(len(ap))]) - # if you need the the closest point belonging to the segment - if not extend_line: - t = np.maximum(0, np.minimum(1, t)) - result = np.add(start_points, np.array([t[i] * ab[i] for i in range(len(t))])) - return result - -def distance_to_points(point, points): - """Calculate the distance of the vectors between `point` and each one of the entries in `points`. - Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians. - """ - delta = points - point - a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2 - c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a)) - return c * R - - -class DIRECTION(Enum): - NONE = 0 - AHEAD = 1 - BEHIND = 2 - FORWARD = 3 - BACKWARD = 4 diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py deleted file mode 100644 index 04c8163bc7..0000000000 --- a/selfdrive/mapd/lib/osm.py +++ /dev/null @@ -1,37 +0,0 @@ -import overpy -import numpy as np -from selfdrive.mapd.lib.geo import R - - -def create_way(way_id, node_ids, from_way): - """ - Creates and OSM Way with the given `way_id` and list of `node_ids`, copying attributes and tags from `from_way` - """ - return overpy.Way(way_id, node_ids=node_ids, attributes={}, result=from_way._result, - tags=from_way.tags) - - -class OSM(): - def __init__(self): - self.api = overpy.Overpass() - # self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter') - - def fetch_road_ways_around_location(self, lat, lon, radius): - # Calculate the bounding box coordinates for the bbox containing the circle around location. - bbox_angle = np.degrees(radius / R) - # fetch all ways and nodes on this ways in bbox - bbox_str = f'{str(lat - bbox_angle)},{str(lon - bbox_angle)},{str(lat + bbox_angle)},{str(lon + bbox_angle)}' - q = """ - way(""" + bbox_str + """) - [highway] - [highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"]; - (._;>;); - out; - """ - try: - ways = self.api.query(q).ways - except Exception as e: - print(f'Exception while querying OSM:\n{e}') - ways = [] - - return ways diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py deleted file mode 100644 index 8426376bf8..0000000000 --- a/selfdrive/mapd/mapd.py +++ /dev/null @@ -1,267 +0,0 @@ -#!/usr/bin/env python3 -import threading -from traceback import print_exception -import numpy as np -from time import strftime, gmtime -import cereal.messaging as messaging -from common.realtime import Ratekeeper -from selfdrive.mapd.lib.osm import OSM -from selfdrive.mapd.lib.geo import distance_to_points -from selfdrive.mapd.lib.WayCollection import WayCollection -from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME -from system.swaglog import cloudlog - - -_DEBUG = False -_CLOUDLOG_DEBUG = True - - -def _debug(msg, log_to_cloud=True): - if _CLOUDLOG_DEBUG and log_to_cloud: - cloudlog.debug(msg) - if _DEBUG: - print(msg) - - -def excepthook(args): - _debug(f'MapD: Threading exception:\n{args}') - print_exception(args.exc_type, args.exc_value, args.exc_traceback) - - -threading.excepthook = excepthook - - -class MapD(): - def __init__(self): - self.osm = OSM() - self.way_collection = None - self.route = None - self.last_gps_fix_timestamp = 0 - self.last_gps = None - self.location_deg = None # The current location in degrees. - self.location_rad = None # The current location in radians as a Numpy array. - self.bearing_rad = None - self.location_stdev = None # The current location accuracy in mts. 1 standard devitation. - self.gps_speed = 0. - self.last_fetch_location = None - self.last_route_update_fix_timestamp = 0 - self.last_publish_fix_timestamp = 0 - self._op_enabled = False - self._disengaging = False - self._query_thread = None - self._lock = threading.RLock() - - def udpate_state(self, sm): - sock = 'controlsState' - if not sm.updated[sock] or not sm.valid[sock]: - return - - controls_state = sm[sock] - self._disengaging = not controls_state.enabled and self._op_enabled - self._op_enabled = controls_state.enabled - - def update_gps(self, sm): - sock = 'gpsLocationExternal' - if not sm.updated[sock] or not sm.valid[sock]: - return - - log = sm[sock] - self.last_gps = log - - # ignore the message if the fix is invalid - if log.flags % 2 == 0: - return - - self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970. - self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float)) - self.location_deg = (log.latitude, log.longitude) - self.bearing_rad = np.radians(log.bearingDeg, dtype=float) - self.gps_speed = log.speed - self.location_stdev = log.accuracy # log accuracies are presumably 1 standard deviation. - - _debug('Mapd: ********* Got GPS fix' - + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n' - + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n' - + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' - + '*******', log_to_cloud=False) - - def _query_osm_not_blocking(self): - def query(osm, location_deg, location_rad, radius): - _debug(f'Mapd: Start query for OSM map data at {location_deg}') - lat, lon = location_deg - ways = osm.fetch_road_ways_around_location(lat, lon, radius) - _debug(f'Mapd: Query to OSM finished with {len(ways)} ways') - - # Only issue an update if we received some ways. Otherwise it is most likely a connectivity issue. - # Will retry on next loop. - if len(ways) > 0: - new_way_collection = WayCollection(ways, location_rad) - - # Use the lock to update the way_collection as it might be being used to update the route. - _debug('Mapd: Locking to write results from osm.', log_to_cloud=False) - with self._lock: - self.way_collection = new_way_collection - self.last_fetch_location = location_rad - _debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways') - - _debug('Mapd: Releasing Lock to write results from osm', log_to_cloud=False) - - # Ignore if we have a query thread already running. - if self._query_thread is not None and self._query_thread.is_alive(): - return - - self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad, - QUERY_RADIUS)) - self._query_thread.start() - - def updated_osm_data(self): - if self.route is not None: - distance_to_end = self.route.distance_to_end - if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY: - # do not query as long as we have a route with enough distance ahead. - return - - if self.location_rad is None: - return - - if self.last_fetch_location is not None: - distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0] - if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY: - # do not query if are still not close to the border of previous query area - return - - self._query_osm_not_blocking() - - def update_route(self): - def update_proc(): - # Ensure we clear the route on op disengage, this way we can correct possible incorrect map data due - # to wrongly locating or picking up the wrong route. - if self._disengaging: - self.route = None - _debug('Mapd *****: Clearing Route as system is disengaging. ********') - - if self.way_collection is None or self.location_rad is None or self.bearing_rad is None: - _debug('Mapd *****: Can not update route. Missing WayCollection, location or bearing ********') - return - - if self.route is not None and self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: - _debug('Mapd *****: Skipping route update. No new fix since last update ********') - return - - self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp - - # Create the route if not existent or if it was generated by an older way collection - if self.route is None or self.route.way_collection_id != self.way_collection.id: - self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev) - _debug(f'Mapd *****: Route created: \n{self.route}\n********') - return - - # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start - # jumping and creating unnecessary losing of the route. Since the route update timestamp has been updated - # a new liveMapDataSP message will be published with the current values (which is desirable) - if self.gps_speed < FULL_STOP_MAX_SPEED: - _debug('Mapd *****: Route Not updated as car has Stopped ********') - return - - self.route.update(self.location_rad, self.bearing_rad, self.location_stdev) - if self.route.located: - _debug(f'Mapd *****: Route updated: \n{self.route}\n********') - return - - # if an old route did not mange to locate, attempt to regenerate form way collection. - self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev) - _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') - - # We use the lock when updating the route, as it reads `way_collection` which can ben updated by - # a new query result from the _query_thread. - _debug('Mapd: Locking to update route.', log_to_cloud=False) - with self._lock: - update_proc() - - _debug('Mapd: Releasing Lock to update route', log_to_cloud=False) - - def publish(self, pm, sm): - # Ensure we have a route currently located - if self.route is None or not self.route.located: - _debug('Mapd: Skipping liveMapDataSP message as there is no route or is not located.') - return - - # Ensure we have a route update since last publish - if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp: - _debug('Mapd: Skipping liveMapDataSP since there is no new gps fix.') - return - - self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp - - speed_limit = self.route.current_speed_limit - next_speed_limit_section = self.route.next_speed_limit_section - turn_speed_limit_section = self.route.current_curvature_speed_limit_section - horizon_mts = self.gps_speed * LOOK_AHEAD_HORIZON_TIME - next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts) - current_road_name = self.route.current_road_name - - map_data_msg = messaging.new_message('liveMapDataSP') - map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \ - sm.all_valid(service_list=['gpsLocationExternal']) - - liveMapDataSP = map_data_msg.liveMapDataSP - liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis - liveMapDataSP.lastGpsLatitude = float(self.last_gps.latitude) - liveMapDataSP.lastGpsLongitude = float(self.last_gps.longitude) - liveMapDataSP.lastGpsSpeed = float(self.last_gps.speed) - liveMapDataSP.lastGpsBearingDeg = float(self.last_gps.bearingDeg) - liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy) - liveMapDataSP.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg) - - liveMapDataSP.speedLimitValid = bool(speed_limit is not None) - liveMapDataSP.speedLimit = float(speed_limit if speed_limit is not None else 0.0) - liveMapDataSP.speedLimitAheadValid = bool(next_speed_limit_section is not None) - liveMapDataSP.speedLimitAhead = float(next_speed_limit_section.value - if next_speed_limit_section is not None else 0.0) - liveMapDataSP.speedLimitAheadDistance = float(next_speed_limit_section.start - if next_speed_limit_section is not None else 0.0) - - liveMapDataSP.turnSpeedLimitValid = bool(turn_speed_limit_section is not None) - liveMapDataSP.turnSpeedLimit = float(turn_speed_limit_section.value - if turn_speed_limit_section is not None else 0.0) - liveMapDataSP.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign - if turn_speed_limit_section is not None else 0) - liveMapDataSP.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end - if turn_speed_limit_section is not None else 0.0) - liveMapDataSP.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections] - liveMapDataSP.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections] - liveMapDataSP.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections] - - liveMapDataSP.currentRoadName = str(current_road_name if current_road_name is not None else "") - - pm.send('liveMapDataSP', map_data_msg) - _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False) - - -# provides live map data information -def mapd_thread(sm=None, pm=None): - mapd = MapD() - rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz - - # *** setup messaging - if sm is None: - sm = messaging.SubMaster(['gpsLocationExternal', 'controlsState']) - if pm is None: - pm = messaging.PubMaster(['liveMapDataSP']) - - while True: - sm.update() - mapd.udpate_state(sm) - mapd.update_gps(sm) - mapd.updated_osm_data() - mapd.update_route() - mapd.publish(pm, sm) - rk.keep_time() - - -def main(sm=None, pm=None): - mapd_thread(sm, pm) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/mapd/test/__init__.py b/selfdrive/mapd/test/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/mapd/test/mock_data.py b/selfdrive/mapd/test/mock_data.py deleted file mode 100644 index ea26da4e46..0000000000 --- a/selfdrive/mapd/test/mock_data.py +++ /dev/null @@ -1,266 +0,0 @@ -from selfdrive.mapd.lib.WayCollection import WayCollection -from selfdrive.mapd.lib.geo import vectors, R -from selfdrive.mapd.lib.NodesData import _MIN_NODE_DISTANCE, _ADDED_NODES_DIST, _SPLINE_EVAL_STEP, \ - _MIN_SPEED_SECTION_LENGTH, nodes_raw_data_array_for_wr, node_calculations, is_wr_a_valid_divertion_from_node, \ - spline_curvature_calculations, speed_limits_for_curvatures_data -from scipy.interpolate import splev, splprep -import numpy as np -import overpy - - -class MockNodesData(): - def __init__(self, way_coords): - self.degrees = np.array(way_coords) - self.radians = np.radians(self.degrees) - - # ***************** - # Expected code implementation nodes_data - self.v = vectors(self.radians) * R - self.d = np.linalg.norm(self.v, axis=1) - self.b = np.arctan2(self.v[:, 0], self.v[:, 1]) - self.v = np.concatenate(([[0., 0.]], self.v)) - self.dp = np.concatenate(([0.], self.d)) - self.dn = np.concatenate((self.d, [0.])) - self.dr = np.cumsum(self.dp, axis=0) - self.b = np.concatenate((self.b, [self.b[-1]])) - - # Expected code implementation spline_curvature_calculations - vect = self.v - dist_prev = self.dp - too_far_idxs = np.nonzero(self.dp >= _MIN_NODE_DISTANCE)[0] - for idx in too_far_idxs[::-1]: - dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors. - n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added. - new_v = vect[idx, :] / n # new relative vector to insert. - vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors. - vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors - ds = np.cumsum(dist_prev, axis=0) - vs = np.cumsum(vect, axis=0) - tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=W0632 - n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) - unew = np.arange(0, n + 1) / n - d1 = splev(unew, tck, der=1) - d2 = splev(unew, tck, der=2) - num = d1[0] * d2[1] - d1[1] * d2[0] - den = (d1[0]**2 + d1[1]**2)**(1.5) - self.curv = num / den - self.curv_ds = unew * ds[-1] - # ***************** - - -class MockCurveSection(): - def __init__(self, func, di=0., df=1000., step=10.): - self.di = di - self.df = df - self.n = (df - di) // step - self.u = np.arange(0, self.n + 1) / self.n - self.curv_ds = self.u * (df - di) + di - self.curv = func(self.u) - self.curv_abs = np.abs(self.curv) - self.curv_sec = np.column_stack((self.curv_abs, np.sign(self.curv), self.curv_ds)) - - -class MockOSMQueryResponse(): - def __init__(self, xml_path, query_center): - self.api = overpy.Overpass() - self.query_center = np.radians(np.array(query_center)) - - with open(xml_path, 'r') as f: - overpass_xml = f.read() - self.ways = self.api.parse_xml(overpass_xml).ways - - self.wayCollection = WayCollection(self.ways, self.query_center) - -class MockRouteData(): - def __init__(self, way_ids, way_collection, first_node_id): # way)ids must be in order forming a route. - self.wrs = [next(wr for wr in way_collection.way_relations if wr.id == way_id) for way_id in way_ids] - self.way_collection = way_collection - self.first_node_id = first_node_id - - def reset(self): - way_relations = self.wrs - wr_index = self.way_collection.wr_index - - # Nodes Data processing expects way relations to be updated with direction before running. - for idx, wr in enumerate(way_relations): - if idx == 0: - wr.update_direction_from_starting_node(self.first_node_id) - else: - wr.update_direction_from_starting_node(way_relations[idx - 1].last_node.id) - - # ***** Expected calculations - self._nodes_data = np.array([]) - self._divertions = [[]] - self._curvature_speed_sections_data = np.array([]) - way_count = len(way_relations) - if way_count == 0: - return - # We want all the nodes from the last way section - nodes_data = nodes_raw_data_array_for_wr(way_relations[-1]) - # For the ways before the last in the route we want all the nodes but the last, as that one is the first on - # the next section. Collect them, append last way node data and concatenate the numpy arrays. - if way_count > 1: - wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]]) - wrs_data += (nodes_data,) - nodes_data = np.concatenate(wrs_data) - # Get a subarray with lat, lon to compute the remaining node values. - lat_lon_array = nodes_data[:, [1, 2]] - points = np.radians(lat_lon_array) - # Ensure we have more than 3 points, if not calculations are not possible. - if len(points) <= 3: - return - vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points) - # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] - self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing)) - # Build route diversion options data from the wr_index. - wr_ids = [wr.id for wr in way_relations] - self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id) - if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)] - for node_id in nodes_data[:, 0]] - # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process. - # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign] - if len(vect) > 3: - self._curv, self._curv_ds = spline_curvature_calculations(vect, dist_prev) - self._curvature_speed_sections_data = speed_limits_for_curvatures_data(self._curv, self._curv_ds) - # ***** - - -# Test data in degrees from this road: -# https://www.google.de/maps/@52.209263,13.8723137,13z -_WAY_NODES_COORDS_01 = [ - [52.1933703, 13.8723799], - [52.1939477, 13.8711273], - [52.1942004, 13.8705818], - [52.1945408, 13.8698496], - [52.1948447, 13.8691873], - [52.1950772, 13.8685726], - [52.1951168, 13.8684641], - [52.1956681, 13.8670323], - [52.1958716, 13.8664936], - [52.1964366, 13.8649875], - [52.1969283, 13.8636040], - [52.1970203, 13.8634430], - [52.1975486, 13.8626307], - [52.1976354, 13.8624971], - [52.1977827, 13.8621795], - [52.1978564, 13.8619220], - [52.1981843, 13.8604497], - [52.1982614, 13.8602140], - [52.1983351, 13.8600595], - [52.1992768, 13.8579824], - [52.1995107, 13.8574321], - [52.1995948, 13.8572604], - [52.1996818, 13.8571155], - [52.1998000, 13.8570029], - [52.2000659, 13.8568236], - [52.2003868, 13.8566005], - [52.2007182, 13.8564460], - [52.2008760, 13.8564117], - [52.2009865, 13.8564117], - [52.2011390, 13.8564202], - [52.2012267, 13.8564496], - [52.2012544, 13.8564577], - [52.2013179, 13.8564803], - [52.2020491, 13.8571756], - [52.2026014, 13.8576991], - [52.2027592, 13.8578879], - [52.2027960, 13.8579309], - [52.2028960, 13.8580939], - [52.2030170, 13.8583343], - [52.2036587, 13.8597076], - [52.2052946, 13.8633039], - [52.2064332, 13.8658435], - [52.2067856, 13.8666332], - [52.2068961, 13.8668477], - [52.2070777, 13.8670890], - [52.2073723, 13.8674409], - [52.2077457, 13.8679387], - [52.2083874, 13.8687455], - [52.2093341, 13.8699214], - [52.2099652, 13.8707540], - [52.2102282, 13.8712089], - [52.2104228, 13.8715694], - [52.2106122, 13.8718955], - [52.2107619, 13.8721756], - [52.2108695, 13.8723771], - [52.2110747, 13.8727610], - [52.2111514, 13.8729047], - [52.2114010, 13.8733718], - [52.2114694, 13.8735006], - [52.2115430, 13.8736636], - [52.2116086, 13.8737571], - [52.2116770, 13.8738172], - [52.2117611, 13.8738515], - [52.2118664, 13.8738566], - [52.2119322, 13.8738439], - [52.2121058, 13.8737924], - [52.2122583, 13.8737495], - [52.2123265, 13.8737260], - [52.2124213, 13.8736894], - [52.2127466, 13.8734888], - [52.2128263, 13.8734491], - [52.2131313, 13.8733117], - [52.2133943, 13.8731830], - [52.2136625, 13.8731057], - [52.2139465, 13.8730456], - [52.2143619, 13.8730113], - [52.2148773, 13.8729942], - [52.2152275, 13.8730325], - [52.2153110, 13.8730398], - [52.2157442, 13.8730848], - [52.2158833, 13.8731036]] - - -mockNodesData01 = MockNodesData(_WAY_NODES_COORDS_01) - -# OSM Query around B96 south of Berlin -mockOSMResponse01 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_01.xml', - [52.31400353586984, 13.447158941786366]) - -# OSM Query on curvy town area south of Germany. -mockOSMResponse02 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_02.xml', - [48.16573269276522, 9.81418473659117]) - -mockWayCollection01 = WayCollection(mockOSMResponse01.ways, mockOSMResponse01.query_center) -mockWayCollection02 = WayCollection(mockOSMResponse02.ways, mockOSMResponse02.query_center) - -# Normal curvy Way. way id: 179532213 with 35 Nodes. -mockOSMWay_01_01_LongCurvy = next(way for way in mockOSMResponse01.ways if way.id == 179532213) - -# Looped way. way id: 29233907 -mockOSMWay_01_02_Loop = next(way for way in mockOSMResponse01.ways if way.id == 29233907) - -# Complex curvy road through town with intersections. way id:178450395 -mockOSMWay_02_01_CurvyTownWithIntersections = next(way for way in mockOSMResponse02.ways if way.id == 178450395) - -# Valid diversion for way 02_01 at node: 34785115. way id: 27955186 -mockOSMWay_02_02_Divertion_34785115 = next(way for way in mockOSMResponse02.ways if way.id == 27955186) - -# 3 node way. way id: 807781992 -mockOSMWay_02_03_Short_3_node_way = next(way for way in mockOSMResponse02.ways if way.id == 807781992) - -# data composing route 01 in way collection 02 -mockRouteData_02_01 = MockRouteData([60890967, 737120246, 601406617, 60890971, 178450395], mockWayCollection02, - first_node_id=201962346) - -# data composing route 02 in way collection 02. Single WR -mockRouteData_02_02_single_wr = MockRouteData([178450395], mockWayCollection02, first_node_id=762086638) - -# data composing route 03 in way collection 02. Multiple speed limits -mockRouteData_02_03 = MockRouteData([158799549, 798805532, 28707704, 158797898, 602249535, 602249536, 825823509, - 178449088, 916462523, 158796386], mockWayCollection02, - first_node_id=252601829) - -# 1000mt section with one full sin cycle as curv values. -mockCurveSectionSin = MockCurveSection(lambda x: np.sin(x * 2 * np.pi)) - -# 200mt section with changing curvature rate. -mockCurveSteepCurvChange = MockCurveSection(lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=200) - -# _MIN_SPEED_SECTION_LENGTH section with changing curvature rate. -mockCurveSteepCurvChangeShort = MockCurveSection( - lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=_MIN_SPEED_SECTION_LENGTH) - -# 200mt section with smooth changing curvature rate. no deviation over 2. -mockCurveSmoothCurveChange = MockCurveSection(lambda x: 0.0002 * x**3 - 0.001 * x**2 + 0.6 * x, df=200) diff --git a/selfdrive/mapd/test/mock_osm_response_01.xml b/selfdrive/mapd/test/mock_osm_response_01.xml deleted file mode 100644 index 7d9a8cf56f..0000000000 --- a/selfdrive/mapd/test/mock_osm_response_01.xml +++ /dev/null @@ -1,9908 +0,0 @@ - - - -The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/selfdrive/mapd/test/mock_osm_response_02.xml b/selfdrive/mapd/test/mock_osm_response_02.xml deleted file mode 100644 index 85886017a1..0000000000 --- a/selfdrive/mapd/test/mock_osm_response_02.xml +++ /dev/null @@ -1,11529 +0,0 @@ - - - -The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/selfdrive/mapd/test/test_NodesData.py b/selfdrive/mapd/test/test_NodesData.py deleted file mode 100644 index 0138173b90..0000000000 --- a/selfdrive/mapd/test/test_NodesData.py +++ /dev/null @@ -1,354 +0,0 @@ -import unittest -import numpy as np -from selfdrive.mapd.lib.geo import DIRECTION -from common.conversions import Conversions as CV -from selfdrive.mapd.lib.WayRelation import WayRelation -from selfdrive.mapd.lib.NodesData import nodes_raw_data_array_for_wr, node_calculations, \ - spline_curvature_calculations, split_speed_section_by_sign, split_speed_section_by_curv_degree, speed_section, \ - speed_limits_for_curvatures_data, is_wr_a_valid_divertion_from_node, SpeedLimitSection, TurnSpeedLimitSection, \ - NodesData, NodeDataIdx -from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockNodesData01, mockCurveSectionSin, \ - mockCurveSteepCurvChange, mockCurveSteepCurvChangeShort, mockCurveSmoothCurveChange, \ - mockOSMWay_02_01_CurvyTownWithIntersections, mockOSMWay_02_02_Divertion_34785115, mockOSMWay_02_03_Short_3_node_way, \ - mockRouteData_02_01, mockRouteData_02_02_single_wr, mockRouteData_02_03 -from numpy.testing import assert_array_almost_equal - - -class TestNodesDataFileFunctions(unittest.TestCase): - def test_nodes_raw_data_array_for_wr(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float) - data = nodes_raw_data_array_for_wr(wr) - - assert_array_almost_equal(data, data_e) - - def test_nodes_raw_data_array_for_wr_flips_when_backwards(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - wr.direction = DIRECTION.BACKWARD - - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float) - data_e = np.flip(data_e, axis=0) - - data = nodes_raw_data_array_for_wr(wr) - - assert_array_almost_equal(data, data_e) - - def test_nodes_raw_data_array_for_wr_drops_last(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)[:-1] - data = nodes_raw_data_array_for_wr(wr, drop_last=True) - - assert_array_almost_equal(data, data_e) - - def test_node_calculations(self): - points = mockNodesData01.radians - - v, dp, dn, dr, b = node_calculations(points) - - assert_array_almost_equal(v, mockNodesData01.v) - assert_array_almost_equal(dp, mockNodesData01.dp) - assert_array_almost_equal(dn, mockNodesData01.dn) - assert_array_almost_equal(dr, mockNodesData01.dr) - assert_array_almost_equal(b, mockNodesData01.b) - - def test_node_calculations_index_error(self): - points = mockNodesData01.radians[:2] - - with self.assertRaises(IndexError): - node_calculations(points) - - def test_spline_curvature_calculations(self): - vect = mockNodesData01.v - dist_prev = mockNodesData01.dp - - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - - assert_array_almost_equal(curv, mockNodesData01.curv) - assert_array_almost_equal(curv_ds, mockNodesData01.curv_ds) - - def test_spline_curvature_calculations_with_route_data(self): - mockRouteData_02_01.reset() - nodes_data = mockRouteData_02_01._nodes_data - vect = np.column_stack((nodes_data[:, 4], nodes_data[:, 5])) - dist_prev = nodes_data[:, 6] - - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - - assert_array_almost_equal(curv, mockRouteData_02_01._curv) - assert_array_almost_equal(curv_ds, mockRouteData_02_01._curv_ds) - - def test_split_speed_section_by_sign(self): - curv_sec = mockCurveSectionSin.curv_sec - new_secs = split_speed_section_by_sign(curv_sec) - - # 3 sections with matching initial and final distance - self.assertEqual(len(new_secs), 3) - self.assertEqual(new_secs[0][0][2], mockCurveSectionSin.di) - self.assertEqual(new_secs[2][-1][2], mockCurveSectionSin.df) - - # All new sections has same sign internally - for sec in new_secs: - self.assertEqual(np.average(sec, axis=0)[1], sec[0][1]) - - # Sections change sign - for idx in range(2): - self.assertNotEqual(new_secs[idx][0][1], new_secs[idx + 1][0][1]) - - # total items consistency - lengths = [len(sec) for sec in new_secs] - self.assertEqual(len(curv_sec), sum(lengths)) - - def test_split_speed_section_by_curv_degree(self): - curv_sec = mockCurveSteepCurvChange.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - # 3 sections with matching initial and final distance - self.assertEqual(len(new_secs), 3) - self.assertEqual(new_secs[0][0][2], mockCurveSteepCurvChange.di) - self.assertEqual(new_secs[2][-1][2], mockCurveSteepCurvChange.df) - - # Sections split at the right points - split_dist = [sec[-1][2] for sec in new_secs] - self.assertListEqual(split_dist, [50., 150., 200.]) - - def test_split_speed_section_by_curv_degree_does_nothing_if_short(self): - curv_sec = mockCurveSteepCurvChangeShort.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - self.assertEqual(len(new_secs), 1) - assert_array_almost_equal(curv_sec, new_secs[0]) - - def test_split_speed_section_by_curv_degree_does_nothing_if_no_substantial_change(self): - curv_sec = mockCurveSmoothCurveChange.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - self.assertEqual(len(new_secs), 1) - assert_array_almost_equal(curv_sec, new_secs[0]) - - def test_speed_section(self): - curv_sec = mockCurveSectionSin.curv_sec - - speed_secs = speed_section(curv_sec) - expected = np.array([0., 1000., 1.51657509, 1.]) - - assert_array_almost_equal(speed_secs, expected) - - def test_speed_limits_for_curvatures_data(self): - curv = mockCurveSectionSin.curv - curv_ds = mockCurveSectionSin.curv_ds - - expected = np.array([ - [10., 490., 1.51657509, 1.], - [510., 990., 1.51657509, -1.]]) - limits = speed_limits_for_curvatures_data(curv, curv_ds) - - assert_array_almost_equal(limits, expected) - - def test_is_wr_a_valid_divertion_from_node(self): - wr = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections) - mockOSMWay_02_02_Divertion_34785115.tags['oneway'] = 'yes' - wr_div = WayRelation(mockOSMWay_02_02_Divertion_34785115) - - # False if id already in route - wr_ids = [wr.id, wr_div.id] - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids)) - - # True if id not in route, node_id is edge and not prohibited - wr_ids = [wr.id, 11111, 22222] - self.assertTrue(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids)) - - # False if id not in route, node_id is edge but prohibited (wrong direction from node 319503453) - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 319503453, wr_ids)) - - # False if id not in route, node_id is not edge - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 44444, wr_ids)) - - -class TestSpeedLimitSection(unittest.TestCase): - def test_speed_limit_section_init(self): - section = SpeedLimitSection(10., 20., 50.) - - self.assertEqual(section.start, 10.) - self.assertEqual(section.end, 20.) - self.assertEqual(section.value, 50.) - - -class TestTurnSpeedLimitSection(unittest.TestCase): - def test_turn_speed_limit_section_init(self): - section = TurnSpeedLimitSection(10., 20., 50., -1.) - - self.assertEqual(section.start, 10.) - self.assertEqual(section.end, 20.) - self.assertEqual(section.value, 50.) - self.assertEqual(section.curv_sign, -1.) - - -class TestNodesData(unittest.TestCase): - def test_init_with_empty_list(self): - nodesData = NodesData([], {}) - - self.assertEqual(len(nodesData._nodes_data), 0) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 0) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 0) - - def test_init_with_single_wr_includes_all_wr_nodes(self): - mockRouteData_02_02_single_wr.reset() - way_relations = mockRouteData_02_02_single_wr.wrs - wr_index = mockRouteData_02_02_single_wr.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData._nodes_data, mockRouteData_02_02_single_wr._nodes_data) - assert_array_almost_equal(nodesData._curvature_speed_sections_data, - mockRouteData_02_02_single_wr._curvature_speed_sections_data) - self.assertListEqual(nodesData._divertions, mockRouteData_02_02_single_wr._divertions) - self.assertEqual(len(nodesData._nodes_data), len(way_relations[0].way.nodes)) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 6) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 6) - - def test_init_with_less_than_4_nodes(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - - self.assertEqual(len(nodesData._nodes_data), 0) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 0) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 0) - - def test_init_with_multiple_wr(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData._nodes_data, mockRouteData_02_01._nodes_data) - assert_array_almost_equal(nodesData._curvature_speed_sections_data, mockRouteData_02_01._curvature_speed_sections_data) - self.assertListEqual(nodesData._divertions, mockRouteData_02_01._divertions) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 9) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 14) - - def test_count(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - num_n = sum([len(wr.way.nodes) for wr in way_relations]) - len(way_relations) + 1 - - nodesData = NodesData(way_relations, wr_index) - - self.assertEqual(nodesData.count, num_n) - - def test_get_on_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - assert_array_almost_equal(nodesData.get(NodeDataIdx.node_id), np.array([])) - - def test_get_values(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData.get(NodeDataIdx.node_id), mockRouteData_02_01._nodes_data[:, 0]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.lat), mockRouteData_02_01._nodes_data[:, 1]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.lon), mockRouteData_02_01._nodes_data[:, 2]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.speed_limit), mockRouteData_02_01._nodes_data[:, 3]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.x), mockRouteData_02_01._nodes_data[:, 4]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.y), mockRouteData_02_01._nodes_data[:, 5]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_prev), mockRouteData_02_01._nodes_data[:, 6]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_next), mockRouteData_02_01._nodes_data[:, 7]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_route), mockRouteData_02_01._nodes_data[:, 8]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.bearing), mockRouteData_02_01._nodes_data[:, 9]) - - def test_speed_limits_ahead_from_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - self.assertEqual(len(nodesData.speed_limits_ahead(1, 10.)), 0) - - def test_speed_limits_ahead(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - # empty when ahead_idx is none. - self.assertEqual(len(nodesData.speed_limits_ahead(None, 10.)), 0) - - # All limist from 0 - all_limits = nodesData.speed_limits_ahead(1, nodesData.get(NodeDataIdx.dist_next)[0]) - self.assertEqual(len(all_limits), 4) # 4 limits on this mock road. - self.assertListEqual([sl.value for sl in all_limits], [v * CV.KPH_TO_MS for v in [50, 100, 50, 100]]) - for idx, sl in enumerate(all_limits): - self.assertTrue(sl.end > sl.start) - self.assertTrue(sl.value > 0.) - if idx == 0: - self.assertEqual(sl.start, 0.) - else: - self.assertEqual(sl.start, all_limits[idx - 1].end) - self.assertNotEqual(sl.value, all_limits[idx - 1].value) - - def test_distance_to_end_from_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - self.assertIsNone(nodesData.distance_to_end(1, 10.)) - - def test_distance_to_end(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - # none when ahead_idx is none. - self.assertIsNone(nodesData.distance_to_end(None, 10.)) - - # From the beginning - expected = np.sum(nodesData.get(NodeDataIdx.dist_next)) - self.assertAlmostEqual(nodesData.distance_to_end(1, nodesData.get(NodeDataIdx.dist_next)[0]), expected) - self.assertAlmostEqual(nodesData.get(NodeDataIdx.dist_route)[-1], expected) - - # From the node next to last - expected = nodesData.get(NodeDataIdx.dist_next)[-2] - self.assertAlmostEqual(nodesData.distance_to_end(nodesData.count - 2, 0.), expected) - - def test_distance_to_node(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - dist_to_node_ahead = 10. - node_id = 1887995486 # Some node id in the middle of the way. idx 50 - node_idx = np.nonzero(nodesData.get(NodeDataIdx.node_id) == node_id)[0][0] - - # none when ahead_idx is none. - self.assertIsNone(nodesData.distance_to_node(node_id, None, dist_to_node_ahead)) - - # From the beginning - expected = nodesData.get(NodeDataIdx.dist_route)[node_idx] - self.assertAlmostEqual(nodesData.distance_to_node(node_id, 1, nodesData.get(NodeDataIdx.dist_next)[0]), expected) - - # From the end - expected = -np.sum(nodesData.get(NodeDataIdx.dist_next)[node_idx:]) - self.assertAlmostEqual(nodesData.distance_to_node(node_id, len(nodesData.get(NodeDataIdx.node_id)) - 1, 0.), expected) - - # From some node behind including dist to node ahead - ahead_idx = node_idx - 10 - expected = np.sum(nodesData.get(NodeDataIdx.dist_next)[ahead_idx:node_idx]) + dist_to_node_ahead - self.assertAlmostEqual(nodesData.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected) - - # From some node ahead including dist to node ahead - ahead_idx = node_idx + 10 - expected = -np.sum(nodesData.get(NodeDataIdx.dist_next)[node_idx:ahead_idx]) + dist_to_node_ahead - self.assertAlmostEqual(nodesData.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected) - -# TODO: Missing tests for curvatures_speed_limit_sections_ahead and possible_divertions diff --git a/selfdrive/mapd/test/test_WayRelation.py b/selfdrive/mapd/test/test_WayRelation.py deleted file mode 100644 index 0e3ea030c6..0000000000 --- a/selfdrive/mapd/test/test_WayRelation.py +++ /dev/null @@ -1,651 +0,0 @@ -import copy -import unittest -import numpy as np -from unittest import mock -from numpy.testing import assert_array_almost_equal -from datetime import datetime as dt, timezone, timedelta -from common.conversions import Conversions as CV -from selfdrive.mapd.lib.WayRelation import WayRelation, is_osm_time_condition_active, \ - conditional_speed_limit_for_osm_tag_limit_string, speed_limit_for_osm_tag_limit_string -from selfdrive.mapd.config import LANE_WIDTH -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors -from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockOSMWay_01_02_Loop, \ - mockOSMWay_02_01_CurvyTownWithIntersections - - -class TestWayRelationFileFunctions(unittest.TestCase): - def test_speed_limit_for_osm_tag_limit_string(self): - values = [ - None, # Invalid - "1000", # Invalid - "60 kph", # Invalid - "100", - "30 mph", - "DE:zone:40", - "DE:zone:50 mph", - "AR:urban", - "CZ:pedestrian_zone", - "DK:urban", - "DK:rural", - "DK:motorway", - "DE:living_street", - "DE:residential", - "DE:urban", - "DE:rural", - "DE:trunk", # No limit - "DE:motorway", # No limit - "GB:nsl_restricted", - "GB:nsl_single", - "GB:nsl_dual", - "GB:motorway", - "GB:invalid", # Invalid - ] - - expected = [ - 0., - 0., - 0., - 100. * CV.KPH_TO_MS, - 30. * CV.MPH_TO_MS, - 40. * CV.KPH_TO_MS, - 50. * CV.MPH_TO_MS, - 40. * CV.KPH_TO_MS, - 20. * CV.KPH_TO_MS, - 50. * CV.KPH_TO_MS, - 80. * CV.KPH_TO_MS, - 130. * CV.KPH_TO_MS, - 7. * CV.KPH_TO_MS, - 30. * CV.KPH_TO_MS, - 50. * CV.KPH_TO_MS, - 100. * CV.KPH_TO_MS, - 0., - 0., - 30. * CV.MPH_TO_MS, - 60. * CV.MPH_TO_MS, - 70. * CV.MPH_TO_MS, - 70. * CV.MPH_TO_MS, - 0., - ] - - result = [speed_limit_for_osm_tag_limit_string(sls) for sls in values] - - self.assertEqual(result, expected) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_is_osm_time_condition_active(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - values = [ - "WE", # Invalid - "We", - "Mo", - "Fr", - "Tu-Th", - "10:00", # Invalid - "10:00-10:30", - "We 10:00-10:30", - "SU 10:00-10:30", # Valid, SU string not considered a day string. - "Sa 10:00-10:30", - "Tu-Th 10:00-10:30", - ] - - expected = [ - False, # Invalid - True, - False, - False, - True, - False, # Invalid - True, - True, - True, - False, - True, - ] - - result = [is_osm_time_condition_active(cs) for cs in values] - - self.assertEqual(result, expected) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_conditional_speed_limit_for_osm_tag_limit_string(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - values = [ - None, # Invalid - "Hola", # Invalid - "100 @ (WE)", # Invalid - "x @ (We)", # Invalid - "100 @ (We)", - "100 @ (Mo)", - "100 @ (Fr)", - "100 @ (Tu-Th)", - "100 @ (10:00)", # Invalid - "100 @ (10:00-10:30)", - "100 @ (We 10:00-10:30)", - "100 @ (SU 10:00-10:30)", # Valid, SU string not considered a day string. - "100 @ (Sa 10:00-10:30)", - "100 @ (Tu-Th 10:00-10:30)", - "100 @ (Mo-Th;Su)", - "100 @ (Mo Th;Fr-Sa)", - "100 @ (Fr-Su;Mo-Tu)", - "100 @ (10:00-10:30;15:00-16:00)", - "100 @ (We;Mo-Tu)", - "100 @ (We 10:00-10:30;Th 15:00-16:00)", - "100 @ (Tu 10:00-10:30;Th 15:00-16:00)", - ] - - _100 = 100. * CV.KPH_TO_MS - - expected = [ - 0., # Invalid - 0., # Invalid - 0., # Invalid - 0., # Invalid - _100, - 0., - 0., - _100, - 0., # Invalid - _100, - _100, - _100, - 0., - _100, - _100, - _100, - 0., - _100, - _100, - _100, - 0. - ] - - result = [conditional_speed_limit_for_osm_tag_limit_string(ls) for ls in values] - - self.assertEqual(result, expected) - - -class TestWayRelation(unittest.TestCase): - def test_way_relation_init(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - nodes_np_expected = np.radians(np.array([[node.lat, node.lon] for node in wayRelation.way.nodes], dtype=float)) - v = vectors(wayRelation._nodes_np) - way_distances_expected = np.linalg.norm(v * R, axis=1) - way_bearings_expected = np.arctan2(v[:, 0], v[:, 1]) - bbox_expected = np.array([ - [0.91321784, 0.2346417], - [0.91344672, 0.23475751]]) - - self.assertEqual(wayRelation.way.id, 179532213) - self.assertIsNone(wayRelation.parent_wr_id) - self.assertEqual(wayRelation.direction, DIRECTION.NONE) - self.assertEqual(wayRelation._speed_limit, None) - self.assertEqual(wayRelation._one_way, 'yes') - self.assertEqual(wayRelation.name, None) - self.assertEqual(wayRelation.ref, 'B 96') - self.assertEqual(wayRelation.highway_type, 'trunk') - self.assertEqual(wayRelation.highway_rank, 10) - self.assertEqual(wayRelation.lanes, 2) - assert_array_almost_equal(wayRelation._nodes_np, nodes_np_expected) - assert_array_almost_equal(wayRelation._way_distances, way_distances_expected) - assert_array_almost_equal(wayRelation._way_bearings, way_bearings_expected) - assert_array_almost_equal(wayRelation.bbox, bbox_expected) - self.assertEqual(wayRelation.edge_nodes_ids, [wayRelation.way.nodes[0].id, wayRelation.way.nodes[-1].id]) - - def test_way_relation_init_with_parent(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy, parent=WayRelation(mockOSMWay_01_02_Loop)) - - self.assertEqual(wayRelation.way.id, 179532213) - self.assertEqual(wayRelation.parent_wr_id, 29233907) - - def test_way_relation_equality(self): - wayRelation1 = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation2 = copy.copy(wayRelation1) - wayRelation3 = copy.deepcopy(wayRelation1) - wayRelation3.way.id = 123 - - self.assertEqual(wayRelation1, wayRelation2) - self.assertNotEqual(wayRelation1, wayRelation3) - - def test_way_relation_reset_location_variables(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.make_wayRelation_location_dirty(wayRelation) - - wayRelation.reset_location_variables() - - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_id(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - self.assertEqual(wayRelation.id, 179532213) - - def test_way_relation_road_name(self): - # road name when no tag for name or ref - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - self.assertIsNone(wayRelation.road_name) - # road name based on ref tag - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.assertEqual(wayRelation.road_name, "B 96") - # road name based on name tag - wayRelation = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections) - self.assertEqual(wayRelation.road_name, "Hauptstraße") - - def test_way_relation_update_resets_on_update(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.make_wayRelation_location_dirty(wayRelation) - location_rad = np.array([0., 0.]) # Location outside bbox - - wayRelation.update(location_rad, 0., 10.) - - self.assertFalse(wayRelation.is_location_in_bbox(location_rad)) - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_update_only_resets_if_no_possible_found(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - location_rad = wayRelation.bbox[0] # Location inside bbox but outside actual way (due to padding) - - wayRelation.update(location_rad, 0., 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_updates_in_the_correct_direction_with_correct_property_values(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - location_rad = np.radians(np.array([52.32855593146639, 13.445320150125069])) - bearing_rad = 0. - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.FORWARD) - self.assertEqual(wayRelation.ahead_idx, 17) - self.assertEqual(wayRelation.behind_idx, 16) - self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.320717420388962) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 25.4998961709014) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - bearing_rad = 180. - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - self.assertEqual(wayRelation.ahead_idx, 16) - self.assertEqual(wayRelation.behind_idx, 17) - self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.9507682562504284) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 11.11623371145368) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_updates_with_location_closest_to_way_when_multiple_possible(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - location_rad = np.radians(np.array([52.313303275461564, 13.437729236325788])) - bearing_rad = np.radians(10.) - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - self.assertEqual(wayRelation.ahead_idx, 26) - self.assertEqual(wayRelation.behind_idx, 27) - self.assertAlmostEqual(wayRelation._distance_to_way, 10.151775235257011) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.06371131069242782) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 10.174073707120915) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_updates_will_become_inactive_if_too_far_from_way(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # Location is 24.9 mts away from the way. There are 2 Lanes in this way. - location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788])) - location_stdev = 5.5 # threshold is 4 * location_stdev + LANE_WIDTH - distance_threshold = 4. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - self.assertTrue(wayRelation.active) - self.assertLess(wayRelation._distance_to_way, distance_threshold) - - location_stdev = 5. - - wayRelation.update(location_rad, 0., location_stdev) - self.assertFalse(wayRelation.active) - - def test_way_relation_updates_will_update_diverting_correctly(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # Location is 24.9 mts away from the way. There are 2 Lanes in this way. - location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788])) - location_stdev = 11. - distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - - self.assertLess(wayRelation._distance_to_way, distance_threshold) - self.assertFalse(wayRelation.diverting) - - location_stdev = 10. - distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - - self.assertGreater(wayRelation._distance_to_way, distance_threshold) - self.assertTrue(wayRelation.diverting) - - def test_way_relation_update_direction_from_starting_node_resets_speed_limit(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation._speed_limit = 10. - - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id) - - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_update_direction_from_starting_node_updates_correctly(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id) - self.assertEqual(wayRelation.direction, DIRECTION.FORWARD) - - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[-1].id) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - - wayRelation.update_direction_from_starting_node(0) - self.assertEqual(wayRelation.direction, DIRECTION.NONE) - - def test_way_relation_is_location_in_bbox(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - bbox = wayRelation.bbox - - loc_avg = np.average(bbox, axis=0) - loc_min = np.min(bbox, axis=0) - loc_max = np.max(bbox, axis=0) - - locations = [ - loc_avg, - loc_min, - loc_max, - [loc_avg[0], loc_min[1]], - [loc_avg[0], loc_max[1]], - [loc_min[0], loc_avg[1]], - [loc_max[0], loc_avg[1]], - loc_min - 0.1, - loc_max + 0.1, - [loc_avg[0], loc_min[1] - 0.1], - [loc_avg[0], loc_max[1] + 0.1], - [loc_min[0] - 0.1, loc_avg[1]], - [loc_max[0] + 0.1, loc_avg[1]], - ] - - is_in = [wayRelation.is_location_in_bbox(loc) for loc in locations] - - self.assertEqual(is_in, [True, True, True, True, True, True, True, False, False, False, False, False, False]) - - def test_way_relation_speed_limit_when_set(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._speed_limit = 10. - - self.assertEqual(wayRelation.speed_limit, 10.) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_way_relation_speed_limit_conditional(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - # Reset all tags before teting - mockOSMWay_01_02_Loop.tags = {} - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # No Value - self.assertEqual(wayRelation.speed_limit, 0.) - - # Value on both directions - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on forward - wayRelation.way.tags.pop("maxspeed:conditional") - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:forward:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on backward - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:backward:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - def test_way_relation_speed_limit_maxspeed(self): - # Reset all tags before teting - mockOSMWay_01_02_Loop.tags = {} - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # No Value - self.assertEqual(wayRelation.speed_limit, 0.) - - # Value on both directions - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on forward - wayRelation.way.tags.pop("maxspeed") - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:forward"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on backward - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:backward"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - def test_way_relation_active_bearing_delta_reflects_internal_value(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._active_bearing_delta = 10. - self.assertEqual(wayRelation.active_bearing_delta, 10.) - - def test_way_relation_is_one_way(self): - # Setup initial tags - mockOSMWay_01_02_Loop.tags = { - 'oneway': 'yes', - 'highway': 'unclassified' - } - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # oneway = yes - self.assertTrue(wayRelation.is_one_way) - - # oneway non existing - wayRelation._one_way = None - self.assertFalse(wayRelation.is_one_way) - - # highway = motorway - wayRelation.highway_type = 'motorway' - self.assertTrue(wayRelation.is_one_way) - - def test_way_relation_is_prohibited(self): - # Setup initial tags - mockOSMWay_01_02_Loop.tags = { - 'oneway': 'yes' - } - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # Direction undefined - wayRelation.direction = DIRECTION.NONE - self.assertTrue(wayRelation.is_prohibited) - - # oneway = yes - wayRelation.direction = DIRECTION.BACKWARD - self.assertTrue(wayRelation.is_prohibited) - - wayRelation.direction = DIRECTION.FORWARD - self.assertFalse(wayRelation.is_prohibited) - - # oneway non existing - wayRelation._one_way = None - self.assertFalse(wayRelation.is_one_way) - - wayRelation.direction = DIRECTION.BACKWARD - self.assertFalse(wayRelation.is_prohibited) - - def test_way_relation_distance_to_way_reflects_internal_value(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._distance_to_way = 10. - self.assertEqual(wayRelation.distance_to_way, 10.) - - def test_way_relation_node_ahead(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # ahead_ids is None on init - self.assertIsNone(wayRelation.node_ahead) - - wayRelation.ahead_idx = 15 - self.assertEqual(wayRelation.node_ahead, wayRelation.way.nodes[15]) - - def test_way_relation_last_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # direction is NONE on init - self.assertIsNone(wayRelation.last_node) - - # forward - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[-1]) - - # backward - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[0]) - - def test_way_relation_last_node_coordinates(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # direction is NONE on init - self.assertIsNone(wayRelation.last_node_coordinates) - - # forward - wayRelation.direction = DIRECTION.FORWARD - coords = np.radians(np.array([wayRelation.way.nodes[-1].lat, wayRelation.way.nodes[-1].lon], dtype=float)) - assert_array_almost_equal(wayRelation.last_node_coordinates, coords) - - # backward - wayRelation.direction = DIRECTION.BACKWARD - coords = np.radians(np.array([wayRelation.way.nodes[0].lat, wayRelation.way.nodes[0].lon], dtype=float)) - assert_array_almost_equal(wayRelation.last_node_coordinates, coords) - - def test_way_relation_node_before_edge_coordinates(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - coords = wayRelation.node_before_edge_coordinates(0) - assert_array_almost_equal(coords, np.array([0., 0.])) - - coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[0].id) - coords_e = np.radians(np.array([wayRelation.way.nodes[1].lat, wayRelation.way.nodes[1].lon], dtype=float)) - assert_array_almost_equal(coords, coords_e) - - coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[-1].id) - coords_e = np.radians(np.array([wayRelation.way.nodes[-2].lat, wayRelation.way.nodes[-2].lon], dtype=float)) - assert_array_almost_equal(coords, coords_e) - - def test_way_relation_split_no_matching_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - wrs = wayRelation.split(0) - self.assertEqual(len(wrs), 0) - - def test_way_relation_split_use_correct_ids(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - wrs = wayRelation.split(wayRelation._nodes_ids[5], [-100, -200]) - self.assertEqual(wrs[0].id, -100) - self.assertEqual(wrs[1].id, -200) - - def test_way_relation_split_on_edge_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - edge_node_ids = wayRelation.edge_nodes_ids - - for edge_node_id in edge_node_ids: - wrs = wayRelation.split(edge_node_id) - self.assertEqual(len(wrs), 1) - self.assertEqual(wrs[0], wayRelation) - self.assertEqual(wrs[0].way.tags, wayRelation.way.tags) - - def test_way_relation_split_on_internal_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - way_ids = [-10, -20] - - for idx, node_id in enumerate(wayRelation._nodes_ids): - if idx == 0 or idx == len(wayRelation._nodes_ids) - 1: - continue - wrs = wayRelation.split(node_id, way_ids) - self.assertEqual(len(wrs), 2) - assert_array_almost_equal(wrs[0]._nodes_ids, wayRelation._nodes_ids[:idx + 1]) - assert_array_almost_equal(wrs[1]._nodes_ids, wayRelation._nodes_ids[idx:]) - self.assertIn(node_id, wrs[0].edge_nodes_ids) - self.assertIn(node_id, wrs[1].edge_nodes_ids) - self.assertEqual(wrs[0].way.tags, wayRelation.way.tags) - self.assertEqual(wrs[1].way.tags, wayRelation.way.tags) - self.assertEqual(way_ids, [wr.id for wr in wrs]) - - # Helpers - def make_wayRelation_location_dirty(self, wayRelation): - wayRelation.distance_to_node_ahead = 10. - wayRelation.location_rad = 0.8 - wayRelation.bearing_rad = 2. - wayRelation.active = True - wayRelation.diverting = True - wayRelation.ahead_idx = 5 - wayRelation.behind_idx = 4 - wayRelation._active_bearing_delta = 3. - wayRelation._distance_to_way = 20. - - def assert_wayRelation_variables_reset(self, wayRelation): - self.assertEqual(wayRelation.distance_to_node_ahead, 0.) - self.assertIsNone(wayRelation.location_rad) - self.assertIsNone(wayRelation.bearing_rad) - self.assertFalse(wayRelation.active) - self.assertFalse(wayRelation.diverting) - self.assertIsNone(wayRelation.ahead_idx) - self.assertIsNone(wayRelation.behind_idx) - self.assertIsNone(wayRelation._active_bearing_delta) - self.assertIsNone(wayRelation._distance_to_way) - - def wayRelation_mid_point_rad(self, wayRelation): - return np.average(wayRelation.bbox, axis=0) diff --git a/selfdrive/mapd/test/test_WayRelationIndex.py b/selfdrive/mapd/test/test_WayRelationIndex.py deleted file mode 100644 index f8ef212bc3..0000000000 --- a/selfdrive/mapd/test/test_WayRelationIndex.py +++ /dev/null @@ -1,74 +0,0 @@ -import unittest -from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex -from selfdrive.mapd.test.mock_data import mockWayCollection01 - - -class TestWayRelationIndex(unittest.TestCase): - def test_init_and_add(self): - wrs = mockWayCollection01.way_relations - wr_index = WayRelationIndex(wrs) - - # expected init logic, including add logic. - edge_nodes_index_dict = {} - full_nodes_index_dict = {} - for wr in wrs: - for node in wr.way.nodes: - node_id = node.id - full_nodes_index_dict[node_id] = full_nodes_index_dict.get(node_id, []) + [wr] - if node_id in wr.edge_nodes_ids: - edge_nodes_index_dict[node_id] = edge_nodes_index_dict.get(node_id, []) + [wr] - - # assert logic delivers same result - self.assertDictEqual(edge_nodes_index_dict, wr_index._edge_nodes_index_dict) - self.assertDictEqual(full_nodes_index_dict, wr_index._full_nodes_index_dict) - self.assertEqual(len(wr_index._edge_nodes_index_dict), 586) - self.assertEqual(len(wr_index._full_nodes_index_dict), 2342) - - def test_remove(self): - wrs = mockWayCollection01.way_relations - wr_index = WayRelationIndex(wrs) - - wr_to_remove = wrs[0] - affected_full_node_ids = [nodesData.id for nodesData in wr_to_remove.way.nodes] - affected_edge_node_ids = wr_to_remove.edge_nodes_ids - - initial_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids] - initial_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids] - - expected_final_full_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_full_lists] - expected_final_edge_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_edge_lists] - - wr_index.remove(wr_to_remove) - - final_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids] - final_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids] - - for idx, li in enumerate(final_full_lists): - self.assertListEqual(li, expected_final_full_lists[idx]) - - for idx, li in enumerate(final_edge_lists): - self.assertListEqual(li, expected_final_edge_lists[idx]) - - def test_way_relations_with_edge_node_id(self): - wr_index = WayRelationIndex([]) - ref_dict = { - 0: ["fake_wr1", "fake_wr2"], - 1: ["fake_wr3"], - 3: ["fake_wr4", "fake_wr5", "fake_wr6"], - } - wr_index._edge_nodes_index_dict = ref_dict - - for key, li in ref_dict.items(): - self.assertListEqual(li, wr_index.way_relations_with_edge_node_id(key)) - - def test_way_relations_with_node_id(self): - wr_index = WayRelationIndex([]) - ref_dict = { - 0: ["fake_wr1", "fake_wr2"], - 1: ["fake_wr3"], - 3: ["fake_wr4", "fake_wr5", "fake_wr6"], - } - wr_index._full_nodes_index_dict = ref_dict - - for key, li in ref_dict.items(): - self.assertListEqual(li, wr_index.way_relations_with_node_id(key)) diff --git a/selfdrive/mapd/test/test_geo.py b/selfdrive/mapd/test/test_geo.py deleted file mode 100644 index a18fe717b0..0000000000 --- a/selfdrive/mapd/test/test_geo.py +++ /dev/null @@ -1,234 +0,0 @@ -import unittest -from selfdrive.mapd.lib.geo import vectors, ref_vectors, bearing_to_points, distance_to_points -import numpy as np -from numpy.testing import assert_array_almost_equal -from selfdrive.mapd.test.mock_data import mockNodesData01 - - -class TestMapsdGeoLibrary(unittest.TestCase): - def test_vectors(self): - points = mockNodesData01.radians - expected = np.array([ - [-1.34011951e-05, 1.00776468e-05], - [-5.83610920e-06, 4.41046897e-06], - [-7.83348567e-06, 5.94114032e-06], - [-7.08560788e-06, 5.30408795e-06], - [-6.57632550e-06, 4.05791838e-06], - [-1.16077872e-06, 6.91151252e-07], - [-1.53178098e-05, 9.62215139e-06], - [-5.76314175e-06, 3.55176643e-06], - [-1.61124141e-05, 9.86127759e-06], - [-1.48006628e-05, 8.58192512e-06], - [-1.72237209e-06, 1.60570482e-06], - [-8.68985228e-06, 9.22062311e-06], - [-1.42922812e-06, 1.51494711e-06], - [-3.39761486e-06, 2.57087743e-06], - [-2.75467373e-06, 1.28631255e-06], - [-1.57501989e-05, 5.72309451e-06], - [-2.52143954e-06, 1.34565295e-06], - [-1.65278643e-06, 1.28630942e-06], - [-2.22196114e-05, 1.64360838e-05], - [-5.88675934e-06, 4.08234746e-06], - [-1.83673390e-06, 1.46782408e-06], - [-1.55004206e-06, 1.51843800e-06], - [-1.20451533e-06, 2.06298011e-06], - [-1.91801338e-06, 4.64083285e-06], - [-2.38653483e-06, 5.60076524e-06], - [-1.65269781e-06, 5.78402290e-06], - [-3.66908309e-07, 2.75412965e-06], - [0.00000000e+00, 1.92858882e-06], - [9.09242615e-08, 2.66162711e-06], - [3.14490354e-07, 1.53065382e-06], - [8.66452477e-08, 4.83456208e-07], - [2.41750593e-07, 1.10828411e-06], - [7.43745228e-06, 1.27618831e-05], - [5.59968054e-06, 9.63947367e-06], - [2.01951467e-06, 2.75413219e-06], - [4.59952643e-07, 6.42281301e-07], - [1.74353749e-06, 1.74533121e-06], - [2.57144338e-06, 2.11185266e-06], - [1.46893187e-05, 1.11999169e-05], - [3.84659229e-05, 2.85527952e-05], - [2.71627936e-05, 1.98727946e-05], - [8.44632540e-06, 6.15058628e-06], - [2.29420323e-06, 1.92859222e-06], - [2.58083439e-06, 3.16952222e-06], - [3.76373643e-06, 5.14174911e-06], - [5.32416098e-06, 6.51707770e-06], - [8.62890928e-06, 1.11998258e-05], - [1.25762497e-05, 1.65231340e-05], - [8.90452991e-06, 1.10148240e-05], - [4.86505726e-06, 4.59023120e-06], - [3.85545276e-06, 3.39642031e-06], - [3.48753893e-06, 3.30566145e-06], - [2.99557303e-06, 2.61276368e-06], - [2.15496788e-06, 1.87797727e-06], - [4.10564937e-06, 3.58142649e-06], - [1.53680853e-06, 1.33866906e-06], - [4.99540175e-06, 4.35635790e-06], - [1.37744970e-06, 1.19380643e-06], - [1.74319821e-06, 1.28456429e-06], - [9.99931238e-07, 1.14493663e-06], - [6.42735560e-07, 1.19380547e-06], - [3.66818436e-07, 1.46782199e-06], - [5.45413874e-08, 1.83783170e-06], - [-1.35818548e-07, 1.14842666e-06], - [-5.50758101e-07, 3.02989178e-06], - [-4.58785270e-07, 2.66162724e-06], - [-2.51315555e-07, 1.19031459e-06], - [-3.91409773e-07, 1.65457223e-06], - [-2.14525206e-06, 5.67755902e-06], - [-4.24558096e-07, 1.39102753e-06], - [-1.46936730e-06, 5.32325561e-06], - [-1.37632061e-06, 4.59021715e-06], - [-8.26642899e-07, 4.68097349e-06], - [-6.42702724e-07, 4.95673534e-06], - [-3.66796960e-07, 7.25009780e-06], - [-1.82861669e-07, 8.99542699e-06], - [4.09564134e-07, 6.11214315e-06], - [7.80629912e-08, 1.45734993e-06], - [4.81205526e-07, 7.56076647e-06], - [2.01036346e-07, 2.42775302e-06]]) - - v = vectors(points) - assert_array_almost_equal(v, expected) - - def test_ref_vectors(self): - points = mockNodesData01.radians - expected = np.array([ - [1.59924145e-04, -1.07153714e-04], - [1.46520873e-04, -9.70788297e-05], - [1.40683931e-04, -9.26694631e-05], - [1.32849368e-04, -8.67297434e-05], - [1.25762852e-04, -8.14268689e-05], - [1.19185869e-04, -7.73700167e-05], - [1.18024984e-04, -7.66790438e-05], - [1.02705711e-04, -6.70592230e-05], - [9.69420991e-05, -6.35082196e-05], - [8.08284530e-05, -5.36489556e-05], - [6.60268961e-05, -4.50685727e-05], - [6.43043874e-05, -4.34630144e-05], - [5.56137708e-05, -3.42431117e-05], - [5.41844341e-05, -3.27282671e-05], - [5.07866397e-05, -3.01576270e-05], - [4.80318817e-05, -2.88714948e-05], - [3.22813286e-05, -2.31493755e-05], - [2.97598330e-05, -2.18038275e-05], - [2.81069973e-05, -2.05175815e-05], - [5.88679032e-06, -4.08230278e-06], - [0.00000000e+00, 0.00000000e+00], - [-1.83673390e-06, 1.46782408e-06], - [-3.38677236e-06, 2.98626574e-06], - [-4.59127869e-06, 5.04925111e-06], - [-6.50926460e-06, 9.69009532e-06], - [-8.89575243e-06, 1.52908806e-05], - [-1.05483839e-05, 2.10749224e-05], - [-1.09152548e-05, 2.38290571e-05], - [-1.09152276e-05, 2.57576459e-05], - [-1.08242659e-05, 2.84192717e-05], - [-1.05097542e-05, 2.99499212e-05], - [-1.04231024e-05, 3.04333762e-05], - [-1.01813369e-05, 3.15416571e-05], - [-2.74371711e-06, 4.43034426e-05], - [2.85599752e-06, 5.39428964e-05], - [4.87550206e-06, 5.66970360e-05], - [5.33545066e-06, 5.73393202e-05], - [7.07897615e-06, 5.90846634e-05], - [9.65040026e-06, 6.11965396e-05], - [2.43395796e-05, 7.23966392e-05], - [6.28046063e-05, 1.00950641e-04], - [8.99657904e-05, 1.20825635e-04], - [9.84114021e-05, 1.26977201e-04], - [1.00705361e-04, 1.28906084e-04], - [1.03285783e-04, 1.32075942e-04], - [1.07048835e-04, 1.37218192e-04], - [1.12372096e-04, 1.43736004e-04], - [1.20999382e-04, 1.54937080e-04], - [1.33573053e-04, 1.71462176e-04], - [1.42475686e-04, 1.82478533e-04], - [1.47339899e-04, 1.87069658e-04], - [1.51194707e-04, 1.90466811e-04], - [1.54681601e-04, 1.93773152e-04], - [1.57676653e-04, 1.96386513e-04], - [1.59831239e-04, 1.98264929e-04], - [1.63936150e-04, 2.01847201e-04], - [1.65472675e-04, 2.03186195e-04], - [1.70467147e-04, 2.07543619e-04], - [1.71844334e-04, 2.08737728e-04], - [1.73587247e-04, 2.10022678e-04], - [1.74586922e-04, 2.11167839e-04], - [1.75229389e-04, 2.12361789e-04], - [1.75595876e-04, 2.13829694e-04], - [1.75650001e-04, 2.15667538e-04], - [1.75513922e-04, 2.16815933e-04], - [1.74962478e-04, 2.19845700e-04], - [1.74503092e-04, 2.22507224e-04], - [1.74251509e-04, 2.23697482e-04], - [1.73859727e-04, 2.25351966e-04], - [1.71713202e-04, 2.31029044e-04], - [1.71288336e-04, 2.32419977e-04], - [1.69817793e-04, 2.37742908e-04], - [1.68440467e-04, 2.42332824e-04], - [1.67612807e-04, 2.47013617e-04], - [1.66969033e-04, 2.51970213e-04], - [1.66600674e-04, 2.59220232e-04], - [1.66415880e-04, 2.68215619e-04], - [1.66824132e-04, 2.74327850e-04], - [1.66901881e-04, 2.75785216e-04], - [1.67381459e-04, 2.83346086e-04], - [1.67581971e-04, 2.85773882e-04]]) - - v = ref_vectors(points[20], points) - assert_array_almost_equal(v, expected) - - def test_bearing_to_points(self): - points = mockNodesData01.radians - expected = np.array([ - 2.16112265, 2.15595027, 2.15326799, 2.14916735, 2.14538642, - 2.14657678, 2.14694997, 2.1492257, 2.1507589, 2.15676899, - 2.16973441, 2.1651606, 2.12270237, 2.11416356, 2.10665211, - 2.11201708, 2.19291574, 2.2031069, 2.20136186, 2.17712517, - 0., -0.8965745, -0.84815954, -0.73792895, -0.59150953, - -0.5269061, -0.46406215, -0.42954043, -0.4008254, -0.36391371, - -0.33748609, -0.32996807, -0.31223189, -0.06185112, 0.05289544, - 0.08578116, 0.0927833, 0.11924233, 0.15640718, 0.32432622, - 0.55653415, 0.64003094, 0.6593301, 0.66319086, 0.66367982, - 0.66251077, 0.66354137, 0.66302176, 0.66181884, 0.66291139, - 0.66714676, 0.67095594, 0.67367984, 0.6765003, 0.67847961, - 0.68212344, 0.68345356, 0.68762778, 0.68876073, 0.69070183, - 0.69085143, 0.68988665, 0.68753177, 0.68348884, 0.68051081, - 0.67220053, 0.66506824, 0.66177969, 0.65712162, 0.63916951, - 0.6351146, 0.62025347, 0.60741567, 0.59618923, 0.58521935, - 0.57122582, 0.55532475, 0.54636839, 0.54422542, 0.53357655, - 0.53037033]) - - v = bearing_to_points(points[20], points) - assert_array_almost_equal(v, expected) - - def test_distance_to_points(self): - points = mockNodesData01.radians - expected = np.array([ - 1226.82569068, 1120.13820773, 1073.61121415, 1011.10016574, - 954.81557436, 905.58045038, 896.97734399, 781.7102819, - 738.58271117, 618.26145463, 509.47052142, 494.6403804, - 416.22483123, 403.42108699, 376.42615499, 357.15106681, - 253.15957483, 235.11572972, 221.77439728, 45.65465979, - 0., 14.98414, 28.77606056, 43.49299446, - 74.39463425, 112.74005248, 150.19482607, 167.03665191, - 178.28443483, 193.80834084, 202.28154097, 205.01173833, - 211.22777104, 282.88676739, 344.25957352, 362.66370657, - 367.00206795, 379.23951996, 394.82505328, 486.76073331, - 757.70254732, 960.03439155, 1023.81434529, 1042.49401713, - 1068.53770096, 1109.12696535, 1162.74555108, 1252.847351, - 1385.17179405, 1475.42502599, 1517.57849916, 1549.79838056, - 1580.12405964, 1605.05483058, 1622.98937809, 1657.19268821, - 1669.99157205, 1711.63883132, 1723.09133393, 1736.47655688, - 1746.16073119, 1754.63481838, 1763.34186103, 1772.62691273, - 1777.76189094, 1790.62024447, 1802.11488235, 1807.1040605, - 1813.90756815, 1834.49265566, 1840.00708445, 1861.96087374, - 1880.81678093, 1902.42091191, 1926.37194131, 1963.78301115, - 2011.62679077, 2046.18028824, 2054.37811294, 2097.30347724, - 2111.28586072]) - - v = distance_to_points(points[20], points) - assert_array_almost_equal(v, expected) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index a9e8d7f20d..d2949df630 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -8,7 +8,6 @@ #include #include -#include #include "common/timing.h" #include "selfdrive/ui/qt/util.h" @@ -74,7 +73,7 @@ void OnroadWindow::updateState(const UIState &s) { } QColor bgColor = bg_colors[s.status]; - Alert alert = Alert::get(*(s.sm), s.scene.started_frame, s.scene.display_debug_alert_frame); + Alert alert = Alert::get(*(s.sm), s.scene.started_frame); alerts->updateAlert(alert); if (s.scene.map_on_left) { @@ -93,69 +92,6 @@ void OnroadWindow::updateState(const UIState &s) { } -void issue_debug_snapshot(SubMaster &sm) { - auto longitudinal_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); - auto live_map_data = sm["liveMapDataSP"].getLiveMapDataSP(); - auto car_state = sm["carState"].getCarState(); - - auto t = std::time(nullptr); - auto tm = *std::localtime(&t); - std::ostringstream param_name_os; - param_name_os << std::put_time(&tm, "%Y-%m-%d--%H-%M-%S"); - - std::ostringstream os; - os.setf(std::ios_base::fixed); - os.precision(2); - os << "Datetime: " << param_name_os.str() << ", vEgo: " << car_state.getVEgo() * 3.6 << "\n\n"; - os.precision(6); - os << "Location: (" << live_map_data.getLastGpsLatitude() << ", " << live_map_data.getLastGpsLongitude() << ")\n"; - os.precision(2); - os << "Bearing: " << live_map_data.getLastGpsBearingDeg() << "; "; - os << "GPSSpeed: " << live_map_data.getLastGpsSpeed() * 3.6 << "\n\n"; - os.precision(1); - os << "Speed Limit: " << live_map_data.getSpeedLimit() * 3.6 << ", "; - os << "Valid: " << live_map_data.getSpeedLimitValid() << "\n"; - os << "Speed Limit Ahead: " << live_map_data.getSpeedLimitAhead() * 3.6 << ", "; - os << "Valid: " << live_map_data.getSpeedLimitAheadValid() << ", "; - os << "Distance: " << live_map_data.getSpeedLimitAheadDistance() << "\n"; - os << "Turn Speed Limit: " << live_map_data.getTurnSpeedLimit() * 3.6 << ", "; - os << "Valid: " << live_map_data.getTurnSpeedLimitValid() << ", "; - os << "End Distance: " << live_map_data.getTurnSpeedLimitEndDistance() << ", "; - os << "Sign: " << live_map_data.getTurnSpeedLimitSign() << "\n\n"; - - const auto turn_speeds = live_map_data.getTurnSpeedLimitsAhead(); - os << "Turn Speed Limits Ahead:\n"; - os << "VALUE\tDIST\tSIGN\n"; - - if (turn_speeds.size() == 0) { - os << "-\t-\t-" << "\n\n"; - } else { - const auto distances = live_map_data.getTurnSpeedLimitsAheadDistances(); - const auto signs = live_map_data.getTurnSpeedLimitsAheadSigns(); - for(int i = 0; i < turn_speeds.size(); i++) { - os << turn_speeds[i] * 3.6 << "\t" << distances[i] << "\t" << signs[i] << "\n"; - } - os << "\n"; - } - - os << "SPEED LIMIT CONTROLLER:\n"; - os << "sl: " << longitudinal_plan_sp.getSpeedLimit() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan_sp.getSpeedLimitControlState()) << ", "; - os << "isMap: " << longitudinal_plan_sp.getIsMapSpeedLimit() << "\n\n"; - - os << "TURN SPEED CONTROLLER:\n"; - os << "speed: " << longitudinal_plan_sp.getTurnSpeed() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan_sp.getTurnSpeedControlState()) << "\n\n"; - - os << "VISION TURN CONTROLLER:\n"; - os << "speed: " << longitudinal_plan_sp.getVisionTurnSpeed() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan_sp.getVisionTurnControllerState()); - - Params().put(param_name_os.str().c_str(), os.str().c_str(), os.str().length()); - uiState()->scene.display_debug_alert_frame = sm.frame; -} - - void OnroadWindow::mousePressEvent(QMouseEvent* e) { #ifdef ENABLE_MAPS if (map != nullptr) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 5b883c33e9..03df2cf57a 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -213,10 +213,6 @@ void ui_update_params(UIState *s) { auto params = Params(); s->scene.is_metric = params.getBool("IsMetric"); s->scene.map_on_left = params.getBool("NavSettingLeftSide"); - s->scene.speed_limit_control_enabled = params.getBool("SpeedLimitControl"); - s->scene.speed_limit_perc_offset = params.getBool("SpeedLimitPercOffset"); - s->scene.show_debug_ui = params.getBool("ShowDebugUI"); - s->scene.debug_snapshot_enabled = params.getBool("EnableDebugSnapshot"); } void UIState::updateStatus() { @@ -245,7 +241,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "longitudinalPlanSP", "liveMapDataSP", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index df03c1e33e..2ba0775676 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -48,17 +48,12 @@ struct Alert { return text1 == a2.text1 && text2 == a2.text2 && type == a2.type && sound == a2.sound; } - static Alert get(const SubMaster &sm, uint64_t started_frame, uint64_t display_debug_alert_frame = 0) { + static Alert get(const SubMaster &sm, uint64_t started_frame) { const cereal::ControlsState::Reader &cs = sm["controlsState"].getControlsState(); const uint64_t controls_frame = sm.rcv_frame("controlsState"); Alert alert = {}; - if (display_debug_alert_frame > 0 && (sm.frame - display_debug_alert_frame) <= 1 * UI_FREQ) { - return {"Debug snapshot collected", "", - "debugTapDetected", cereal::ControlsState::AlertSize::SMALL, - cereal::ControlsState::AlertStatus::NORMAL, - AudibleAlert::WARNING_SOFT}; - } else if (controls_frame >= started_frame) { // Don't get old alert. + if (controls_frame >= started_frame) { // Don't get old alert. alert = {cs.getAlertText1().cStr(), cs.getAlertText2().cStr(), cs.getAlertType().cStr(), cs.getAlertSize(), cs.getAlertStatus(), @@ -122,13 +117,6 @@ static std::map alert_colors = { {cereal::ControlsState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, }; -const QColor tcs_colors [] = { - [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::DISABLED)] = QColor(0x0, 0x0, 0x0, 0xff), - [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::ENTERING)] = QColor(0xC9, 0x22, 0x31, 0xf1), - [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::TURNING)] = QColor(0xDA, 0x6F, 0x25, 0xf1), - [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::LEAVING)] = QColor(0x17, 0x86, 0x44, 0xf1), -}; - typedef struct UIScene { bool calibration_valid = false; bool calibration_wide_valid = false; @@ -137,16 +125,6 @@ typedef struct UIScene { mat3 view_from_wide_calib = DEFAULT_CALIBRATION; cereal::PandaState::PandaType pandaType; - // Debug UI - bool show_debug_ui; - bool debug_snapshot_enabled; - uint64_t display_debug_alert_frame; - - // Speed limit control - bool speed_limit_control_enabled; - bool speed_limit_perc_offset; - double last_speed_limit_sign_tap; - // modelV2 float lane_line_probs[4]; float road_edge_stds[2];