mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-29 23:43:47 +08:00
Enhanced Speed Control: Remove legacy implementation (#271)
Remove outdated Enhanced Speed Control
This commit is contained in:
Jason Wen
+1
-1
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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.
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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 `<restriction-value> @ (<condition>)` 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]
|
||||
@@ -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, [])
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
@@ -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)
|
||||
@@ -8,7 +8,6 @@
|
||||
|
||||
#include <QDebug>
|
||||
#include <QMouseEvent>
|
||||
#include <iomanip>
|
||||
|
||||
#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) {
|
||||
|
||||
+1
-5
@@ -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<SubMaster, const std::initializer_list<const char *>>({
|
||||
"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;
|
||||
|
||||
+2
-24
@@ -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<cereal::ControlsState::AlertStatus, QColor> 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];
|
||||
|
||||
Reference in New Issue
Block a user