def _setup_collector(self):
self.sm_collector = SubMaster(['liveTracks'])
self.log_auto_df = self.op_params.get('log_auto_df')
if not isinstance(self.log_auto_df, bool):
self.log_auto_df = False
self.data_collector = DataCollector(file_path='/data/df_data',
keys=[
'v_ego', 'a_ego', 'a_lead',
'v_lead', 'x_lead', 'profile',
'time'
],
log_data=self.log_auto_df)
def __init__(self):
self.sm = SubMaster(['laneSpeed'])
self.pm = PubMaster(['dynamicCameraOffset'])
self.op_params = opParams()
self.camera_offset = self.op_params.get('camera_offset')
self.left_lane_oncoming = False # these variables change
self.right_lane_oncoming = False
self.last_left_lane_oncoming = False
self.last_right_lane_oncoming = False
self.last_oncoming_time = 0
self.i = 0.0
self._setup_static()
def comm_issue_alert(CP: car.CarParams, CS: car.CarState,
sm: messaging.SubMaster, metric: bool,
soft_disable_time: int) -> Alert:
bs = [s for s in sm.data.keys() if not sm.all_checks([
s,
])]
msg = ', '.join(bs[:4]) # can't fit too many on one line
return NoEntryAlert(msg,
alert_text_1="Communication Issue Between Processes")
def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState,
sm: messaging.SubMaster, metric: bool,
soft_disable_time: int) -> Alert:
all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
bad_cams = [
s for s in all_cams if s in sm.data.keys() and not sm.all_checks([
s,
])
]
return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams))
def main():
def get_influxdb_line(measurement: str, value: float, timestamp: datetime, tags: dict):
res = f"{measurement}"
for k, v in tags.items():
res += f",{k}={str(v)}"
res += f" value={value} {int(timestamp.timestamp() * 1e9)}\n"
return res
# open statistics socket
ctx = zmq.Context().instance()
sock = ctx.socket(zmq.PULL)
sock.bind(STATS_SOCKET)
# initialize stats directory
Path(STATS_DIR).mkdir(parents=True, exist_ok=True)
# initialize tags
tags = {
'dongleId': Params().get("DongleId", encoding='utf-8'),
'started': False,
'version': get_short_version(),
'branch': get_short_branch(),
'dirty': is_dirty(),
'origin': get_normalized_origin(),
'deviceType': HARDWARE.get_device_type(),
}
# subscribe to deviceState for started state
sm = SubMaster(['deviceState'])
last_flush_time = time.monotonic()
gauges = {}
while True:
started_prev = sm['deviceState'].started
sm.update()
# Update metrics
while True:
try:
metric = sock.recv_string(zmq.NOBLOCK)
try:
metric_type = metric.split('|')[1]
metric_name = metric.split(':')[0]
metric_value = metric.split('|')[0].split(':')[1]
if metric_type == METRIC_TYPE.GAUGE:
gauges[metric_name] = metric_value
else:
cloudlog.event("unknown metric type", metric_type=metric_type)
except Exception:
cloudlog.event("malformed metric", metric=metric)
except zmq.error.Again:
break
# flush when started state changes or after FLUSH_TIME_S
if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
result = ""
current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
tags['started'] = sm['deviceState'].started
for gauge_key in gauges:
result += get_influxdb_line(f"gauge.{gauge_key}", gauges[gauge_key], current_time, tags)
# clear intermediate data
gauges = {}
last_flush_time = time.monotonic()
# check that we aren't filling up the drive
if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
if len(result) > 0:
stats_path = os.path.join(STATS_DIR, str(int(current_time.timestamp())))
with atomic_write_in_dir(stats_path) as f:
f.write(result)
else:
cloudlog.error("stats dir full")
def cputime_total(ct):
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
def cputime_busy(ct):
return ct.user + ct.nice + ct.system + ct.irq + ct.softirq
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--mem', action='store_true')
args = parser.parse_args()
sm = SubMaster(['thermal', 'procLog'])
last_temp = 0.0
last_mem = 0.0
total_times = [0., 0., 0., 0.]
busy_times = [0., 0., 0.0, 0.]
while True:
sm.update()
if sm.updated['thermal']:
t = sm['thermal']
last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10.
last_mem = t.memUsedPercent
if sm.updated['procLog']:
def main() -> NoReturn:
dongle_id = Params().get("DongleId", encoding='utf-8')
def get_influxdb_line(measurement: str, value: Union[float, Dict[str, float]], timestamp: datetime, tags: dict) -> str:
res = f"{measurement}"
for k, v in tags.items():
res += f",{k}={str(v)}"
res += " "
if isinstance(value, float):
value = {'value': value}
for k, v in value.items():
res += f"{k}={v},"
res += f"dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n"
return res
# open statistics socket
ctx = zmq.Context().instance()
sock = ctx.socket(zmq.PULL)
sock.bind(STATS_SOCKET)
# initialize stats directory
Path(STATS_DIR).mkdir(parents=True, exist_ok=True)
# initialize tags
tags = {
'started': False,
'version': get_short_version(),
'branch': get_short_branch(),
'dirty': is_dirty(),
'origin': get_normalized_origin(),
'deviceType': HARDWARE.get_device_type(),
}
# subscribe to deviceState for started state
sm = SubMaster(['deviceState'])
idx = 0
last_flush_time = time.monotonic()
gauges = {}
samples: Dict[str, List[float]] = defaultdict(list)
while True:
started_prev = sm['deviceState'].started
sm.update()
# Update metrics
while True:
try:
metric = sock.recv_string(zmq.NOBLOCK)
try:
metric_type = metric.split('|')[1]
metric_name = metric.split(':')[0]
metric_value = float(metric.split('|')[0].split(':')[1])
if metric_type == METRIC_TYPE.GAUGE:
gauges[metric_name] = metric_value
elif metric_type == METRIC_TYPE.SAMPLE:
samples[metric_name].append(metric_value)
else:
cloudlog.event("unknown metric type", metric_type=metric_type)
except Exception:
cloudlog.event("malformed metric", metric=metric)
except zmq.error.Again:
break
# flush when started state changes or after FLUSH_TIME_S
if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
result = ""
current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
tags['started'] = sm['deviceState'].started
for key, value in gauges.items():
result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
for key, values in samples.items():
values.sort()
sample_count = len(values)
sample_sum = sum(values)
stats = {
'count': sample_count,
'min': values[0],
'max': values[-1],
'mean': sample_sum / sample_count,
}
for percentile in [0.05, 0.5, 0.95]:
value = values[int(round(percentile * (sample_count - 1)))]
stats[f"p{int(percentile * 100)}"] = value
result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
# clear intermediate data
gauges.clear()
samples.clear()
last_flush_time = time.monotonic()
# check that we aren't filling up the drive
if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
if len(result) > 0:
stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}")
with atomic_write_in_dir(stats_path) as f:
f.write(result)
idx += 1
else:
cloudlog.error("stats dir full")
class DynamicCameraOffset:
def __init__(self):
self.sm = SubMaster(['laneSpeed'])
self.pm = PubMaster(['dynamicCameraOffset'])
self.op_params = opParams()
self.camera_offset = self.op_params.get('camera_offset')
self.left_lane_oncoming = False # these variables change
self.right_lane_oncoming = False
self.last_left_lane_oncoming = False
self.last_right_lane_oncoming = False
self.last_oncoming_time = 0
self.i = 0.0
self._setup_static()
def _setup_static(self): # these variables are static
self._enabled = self.op_params.get('dynamic_camera_offset')
self._min_enable_speed = 35 * CV.MPH_TO_MS
self._min_lane_width_certainty = 0.4
hug = 0.1 # how much to hug
self._center_ratio = 0.5
self._hug_left_ratio = self._center_ratio - hug
self._hug_right_ratio = self._center_ratio + hug
self._keep_offset_for = self.op_params.get(
'dynamic_camera_offset_time') # seconds after losing oncoming lane
self._ramp_angles = [0, 12.5, 25]
self._ramp_angle_mods = [1, 0.85,
0.1] # multiply offset by this based on angle
self._ramp_down_times = [
self._keep_offset_for, self._keep_offset_for + 2.
]
self._ramp_down_multipliers = [
1, 0
] # ramp down 1.5s after time has passed
self._poly_prob_speeds = [
0, 25 * CV.MPH_TO_MS, 35 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS
]
self._poly_probs = [0.2, 0.25, 0.45,
0.55] # we're good if only one line is above this
self._k_p = 1.5
_i_rate = 1 / 20
self._k_i = 1.2 * _i_rate
def update(self, v_ego, active, angle_steers, lane_width_estimate,
lane_width_certainty, polys, probs):
if self._enabled:
self.sm.update(0)
self.camera_offset = self.op_params.get(
'camera_offset') # update base offset from user
self.left_lane_oncoming = self.sm['laneSpeed'].leftLaneOncoming
self.right_lane_oncoming = self.sm['laneSpeed'].rightLaneOncoming
self.lane_width_estimate, self.lane_width_certainty = lane_width_estimate, lane_width_certainty
self.l_poly, self.r_poly = polys
self.l_prob, self.r_prob = probs
dynamic_offset = self._get_camera_offset(v_ego, active,
angle_steers)
self._send_state(
) # for alerts, before speed check so alerts don't get stuck on
if dynamic_offset is not None:
return self.camera_offset + dynamic_offset
self.i = 0 # reset when not active
return self.camera_offset # don't offset if no lane line in direction we're going to hug
def _get_camera_offset(self, v_ego, active, angle_steers):
self.keeping_left, self.keeping_right = False, False # reset keeping
time_since_oncoming = sec_since_boot() - self.last_oncoming_time
if not active: # no alert when not engaged
return
if np.isnan(self.l_poly[3]) or np.isnan(self.r_poly[3]):
return
if v_ego < self._min_enable_speed:
return
_min_poly_prob = interp(v_ego, self._poly_prob_speeds,
self._poly_probs)
if self.l_prob < _min_poly_prob and self.r_prob < _min_poly_prob: # we only need one line and an accurate current lane width
return
left_lane_oncoming = self.left_lane_oncoming
right_lane_oncoming = self.right_lane_oncoming
if self.have_oncoming:
if self.lane_width_certainty < self._min_lane_width_certainty:
return
self.last_oncoming_time = sec_since_boot()
self.last_left_lane_oncoming = self.left_lane_oncoming # only update last oncoming vars when currently have oncoming. one should always be True for the 2 second ramp down
self.last_right_lane_oncoming = self.right_lane_oncoming
elif time_since_oncoming > self._keep_offset_for: # return if it's 2+ seconds after last oncoming, no need to offset
return
else: # no oncoming and not yet 2 seconds after we lost an oncoming lane. use last oncoming lane for 2 seconds to ramp down offset
left_lane_oncoming = self.last_left_lane_oncoming
right_lane_oncoming = self.last_right_lane_oncoming
estimated_lane_position = self._get_camera_position()
hug_modifier = interp(
abs(angle_steers), self._ramp_angles,
self._ramp_angle_mods) # don't offset as much when angle is high
if left_lane_oncoming:
self.keeping_right = True
hug_ratio = (self._hug_right_ratio * hug_modifier) + (
self._center_ratio * (1 - hug_modifier)) # weighted average
elif right_lane_oncoming:
self.keeping_left = True
hug_ratio = (self._hug_left_ratio *
hug_modifier) + (self._center_ratio *
(1 - hug_modifier))
else:
raise Exception('Error, no lane is oncoming but we\'re here!')
error = estimated_lane_position - hug_ratio
self.i += error * self._k_i # PI controller
offset = self.i + error * self._k_p
if time_since_oncoming <= self._keep_offset_for and not self.have_oncoming: # not yet 3 seconds after last oncoming, ramp down from 1.5 second
offset *= interp(time_since_oncoming, self._ramp_down_times,
self._ramp_down_multipliers) # ramp down offset
return offset
def _send_state(self):
dco_send = messaging.new_message('dynamicCameraOffset')
dco_send.dynamicCameraOffset.keepingLeft = self.keeping_left
dco_send.dynamicCameraOffset.keepingRight = self.keeping_right
self.pm.send('dynamicCameraOffset', dco_send)
@property
def have_oncoming(self):
return self.left_lane_oncoming != self.right_lane_oncoming # only one lane oncoming
def _get_camera_position(self):
"""
Returns the position of the camera in the lane as a percentage. left to right: [0, 1]; 0.5 is centered
You MUST verify that either left or right polys and lane width are accurate before calling this function.
"""
left_line_pos = self.l_poly[
3] + self.camera_offset # polys have not been offset yet
right_line_pos = self.r_poly[3] + self.camera_offset
cam_pos_left = left_line_pos / self.lane_width_estimate # estimated position of car in lane based on left line
cam_pos_right = 1 - abs(
right_line_pos
) / self.lane_width_estimate # estimated position of car in lane based on right line
# find car's camera position using weighted average of lane poly certainty
# if certainty of both lines are high, then just average ~equally
l_prob = self.l_prob / (self.l_prob + self.r_prob
) # this and next line sums to 1
r_prob = self.r_prob / (self.l_prob + self.r_prob)
# be biased towards position found from most probable lane line
return cam_pos_left * l_prob + cam_pos_right * r_prob
class DynamicFollow:
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.op_params = opParams()
self.df_profiles = dfProfiles()
self.df_manager = dfManager(self.op_params)
if not travis and mpc_id == 1:
self.pm = messaging.PubMaster(['dynamicFollowData'])
else:
self.pm = None
# Model variables
mpc_rate = 1 / 20.
self.model_scales = {
'v_ego': [-0.06112159043550491, 37.96522521972656],
'a_lead': [-3.109330892562866, 3.3612186908721924],
'v_lead': [0.0, 35.27671432495117],
'x_lead': [2.4600000381469727, 141.44000244140625]
}
self.predict_rate = 1 / 4.
self.skip_every = round(0.25 / mpc_rate)
self.model_input_len = round(45 / mpc_rate)
# Dynamic follow variables
self.default_TR = 1.8
self.TR = 1.8
self.v_ego_retention = 2.5
self.v_rel_retention = 1.75
self.sng_TR = 1.8 # reacceleration stop and go TR
self.sng_speed = 18.0 * CV.MPH_TO_MS
self._setup_collector()
self._setup_changing_variables()
def _setup_collector(self):
self.sm_collector = SubMaster(['liveTracks', 'laneSpeed'])
self.log_auto_df = self.op_params.get('log_auto_df')
if not isinstance(self.log_auto_df, bool):
self.log_auto_df = False
self.data_collector = DataCollector(
file_path='/data/df_data',
keys=[
'v_ego', 'a_ego', 'a_lead', 'v_lead', 'x_lead',
'left_lane_speeds', 'middle_lane_speeds', 'right_lane_speeds',
'left_lane_distances', 'middle_lane_distances',
'right_lane_distances', 'profile', 'time'
],
log_data=self.log_auto_df)
def _setup_changing_variables(self):
self.TR = self.default_TR
self.user_profile = self.df_profiles.relaxed # just a starting point
self.model_profile = self.df_profiles.relaxed
self.last_effective_profile = self.user_profile
self.profile_change_time = 0
self.sng = False
self.car_data = CarData()
self.lead_data = LeadData()
self.df_data = dfData() # dynamic follow data
self.last_cost = 0.0
self.last_predict_time = 0.0
self.auto_df_model_data = []
self._get_live_params() # so they're defined just in case
def update(self, CS, libmpc):
self._get_live_params()
self._update_car(CS)
self._get_profiles()
if self.mpc_id == 1 and self.log_auto_df:
self._gather_data()
if not self.lead_data.status:
self.TR = self.default_TR
else:
self._store_df_data()
self.TR = self._get_TR()
if not travis:
self._change_cost(libmpc)
self._send_cur_state()
return self.TR
def _get_profiles(self):
"""This receives profile change updates from dfManager and runs the auto-df prediction if auto mode"""
df_out = self.df_manager.update()
self.user_profile = df_out.user_profile
if df_out.is_auto: # todo: find some way to share prediction between the two mpcs to reduce processing overhead
self._get_pred(
) # sets self.model_profile, all other checks are inside function
def _gather_data(self):
self.sm_collector.update(0)
# live_tracks = [[i.dRel, i.vRel, i.aRel, i.yRel] for i in self.sm_collector['liveTracks']]
if self.car_data.cruise_enabled:
self.data_collector.update([
self.car_data.v_ego, self.car_data.a_ego,
self.lead_data.a_lead, self.lead_data.v_lead,
self.lead_data.x_lead,
list(self.sm_collector['laneSpeed'].leftLaneSpeeds),
list(self.sm_collector['laneSpeed'].middleLaneSpeeds),
list(self.sm_collector['laneSpeed'].rightLaneSpeeds),
list(self.sm_collector['laneSpeed'].leftLaneDistances),
list(self.sm_collector['laneSpeed'].middleLaneDistances),
list(self.sm_collector['laneSpeed'].rightLaneDistances),
self.user_profile,
sec_since_boot()
])
def _norm(self, x, name):
self.x = x
return interp(x, self.model_scales[name], [0, 1])
def _send_cur_state(self):
if self.mpc_id == 1 and self.pm is not None:
dat = messaging.new_message()
dat.init('dynamicFollowData')
dat.dynamicFollowData.mpcTR = self.TR
dat.dynamicFollowData.profilePred = self.model_profile
self.pm.send('dynamicFollowData', dat)
def _change_cost(self, libmpc):
TRs = [0.9, 1.8, 2.7]
costs = [1.15, 0.15, 0.05]
cost = interp(self.TR, TRs, costs)
change_time = sec_since_boot() - self.profile_change_time
change_time_x = [
0, 0.5, 4
] # for three seconds after effective profile has changed
change_mod_y = [
3, 6, 1
] # multiply cost by multiplier to quickly change distance
if change_time < change_time_x[
-1]: # if profile changed in last 3 seconds
cost_mod = interp(change_time, change_time_x, change_mod_y)
cost_mod_speeds = [0, 20 * CV.MPH_TO_MS
] # don't change cost too much under 20 mph
cost_mods = [cost_mod * 0.01, cost_mod]
cost *= interp(cost_mod, cost_mod_speeds, cost_mods)
if self.last_cost != cost:
libmpc.change_costs(
MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK
) # todo: jerk is the derivative of acceleration, could tune that
self.last_cost = cost
def _store_df_data(self):
cur_time = sec_since_boot()
# Store custom relative accel over time
if self.lead_data.status:
if self.lead_data.new_lead:
self.df_data.v_rels = [] # reset when new lead
else:
self.df_data.v_rels = self._remove_old_entries(
self.df_data.v_rels, cur_time, self.v_rel_retention)
self.df_data.v_rels.append({
'v_ego': self.car_data.v_ego,
'v_lead': self.lead_data.v_lead,
'time': cur_time
})
# Store our velocity for better sng
self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos,
cur_time,
self.v_ego_retention)
self.df_data.v_egos.append({
'v_ego': self.car_data.v_ego,
'time': cur_time
})
# Store data for auto-df model
self.auto_df_model_data.append([
self._norm(self.car_data.v_ego, 'v_ego'),
self._norm(self.lead_data.v_lead, 'v_lead'),
self._norm(self.lead_data.a_lead, 'a_lead'),
self._norm(self.lead_data.x_lead, 'x_lead')
])
while len(self.auto_df_model_data) > self.model_input_len:
del self.auto_df_model_data[0]
def _get_pred(self):
cur_time = sec_since_boot()
if self.car_data.cruise_enabled and self.lead_data.status:
if cur_time - self.last_predict_time > self.predict_rate:
if len(self.auto_df_model_data) == self.model_input_len:
pred = predict(
np.array(self.auto_df_model_data[::self.skip_every],
dtype=np.float32).flatten())
self.last_predict_time = cur_time
self.model_profile = int(np.argmax(pred))
def _remove_old_entries(self, lst, cur_time, retention):
return [
sample for sample in lst if cur_time - sample['time'] <= retention
]
def _relative_accel_mod(self):
"""
Returns relative acceleration mod calculated from list of lead and ego velocities over time (longer than 1s)
If min_consider_time has not been reached, uses lead accel and ego accel from openpilot (kalman filtered)
"""
a_ego = self.car_data.a_ego
a_lead = self.lead_data.a_lead
min_consider_time = 0.75 # minimum amount of time required to consider calculation
if len(self.df_data.v_rels) > 0: # if not empty
elapsed_time = self.df_data.v_rels[-1][
'time'] - self.df_data.v_rels[0]['time']
if elapsed_time > min_consider_time:
a_ego = (self.df_data.v_rels[-1]['v_ego'] -
self.df_data.v_rels[0]['v_ego']) / elapsed_time
a_lead = (self.df_data.v_rels[-1]['v_lead'] -
self.df_data.v_rels[0]['v_lead']) / elapsed_time
mods_x = [-1.5, -.75, 0]
mods_y = [1, 1.25, 1.3]
if a_lead < 0: # more weight to slight lead decel
a_lead *= interp(a_lead, mods_x, mods_y)
if a_lead - a_ego > 0: # return only if adding distance
return 0
rel_x = [
-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447,
0.8941, 1.7882, 2.6822
]
mod_y = [
0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045,
0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35
]
return interp(a_lead - a_ego, rel_x, mod_y)
def global_profile_mod(self, profile_mod_x, profile_mod_pos,
profile_mod_neg, x_vel, y_dist):
"""
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
"""
if self.global_df_mod == 1.:
return profile_mod_pos, profile_mod_neg, y_dist
global_df_mod = 1 - self.global_df_mod
# Calculate new TRs
speeds = [
0, self.sng_speed, 18, x_vel[-1]
] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
mods = [
0, 0.1, 0.7, 1
] # how much to limit global_df_mod at each speed, 1 is full effect
y_dist_new = [
y - (y * global_df_mod * interp(x, speeds, mods))
for x, y in zip(x_vel, y_dist)
]
# Calculate how to change profile mods based on change in TR
# eg. if df mod is 0.7, then increase positive mod and decrease negative mod
calc_profile_mods = [(interp(mod_x, x_vel, y_dist) -
interp(mod_x, x_vel, y_dist_new) + 1)
for mod_x in profile_mod_x]
profile_mod_pos = [
mod_pos * mod
for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)
]
profile_mod_neg = [
mod_neg * ((1 - mod) + 1)
for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)
]
return profile_mod_pos, profile_mod_neg, y_dist_new
def _get_TR(self):
x_vel = [
0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645,
22.352, 31.2928, 33.528, 35.7632, 40.2336
] # velocities
profile_mod_x = [
5 * CV.MPH_TO_MS, 30 * CV.MPH_TO_MS, 55 * CV.MPH_TO_MS,
80 * CV.MPH_TO_MS
] # profile mod speeds
if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this
df_profile = self.model_profile
else:
df_profile = self.user_profile
if df_profile != self.last_effective_profile:
self.profile_change_time = sec_since_boot()
self.last_effective_profile = df_profile
if df_profile == self.df_profiles.roadtrip:
y_dist = [
1.5486, 1.556, 1.5655, 1.5773, 1.5964, 1.6246, 1.6715, 1.7057,
1.7859, 1.8542, 1.8697, 1.8833, 1.8961
] # TRs
profile_mod_pos = [0.5, 0.35, 0.1, 0.03]
profile_mod_neg = [1.3, 1.4, 1.8, 2.0]
elif df_profile == self.df_profiles.traffic: # for in congested traffic
x_vel = [
0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275,
22.4049, 28.6752, 34.8858, 40.35
]
y_dist = [
1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257,
1.144, 0.979, 0.9461, 0.9156
]
profile_mod_pos = [1.075, 1.55, 2.6, 3.75]
profile_mod_neg = [0.95, .275, 0.1, 0.05]
elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock
y_dist = [
1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568,
1.588, 1.599, 1.613, 1.634
]
profile_mod_pos = [1.0, 0.955, 0.898, 0.905]
profile_mod_neg = [1.0, 1.0825, 1.1877, 1.174]
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(
profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
# Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_pos)
profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_neg)
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.sng_speed
or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego
) and not self.sng:
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
TR = interp(self.car_data.v_ego, x_vel, y_dist)
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
self.sng = True
x = [
self.sng_speed * 0.7, self.sng_speed
] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [
-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541,
-2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898,
2.7316, 4.4704
] # relative velocity values
y = [
.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914 * 1.025,
0.12269 * 1.05, 0.10483 * 1.075, 0.08074 * 1.15, 0.04886 * 1.25,
0.0072 * 1.075, 0.0, -0.05648, -0.0792, -0.15675, -0.23289, -0.315
] # modification values
TR_mods.append(
interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
x = [
-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466,
0.5144, 0.6903, 0.9302
] # lead acceleration values
y = [
0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042,
-0.053, -0.059
] # modification values
TR_mods.append(interp(self.lead_data.a_lead, x, y))
# deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# TR_mods.append(self._relative_accel_mod())
x = [self.sng_speed,
self.sng_speed / 5.0] # as we approach 0, apply x% more distance
y = [1.0, 1.05]
profile_mod_pos *= interp(self.car_data.v_ego, x,
y) # but only for currently positive mods
TR_mod = sum([
mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos
for mod in TR_mods
]) # alter TR modification according to profile
TR += TR_mod
if (self.car_data.left_blinker or self.car_data.right_blinker
) and df_profile != self.df_profiles.traffic:
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
y = [1.0, .75, .65]
TR *= interp(self.car_data.v_ego, x,
y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update_lead(self,
v_lead=None,
a_lead=None,
x_lead=None,
status=False,
new_lead=False):
self.lead_data.v_lead = v_lead
self.lead_data.a_lead = a_lead
self.lead_data.x_lead = x_lead
self.lead_data.status = status
self.lead_data.new_lead = new_lead
def _update_car(self, CS):
self.car_data.v_ego = CS.vEgo
self.car_data.a_ego = CS.aEgo
self.car_data.left_blinker = CS.leftBlinker
self.car_data.right_blinker = CS.rightBlinker
self.car_data.cruise_enabled = CS.cruiseState.enabled
def _get_live_params(self):
self.global_df_mod = self.op_params.get('global_df_mod')
if self.global_df_mod != 1.:
self.global_df_mod = clip(self.global_df_mod, 0.85, 1.2)
self.min_TR = self.op_params.get('min_TR')
if self.min_TR != 1.:
self.min_TR = clip(self.min_TR, 0.85, 1.6)
name = proc.name
if len(proc.cmdline):
name = proc.cmdline[0]
if len(proc.exe):
name = proc.exe + " - " + name
return name
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--mem', action='store_true')
parser.add_argument('--cpu', action='store_true')
args = parser.parse_args()
sm = SubMaster(['deviceState', 'procLog'])
last_temp = 0.0
last_mem = 0.0
total_times = [0.] * 8
busy_times = [0.] * 8
prev_proclog: Optional[capnp._DynamicStructReader] = None
prev_proclog_t: Optional[int] = None
while True:
sm.update()
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = mean(t.cpuTempC)
class DynamicFollow:
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.op_params = opParams()
self.df_profiles = dfProfiles()
self.df_manager = dfManager()
self.dmc_v_rel = DistanceModController(k_i=0.042,
k_d=0.08,
x_clip=[-1, 0, 0.66],
mods=[1.15, 1., 0.95])
self.dmc_a_rel = DistanceModController(
k_i=0.042 * 1.05,
k_d=0.08,
x_clip=[-1, 0, 0.33],
mods=[1.15, 1., 0.98]) # a_lead loop is 5% faster
if not travis and mpc_id == 1:
self.pm = messaging.PubMaster(['dynamicFollowData'])
else:
self.pm = None
# Model variables
mpc_rate = 1 / 20.
self.model_scales = {
'v_ego': [-0.06112159043550491, 37.96522521972656],
'a_lead': [-3.109330892562866, 3.3612186908721924],
'v_lead': [0.0, 35.27671432495117],
'x_lead': [2.4600000381469727, 141.44000244140625]
}
self.predict_rate = 1 / 4.
self.skip_every = round(0.25 / mpc_rate)
self.model_input_len = round(45 / mpc_rate)
# Dynamic follow variables
self.default_TR = 1.8
self.TR = 1.8
self.v_ego_retention = 2.5
self.v_rel_retention = 1.75
#self.sng_TR = 1.8 # reacceleration stop and go TR #Orig
#self.sng_TR = 2.3 # Last value
#self.sng_TR = 1.5 # Try to decrease stop and start TR so would start move faster from stop
self.sng_TR = 1. # Try to decrease stop and start TR so would start move faster from stop
self.sng_TR = 2.5 # Maybe I thought this wrong
self.sng_speed = 18.0 * CV.MPH_TO_MS #Orig
#self.sng_speed = 12.0 * CV.MPH_TO_MS
self._setup_collector()
self._setup_changing_variables()
def _setup_collector(self):
self.sm_collector = SubMaster(['liveTracks', 'laneSpeed'])
self.log_auto_df = self.op_params.get('log_auto_df')
if not isinstance(self.log_auto_df, bool):
self.log_auto_df = False
self.data_collector = DataCollector(
file_path='/data/df_data',
keys=[
'v_ego', 'a_ego', 'a_lead', 'v_lead', 'x_lead',
'left_lane_speeds', 'middle_lane_speeds', 'right_lane_speeds',
'left_lane_distances', 'middle_lane_distances',
'right_lane_distances', 'profile', 'time'
],
log_data=self.log_auto_df)
def _setup_changing_variables(self):
self.TR = self.default_TR
self.user_profile = self.df_profiles.relaxed # just a starting point
self.model_profile = self.df_profiles.relaxed
self.last_effective_profile = self.user_profile
self.profile_change_time = 0
self.sng = False
self.car_data = CarData()
self.lead_data = LeadData()
self.df_data = dfData() # dynamic follow data
self.last_cost = 0.0
self.last_predict_time = 0.0
self.auto_df_model_data = []
self._get_live_params() # so they're defined just in case
def update(self, CS, libmpc):
self._get_live_params()
self._update_car(CS)
self._get_profiles()
if self.mpc_id == 1 and self.log_auto_df:
self._gather_data()
if not self.lead_data.status:
self.TR = self.default_TR
else:
self._store_df_data()
self.TR = self._get_TR()
if not travis:
self._change_cost(libmpc)
self._send_cur_state()
return self.TR
def _get_profiles(self):
"""This receives profile change updates from dfManager and runs the auto-df prediction if auto mode"""
df_out = self.df_manager.update()
self.user_profile = df_out.user_profile
if df_out.is_auto: # todo: find some way to share prediction between the two mpcs to reduce processing overhead
self._get_pred(
) # sets self.model_profile, all other checks are inside function
def _gather_data(self):
self.sm_collector.update(0)
# live_tracks = [[i.dRel, i.vRel, i.aRel, i.yRel] for i in self.sm_collector['liveTracks']]
if self.car_data.cruise_enabled:
self.data_collector.update([
self.car_data.v_ego, self.car_data.a_ego,
self.lead_data.a_lead, self.lead_data.v_lead,
self.lead_data.x_lead,
list(self.sm_collector['laneSpeed'].leftLaneSpeeds),
list(self.sm_collector['laneSpeed'].middleLaneSpeeds),
list(self.sm_collector['laneSpeed'].rightLaneSpeeds),
list(self.sm_collector['laneSpeed'].leftLaneDistances),
list(self.sm_collector['laneSpeed'].middleLaneDistances),
list(self.sm_collector['laneSpeed'].rightLaneDistances),
self.user_profile,
sec_since_boot()
])
def _norm(self, x, name):
self.x = x
return interp(x, self.model_scales[name], [0, 1])
def _send_cur_state(self):
if self.mpc_id == 1 and self.pm is not None:
dat = messaging.new_message('dynamicFollowData')
dat.dynamicFollowData.mpcTR = self.TR
dat.dynamicFollowData.profilePred = self.model_profile
self.pm.send('dynamicFollowData', dat)
def _change_cost(self, libmpc):
TRs = [0.9, 1.8, 2.7]
#costs = [1., 0.1, 0.01] #Original values
#costs = [1., 0.5, 0.1] # Last values
costs = [1., 0.75, 0.5]
cost = interp(self.TR, TRs, costs)
# change_time = sec_since_boot() - self.profile_change_time
# change_time_x = [0, 0.5, 4] # for three seconds after effective profile has changed
# change_mod_y = [3, 6, 1] # multiply cost by multiplier to quickly change distance
# if change_time < change_time_x[-1]: # if profile changed in last 3 seconds
# cost_mod = interp(change_time, change_time_x, change_mod_y)
# cost_mod_speeds = [0, 20 * CV.MPH_TO_MS] # don't change cost too much under 20 mph
# cost_mods = [cost_mod * 0.01, cost_mod]
# cost *= interp(cost_mod, cost_mod_speeds, cost_mods)
if self.last_cost != cost:
libmpc.change_costs(
MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK
) # todo: jerk is the derivative of acceleration, could tune that
self.last_cost = cost
def _store_df_data(self):
cur_time = sec_since_boot()
# Store custom relative accel over time
if self.lead_data.status:
if self.lead_data.new_lead:
self.df_data.v_rels = [] # reset when new lead
else:
self.df_data.v_rels = self._remove_old_entries(
self.df_data.v_rels, cur_time, self.v_rel_retention)
self.df_data.v_rels.append({
'v_ego': self.car_data.v_ego,
'v_lead': self.lead_data.v_lead,
'time': cur_time
})
# Store our velocity for better sng
self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos,
cur_time,
self.v_ego_retention)
self.df_data.v_egos.append({
'v_ego': self.car_data.v_ego,
'time': cur_time
})
# Store data for auto-df model
self.auto_df_model_data.append([
self._norm(self.car_data.v_ego, 'v_ego'),
self._norm(self.lead_data.v_lead, 'v_lead'),
self._norm(self.lead_data.a_lead, 'a_lead'),
self._norm(self.lead_data.x_lead, 'x_lead')
])
while len(self.auto_df_model_data) > self.model_input_len:
del self.auto_df_model_data[0]
def _get_pred(self):
cur_time = sec_since_boot()
if self.car_data.cruise_enabled and self.lead_data.status:
if cur_time - self.last_predict_time > self.predict_rate:
if len(self.auto_df_model_data) == self.model_input_len:
pred = predict(
np.array(self.auto_df_model_data[::self.skip_every],
dtype=np.float32).flatten())
self.last_predict_time = cur_time
self.model_profile = int(np.argmax(pred))
@staticmethod
def _remove_old_entries(lst, cur_time, retention):
return [
sample for sample in lst if cur_time - sample['time'] <= retention
]
def _relative_accel_mod(self):
"""
Returns relative acceleration mod calculated from list of lead and ego velocities over time (longer than 1s)
If min_consider_time has not been reached, uses lead accel and ego accel from openpilot (kalman filtered)
"""
a_ego = self.car_data.a_ego
a_lead = self.lead_data.a_lead
min_consider_time = 0.75 # minimum amount of time required to consider calculation
if len(self.df_data.v_rels) > 0: # if not empty
elapsed_time = self.df_data.v_rels[-1][
'time'] - self.df_data.v_rels[0]['time']
if elapsed_time > min_consider_time:
a_ego = (self.df_data.v_rels[-1]['v_ego'] -
self.df_data.v_rels[0]['v_ego']) / elapsed_time
a_lead = (self.df_data.v_rels[-1]['v_lead'] -
self.df_data.v_rels[0]['v_lead']) / elapsed_time
mods_x = [-1.5, -.75, 0]
mods_y = [1, 1.25, 1.3]
if a_lead < 0: # more weight to slight lead decel
a_lead *= interp(a_lead, mods_x, mods_y)
if a_lead - a_ego > 0: # return only if adding distance
return 0
rel_x = [
-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447,
0.8941, 1.7882, 2.6822
]
mod_y = [
0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045,
0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35
]
return interp(a_lead - a_ego, rel_x, mod_y)
def global_profile_mod(self, x_vel, y_dist):
"""
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
"""
if self.global_df_mod == 1.:
return y_dist
global_df_mod = 1 - self.global_df_mod
# Calculate new TRs
speeds, mods = [0.], [1.] # if increasing don't limit
if self.global_df_mod < 1: # if reducing distance
speeds = [
0, self.sng_speed, 18, x_vel[-1]
] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
mods = [
0, 0.25, 0.75, 1
] # how much to limit global_df_mod at each speed, 1 is full effect
return [
y - (y * global_df_mod * interp(x, speeds, mods))
for x, y in zip(x_vel, y_dist)
]
def _get_TR(self):
if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this
df_profile = self.model_profile
else:
df_profile = self.user_profile
if df_profile != self.last_effective_profile:
self.profile_change_time = sec_since_boot()
self.last_effective_profile = df_profile
x_vel = [
0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645,
22.352, 31.2928, 33.528, 35.7632, 40.2336
] # velocities
if df_profile == self.df_profiles.stock:
return 1.8
elif df_profile == self.df_profiles.traffic: # for in congested traffic
x_vel = [
0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275,
22.4049, 28.6752, 34.8858, 40.35
]
y_dist = [
1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257,
1.144, 0.979, 0.9461, 0.9156
]
elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock
#y_dist = [1.411, 1.418, 1.428, 1.441, 1.461, 1.49, 1.535, 1.561, 1.589, 1.612, 1.621, 1.632, 1.648] # Original values
#y_dist = [1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.6, 1.612, 1.621, 1.632, 1.648]
#y_dist = [1.411, 1.418, 1.8, 1.9, 2.1, 2.0, 1.8, 1.6, 1.6, 1.612, 1.621, 1.632, 1.648]
#y_dist = [1.8, 1.8, 1.8, 1.9, 2.1, 2.0, 1.8, 1.6, 1.6, 1.55, 1.5, 1.45, 1.4] # Last used
#y_dist = [3., 2.6, 2.2, 2.1, 2.1, 2.0, 1.8, 1.6, 1.6, 1.55, 1.5, 1.45, 1.4]
y_dist = [
3., 2.6, 2.2, 2.1, 2.1, 2.0, 1.8, 1.7, 1.7, 1.65, 1.55, 1.45,
1.4
]
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
y_dist = self.global_profile_mod(x_vel, y_dist)
v_rel_dist_factor = self.dmc_v_rel.update(self.lead_data.v_lead -
self.car_data.v_ego)
a_lead_dist_factor = self.dmc_a_rel.update(self.lead_data.a_lead -
self.car_data.a_ego)
TR = interp(self.car_data.v_ego, x_vel, y_dist)
TR *= v_rel_dist_factor
TR *= a_lead_dist_factor
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.sng_speed
or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego
) and not self.sng:
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
TR = interp(self.car_data.v_ego, x_vel, y_dist)
#TR = TR
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
self.sng = True
x = [
self.sng_speed * 0.7, self.sng_speed
] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
#x = [self.sng_speed * 0.55, self.sng_speed]
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
return float(clip(TR, self.min_TR, 2.7))
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [
-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26,
-0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704
] # relative velocity values
y = [
1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0,
0.985, 0.958, 0.87, 0.81, 0.685
] # multiplier values
y = np.array(y) - 1 # converts back to original abs mod
y *= 1.1 # multiplier for how much to mod
y = y / TR + 1 # converts back to multipliers
TR_mods.append(
interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
x = [
-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466,
0.5144, 0.6903, 0.9302
] # lead acceleration values
y = [
1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972,
0.9647, 0.9607
] # multiplier values
converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract
absolute_y_TR_mod = np.array(
y
) * converted_with_TR - converted_with_TR # converts back to original abs mod
absolute_y_TR_mod *= 1.2 # multiplier for how much to mod
y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account
TR_mods.append(interp(self.get_rel_accel(), x,
y)) # todo: make this over more than 1 sec
# deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# TR_mods.append(self._relative_accel_mod())
# x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance
# y = [1.0, 1.05]
TR *= mean(
TR_mods
) # with mods as multipliers, profile mods shouldn't be needed
# if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic:
# x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
# y = [1.0, .75, .65]
# TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update_lead(self,
v_lead=None,
a_lead=None,
x_lead=None,
status=False,
new_lead=False):
self.lead_data.v_lead = v_lead
self.lead_data.a_lead = a_lead
self.lead_data.x_lead = x_lead
self.lead_data.status = status
self.lead_data.new_lead = new_lead
def _update_car(self, CS):
self.car_data.v_ego = CS.vEgo
self.car_data.a_ego = CS.aEgo
self.car_data.left_blinker = CS.leftBlinker
self.car_data.right_blinker = CS.rightBlinker
self.car_data.cruise_enabled = CS.cruiseState.enabled
def _get_live_params(self):
self.global_df_mod = self.op_params.get('global_df_mod')
if self.global_df_mod != 1.:
self.global_df_mod = clip(self.global_df_mod, 0.85, 2.5)
self.min_TR = self.op_params.get('min_TR')
if self.min_TR != 1.:
self.min_TR = clip(self.min_TR, 0.85, 2.7)
def _setup_collector(self):
self.sm = SubMaster(['liveTracks'])
class DynamicFollow:
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.df_profiles = dfProfiles()
# Model variables
mpc_rate = 1 / 20.
self.model_scales = {
'v_ego': [-0.06112159043550491, 37.96522521972656],
'a_lead': [-3.109330892562866, 3.3612186908721924],
'v_lead': [0.0, 35.27671432495117],
'x_lead': [2.4600000381469727, 141.44000244140625]
}
self.predict_rate = 1 / 4.
self.skip_every = round(0.25 / mpc_rate)
self.model_input_len = round(45 / mpc_rate)
# Dynamic follow variables
self.default_TR = 1.8
self.TR = 1.8
# self.v_lead_retention = 2.0 # keep only last x seconds
self.v_ego_retention = 2.5
self.v_rel_retention = 1.5
self.sng_TR = 1.8 # reacceleration stop and go TR
self.sng_speed = 18.0 * CV.MPH_TO_MS
# dp params
self.last_ts = 0.
self.dp_df_profile = PROFILE_OFF
self.dp_last_modified = None
self.params = Params()
self._setup_collector()
self._setup_changing_variables()
def _setup_collector(self):
self.sm = SubMaster(['liveTracks'])
def _setup_changing_variables(self):
self.TR = self.default_TR
self.model_profile = None
self.sng = False
self.car_data = CarData()
self.lead_data = LeadData()
self.df_data = dfData() # dynamic follow data
self.last_cost = 0.0
self.last_predict_time = 0.0
self.auto_df_model_data = []
self._get_live_params() # so they're defined just in case
def update(self, CS, libmpc):
self._get_live_params()
self._update_car(CS)
self._get_profiles()
if self.mpc_id == 1:
self._gather_data()
if not self.lead_data.status or self.dp_df_profile == PROFILE_OFF:
self.TR = self.default_TR
else:
self._store_df_data()
self.TR = self._get_TR()
if not travis:
self._change_cost(libmpc)
return self.TR
def _get_profiles(self):
# dp
# update() gets call every time so we can read profile from param here
# as usual, we update every 5 secs
ts = sec_since_boot()
if self.last_ts is None or ts - self.last_ts >= 5.:
modified = get_last_modified()
if self.dp_last_modified != modified:
try:
self.dp_df_profile = int(
self.params.get("DragonDynamicFollow",
encoding='utf8'))
if self.dp_df_profile > 4 or self.dp_df_profile < 0:
self.dp_df_profile = 0
except (TypeError, ValueError):
self.dp_df_profile = PROFILE_OFF
self.dp_last_modified = modified
self.last_ts = ts
if self.dp_df_profile == PROFILE_AUTO:
self._get_pred(
) # sets self.model_profile, all other checks are inside function
def _gather_data(self):
self.sm.update(0)
def _norm(self, x, name):
self.x = x
return np.interp(x, self.model_scales[name], [0, 1])
def _change_cost(self, libmpc):
TRs = [0.9, 1.8, 2.7]
costs = [1.0, 0.115, 0.05]
cost = interp(self.TR, TRs, costs)
if self.last_cost != cost:
libmpc.change_tr(MPC_COST_LONG.TTC, cost,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.last_cost = cost
def _store_df_data(self):
cur_time = sec_since_boot()
# Store custom relative accel over time
if self.lead_data.status:
if self.lead_data.new_lead:
self.df_data.v_rels = [] # reset when new lead
else:
self.df_data.v_rels = self._remove_old_entries(
self.df_data.v_rels, cur_time, self.v_rel_retention)
self.df_data.v_rels.append({
'v_ego': self.car_data.v_ego,
'v_lead': self.lead_data.v_lead,
'time': cur_time
})
# Store our velocity for better sng
self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos,
cur_time,
self.v_ego_retention)
self.df_data.v_egos.append({
'v_ego': self.car_data.v_ego,
'time': cur_time
})
# Store data for auto-df model
self.auto_df_model_data.append([
self._norm(self.car_data.v_ego, 'v_ego'),
self._norm(self.lead_data.v_lead, 'v_lead'),
self._norm(self.lead_data.a_lead, 'a_lead'),
self._norm(self.lead_data.x_lead, 'x_lead')
])
while len(self.auto_df_model_data) > self.model_input_len:
del self.auto_df_model_data[0]
def _get_pred(self):
cur_time = sec_since_boot()
if self.car_data.cruise_enabled and self.lead_data.status:
if cur_time - self.last_predict_time > self.predict_rate:
if len(self.auto_df_model_data) == self.model_input_len:
pred = predict(
np.array(self.auto_df_model_data[::self.skip_every],
dtype=np.float32).flatten())
self.last_predict_time = cur_time
self.model_profile = int(np.argmax(pred))
def _remove_old_entries(self, lst, cur_time, retention):
return [
sample for sample in lst if cur_time - sample['time'] <= retention
]
def _calculate_relative_accel_new(self):
# """
# Moving window returning the following: (final relative velocity - initial relative velocity) / dT with a few extra mods
# Output properties:
# When the lead is starting to decelerate, and our car remains the same speed, the output decreases (and vice versa)
# However when our car finally starts to decelerate at the same rate as the lead car, the output will move to near 0
# >>> a = [(15 - 18), (14 - 17)]
# >>> (a[-1] - a[0]) / 1
# > 0.0
# """
min_consider_time = 0.5 # minimum amount of time required to consider calculation
if len(self.df_data.v_rels) > 0: # if not empty
elapsed_time = self.df_data.v_rels[-1][
'time'] - self.df_data.v_rels[0]['time']
if elapsed_time > min_consider_time:
x = [
-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235,
0.447, 0.8941, 1.7882, 2.6822
]
y = [
0.3245, 0.277, 0.11075, 0.08106, 0.06325, 0.0, -0.09,
-0.09375, -0.125, -0.3, -0.35
]
v_lead_start = self.df_data.v_rels[0][
'v_lead'] # setup common variables
v_ego_start = self.df_data.v_rels[0]['v_ego']
v_lead_end = self.df_data.v_rels[-1]['v_lead']
v_ego_end = self.df_data.v_rels[-1]['v_ego']
v_ego_change = v_ego_end - v_ego_start
v_lead_change = v_lead_end - v_lead_start
if v_lead_change - v_ego_change == 0 or v_lead_change + v_ego_change == 0:
return None
initial_v_rel = v_lead_start - v_ego_start
cur_v_rel = v_lead_end - v_ego_end
delta_v_rel = (cur_v_rel - initial_v_rel) / elapsed_time
neg_pos = False
if v_ego_change == 0 or v_lead_change == 0: # FIXME: this all is a mess, but works. need to simplify
lead_factor = v_lead_change / (v_lead_change -
v_ego_change)
elif (v_ego_change < 0) != (
v_lead_change <
0): # one is negative and one is positive, or ^ = XOR
lead_factor = v_lead_change / (v_lead_change -
v_ego_change)
if v_ego_change > 0 > v_lead_change:
delta_v_rel = -delta_v_rel # switch when appropriate
neg_pos = True
elif v_ego_change * v_lead_change > 0: # both are negative or both are positive
lead_factor = v_lead_change / (v_lead_change +
v_ego_change)
if v_ego_change > 0 and v_lead_change > 0: # both are positive
if v_ego_change < v_lead_change:
delta_v_rel = -delta_v_rel # switch when appropriate
elif v_ego_change > v_lead_change: # both are negative and v_ego_change > v_lead_change
delta_v_rel = -delta_v_rel
else:
raise Exception(
'Uncovered case! Should be impossible to be be here')
if not neg_pos: # negative and positive require different mod code to be correct
rel_vel_mod = (-delta_v_rel *
abs(lead_factor)) + (delta_v_rel *
(1 - abs(lead_factor)))
else:
rel_vel_mod = math.copysign(delta_v_rel, v_lead_change -
v_ego_change) * lead_factor
calc_mod = np.interp(rel_vel_mod, x, y)
if v_lead_end > v_ego_end and calc_mod >= 0:
# if we're accelerating quicker than lead but lead is still faster, reduce mod
# todo: could remove this since we restrict this mod where called
x = np.array([0, 2, 4, 8]) * CV.MPH_TO_MS
y = [1.0, -0.25, -0.65, -0.95]
v_rel_mod = np.interp(v_lead_end - v_ego_end, x, y)
calc_mod *= v_rel_mod
return calc_mod
return None
def global_profile_mod(self, profile_mod_x, profile_mod_pos,
profile_mod_neg, x_vel, y_dist):
"""
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
"""
if self.global_df_mod is None:
return profile_mod_pos, profile_mod_neg, y_dist
global_df_mod = 1 - self.global_df_mod
# Calculate new TRs
speeds = [
0, self.sng_speed, 18, x_vel[-1]
] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
mods = [
0, 0.1, 0.7, 1
] # how much to limit global_df_mod at each speed, 1 is full effect
y_dist_new = [
y - (y * global_df_mod * np.interp(x, speeds, mods))
for x, y in zip(x_vel, y_dist)
]
# Calculate how to change profile mods based on change in TR
# eg. if df mod is 0.7, then increase positive mod and decrease negative mod
calc_profile_mods = [(np.interp(mod_x, x_vel, y_dist) -
np.interp(mod_x, x_vel, y_dist_new) + 1)
for mod_x in profile_mod_x]
profile_mod_pos = [
mod_pos * mod
for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)
]
profile_mod_neg = [
mod_neg * ((1 - mod) + 1)
for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)
]
return profile_mod_pos, profile_mod_neg, y_dist_new
def _get_TR(self):
x_vel = [
0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645,
22.352, 31.2928, 33.528, 35.7632, 40.2336
] # velocities
profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632
] # profile mod speeds, mph: [5., 30., 55., 80.]
if self.dp_df_profile == PROFILE_AUTO: # decide which profile to use, model profile will be updated before this
# df is 0 = traffic, 1 = relaxed, 2 = roadtrip, 3 = auto
# dp is 0 = off, 1 = short, 2 = normal, 3 = long, 4 = auto
# if it's model profile, we need to convert it
if self.model_profile is None:
# when its none, we use normal instead
df_profile = PROFILE_NORMAL
else:
df_profile = self.model_profile + 1
else:
df_profile = self.dp_df_profile
if df_profile == PROFILE_LONG:
y_dist = [
1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203,
1.7238, 1.8231, 1.8379, 1.8495, 1.8535
] # TRs
profile_mod_pos = [0.92, 0.7, 0.25, 0.15]
profile_mod_neg = [1.1, 1.3, 2.0, 2.3]
elif df_profile == PROFILE_SHORT: # for in congested traffic
x_vel = [
0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275,
22.4049, 28.6752, 34.8858, 40.35
]
# y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original
# y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph
y_dist = [
1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257,
1.144, 0.979, 0.9461, 0.9156
]
profile_mod_pos = [1.05, 1.55, 2.6, 3.75]
profile_mod_neg = [0.84, .275, 0.1, 0.05]
elif df_profile == PROFILE_NORMAL: # default to relaxed/stock
y_dist = [
1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546,
1.568, 1.579, 1.593, 1.614
]
profile_mod_pos = [1.0] * 4
profile_mod_neg = [1.0] * 4
else:
raise Exception('Unknown profile type: {}'.format(df_profile))
# Global df mod
profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(
profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
# Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_pos)
profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x,
profile_mod_neg)
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False
if (self.car_data.v_ego >= self.sng_speed
or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego
) and not self.sng:
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
TR = interp(self.car_data.v_ego, x_vel, y_dist)
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
self.sng = True
x = [
self.sng_speed * 0.7, self.sng_speed
] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [
-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541,
-2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898,
2.7316, 4.4704
] # relative velocity values
y = [
.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914, 0.12269, 0.10483,
0.08074, 0.04886, 0.0072, 0.0, -0.05648, -0.0792, -0.15675,
-0.23289, -0.315
] # modification values
TR_mods.append(
interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
x = [
-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466,
0.5144, 0.6903, 0.9302
] # lead acceleration values
y = [
0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042,
-0.053, -0.059
] # modification values
TR_mods.append(interp(self.lead_data.a_lead, x, y))
rel_accel_mod = self._calculate_relative_accel_new()
if rel_accel_mod is not None: # if available
deadzone = 2 * CV.MPH_TO_MS
if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
TR_mods.append(rel_accel_mod)
x = [self.sng_speed / 5.0,
self.sng_speed] # as we approach 0, apply x% more distance
y = [1.05, 1.0]
profile_mod_pos *= interp(self.car_data.v_ego, x,
y) # but only for currently positive mods
TR_mod = sum([
mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos
for mod in TR_mods
]) # alter TR modification according to profile
TR += TR_mod
if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic:
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
y = [1.0, .75, .65]
TR *= interp(self.car_data.v_ego, x,
y) # reduce TR when changing lanes
return float(clip(TR, self.min_TR, 2.7))
def update_lead(self,
v_lead=None,
a_lead=None,
x_lead=None,
status=False,
new_lead=False):
self.lead_data.v_lead = v_lead
self.lead_data.a_lead = a_lead
self.lead_data.x_lead = x_lead
self.lead_data.status = status
self.lead_data.new_lead = new_lead
def _update_car(self, CS):
self.car_data.v_ego = CS.vEgo
self.car_data.a_ego = CS.aEgo
self.car_data.left_blinker = CS.leftBlinker
self.car_data.right_blinker = CS.rightBlinker
self.car_data.cruise_enabled = CS.cruiseState.enabled
def _get_live_params(self):
self.global_df_mod = None #self.op_params.get('global_df_mod', None)
if self.global_df_mod is not None:
self.global_df_mod = clip(self.global_df_mod, 0.85, 1.2)
self.min_TR = 0.9 # default
from cereal.messaging import SubMaster
import time
sm = SubMaster(['dynamicFollowData'])
while True:
sm.update(0)
print('mpc_TR: {}'.format(sm['dynamicFollowData'].mpcTR))
time.sleep(1 / 20)
from cereal.messaging import SubMaster
sm = SubMaster(['modelV2'], poll=['modelV2'])
model_t = [
0, 0.009765625, 0.0390625, 0.087890625, 0.15625, 0.24414062, 0.3515625,
0.47851562, 0.625, 0.79101562, 0.9765625, 1.1816406, 1.40625, 1.6503906,
1.9140625, 2.1972656, 2.5, 2.8222656, 3.1640625, 3.5253906, 3.90625,
4.3066406, 4.7265625, 5.1660156, 5.625, 6.1035156, 6.6015625, 7.1191406,
7.65625, 8.2128906, 8.7890625, 9.3847656, 10
]
mpc_idxs = list(range(10))
model_t_idx = [
sorted(range(len(model_t)),
key=[abs(idx - t) for t in model_t].__getitem__)[0]
for idx in mpc_idxs
] # matches 0 to 9 interval to idx from t
# speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx
while 1:
sm.update()
modelV2 = sm['modelV2']
if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
continue
distances, speeds, accelerations = [], [], [
] # everything is derived from x position since velocity is outputting weird values
for t in model_t_idx:
speeds.append(modelV2.velocity.x[t])
distances.append(modelV2.position.x[t])