def pull_features(self, rec: SceneRecord, roadway: Roadway, veh_idx: int,
models: {}, pastframe: int = 0):
idx = 0
self.features[idx] = convert_2_float(get_Acc(rec, roadway, veh_idx, pastframe))
idx += 1
self.features[idx] = convert_2_float(get_Jerk(rec, roadway, veh_idx, pastframe))
idx += 1
self.features[idx] = convert_2_float(get_TurnRateG(rec, roadway, veh_idx, pastframe))
idx += 1
self.features[idx] = convert_2_float(get_AngularRateG(rec, roadway, veh_idx, pastframe))
idx += 1
self.features[idx] = convert_2_float(get_TurnRateF(rec, roadway, veh_idx, pastframe))
idx += 1
self.features[idx] = convert_2_float(get_AngularRateF(rec, roadway, veh_idx, pastframe))
# timegap is the time between when this vehicle's front bumper
# will be in the position currently occupied by the vehicle
# infront's back bumper
timegap_censor_hi = 30.
neighborfore = get_neighbor_fore_along_lane_3(rec[pastframe], veh_idx, roadway)
timegap = get_TimeGap(rec, roadway, veh_idx, pastframe, neighborfore=neighborfore, censor_hi=timegap_censor_hi)
if timegap.v > timegap_censor_hi:
timegap = FeatureState.FeatureValue(timegap_censor_hi, timegap.i)
self.features = set_dual_feature(self.features, idx, timegap, censor=timegap_censor_hi)
idx += 2
# inverse time to collision is the time until a collision
# assuming that no actions are taken
# inverse is taken so as to avoid infinite value, so flip here to get back
# to TTC
neighborfore = get_neighbor_fore_along_lane_3(rec[pastframe], veh_idx, roadway)
inv_ttc = get_Inv_TTC(rec, roadway, veh_idx, pastframe, neighborfore=neighborfore)
ttc = FeatureState.inverse_ttc_to_ttc(inv_ttc, censor_hi=30.0)
self.features = set_dual_feature(self.features, idx, ttc, censor=30.0)
return self.features
def get_Inv_TTC(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0,
neighborfore: NeighborLongitudinalResult = None,
censor_hi: float = 10.0):
if neighborfore.ind is None:
return FeatureState.FeatureValue(0.0, FeatureState.MISSING)
else:
scene = rec[pastframe]
veh_fore = scene[neighborfore.ind]
veh_rear = scene[vehicle_index]
len_ego = veh_fore.definition.length_
len_oth = veh_rear.definition.length_
delta_s = neighborfore.delta_s - len_ego / 2 - len_oth / 2
delta_v = veh_fore.state.v - veh_rear.state.v
if delta_s < 0.0: # collision!
return FeatureState.FeatureValue(censor_hi,
FeatureState.CENSORED_HI)
elif delta_v > 0.0: # front car is pulling away
return FeatureState.FeatureValue(0.0)
else:
f = -delta_v / delta_s
if f > censor_hi:
return FeatureState.FeatureValue(f, FeatureState.CENSORED_HI)
else:
return FeatureState.FeatureValue(f)
def _get_feature_derivative_backwards(f: str,
rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0,
frames_back: int = 1):
id = rec[pastframe][vehicle_index].id
retval = FeatureState.FeatureValue(0.0, FeatureState.INSUF_HIST)
pastframe2 = pastframe - frames_back
if pastframe_inbounds(rec, pastframe) and pastframe_inbounds(
rec, pastframe2):
veh_index_curr = vehicle_index
veh_index_prev = rec[pastframe2].findfirst(id)
if veh_index_prev is not None:
curr = None
past = None
if f == "VELFS":
curr = convert_2_float(
Get.get_VelFs(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
Get.get_VelFs(rec, roadway, veh_index_prev, pastframe2))
elif f == "SPEED":
curr = convert_2_float(
Get.get_Speed(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
Get.get_Speed(rec, roadway, veh_index_prev, pastframe2))
elif f == "ACC":
curr = convert_2_float(
get_Acc(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
get_Acc(rec, roadway, veh_index_prev, pastframe2))
elif f == "TURNRATEG":
curr = convert_2_float(
Get.get_TurnRateG(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
Get.get_TurnRateG(rec, roadway, veh_index_prev,
pastframe2))
elif f == "POSFYAW":
curr = convert_2_float(
Get.get_PosFyaw(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
Get.get_PosFyaw(rec, roadway, veh_index_prev, pastframe2))
elif f == "TURNRATEF":
curr = convert_2_float(
Get.get_TurnRateF(rec, roadway, veh_index_curr, pastframe))
past = convert_2_float(
Get.get_TurnRateF(rec, roadway, veh_index_prev,
pastframe2))
else:
raise ValueError("No matching feature as {}".format(f))
delta_t = get_elapsed_time_3(rec, pastframe2, pastframe)
retval = FeatureState.FeatureValue((curr - past) / delta_t)
return retval
def get_LaneCurvature(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0):
veh = rec[pastframe][vehicle_index]
curvept = roadway.get_by_roadindex(veh.state.posF.roadind)
val = curvept.k
if val is None:
return FeatureState.FeatureValue(0.0, FeatureState.MISSING)
else:
return FeatureState.FeatureValue(val)
def get_VelFt(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0):
veh = rec[pastframe][vehicle_index]
return FeatureState.FeatureValue(veh.state.v *
math.sin(veh.state.posF.phi))
def get_Is_Colliding(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0,
mem: CPAMemory = CPAMemory()):
scene = rec[pastframe]
is_colliding = float(
get_first_collision(scene, vehicle_index, mem).is_colliding)
return FeatureState.FeatureValue(is_colliding)
def get_TimeGap(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0,
neighborfore: NeighborLongitudinalResult = None,
censor_hi: float = 10.0):
v = rec[pastframe][vehicle_index].state.v
if v <= 0.0 or neighborfore.ind is None:
return FeatureState.FeatureValue(censor_hi, FeatureState.CENSORED_HI)
else:
scene = rec[pastframe]
len_ego = scene[vehicle_index].definition.length_
len_oth = scene[neighborfore.ind].definition.length_
delta_s = neighborfore.delta_s - len_ego / 2 - len_oth / 2
if delta_s > 0:
return FeatureState.FeatureValue(delta_s / v)
else:
return FeatureState.FeatureValue(0.0) # collision!
def get_TurnRateG(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0,
frames_back: int = 1):
id = rec[pastframe][vehicle_index].id
retval = FeatureState.FeatureValue(0.0, FeatureState.INSUF_HIST)
pastframe2 = pastframe - frames_back
if pastframe_inbounds(rec, pastframe2):
veh_index_curr = vehicle_index
veh_index_prev = rec[pastframe2].findfirst(id)
if veh_index_prev is not None:
curr = rec[pastframe][veh_index_curr].state.posG.theta
past = rec[pastframe2][veh_index_prev].state.posG.theta
delta_t = get_elapsed_time_3(rec, pastframe2, pastframe)
retval = FeatureState.FeatureValue(
deltaangle(past, curr) / delta_t)
return retval
def get_RoadEdgeDist_Right(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0):
veh = rec[pastframe][vehicle_index]
offset = veh.state.posF.t
footpoint = veh.get_footpoint
seg = roadway.get_by_id(veh.state.posF.roadind.tag.segment)
lane = seg.lanes[0]
roadproj = proj_1(footpoint, lane, roadway)
curvept = roadway.get_by_roadindex(
RoadIndex(roadproj.curveproj.ind, roadproj.tag))
lane = roadway.get_by_tag(roadproj.tag)
vec = curvept.pos - footpoint
return FeatureState.FeatureValue(lane.width / 2 +
VecE2.norm(VecE2.VecE2(vec.x, vec.y)) +
offset)
def get_PosFyaw(rec: SceneRecord,
roadway: Roadway,
vehicle_index: int,
pastframe: int = 0):
return FeatureState.FeatureValue(
rec[pastframe][vehicle_index].state.posF.phi)