def get_by_ind_roadway(self, ind: CurvePt.CurveIndex, roadway):
# print(ind.i, len(self.curve))
if ind.i == -1:
pt_lo = prev_lane_point(self, roadway)
pt_hi = self.curve[0]
s_gap = VecE2.norm(VecE2.VecE2(pt_hi.pos - pt_lo.pos))
pt_lo = CurvePt.CurvePt(pt_lo.pos, -s_gap, pt_lo.k, pt_lo.kd)
return CurvePt.lerp(pt_lo, pt_hi, ind.t)
elif ind.i < len(self.curve) - 1:
return CurvePt.get_curve_list_by_index(self.curve, ind)
else:
pt_hi = next_lane_point(self, roadway)
pt_lo = self.curve[-1]
s_gap = VecE2.norm(VecE2.VecE2(pt_hi.pos - pt_lo.pos))
pt_hi = CurvePt.CurvePt(pt_hi.pos, pt_lo.s + s_gap, pt_hi.k,
pt_hi.kd)
return CurvePt.lerp(pt_lo, pt_hi, ind.t)
def proj(posG: VecSE2.VecSE2, curve: list):
"""
Vec.proj(posG::VecSE2, curve::list of CurvePt)
Return a CurveProjection obtained by projecting posG onto the curve
"""
ind = index_closest_to_point(curve, posG)
# if ind <= len(curve):
# print("project: ")
# print(ind, len(curve))
curveind = CurveIndex(-1, 0)
footpoint = VecSE2.VecSE2(0, 0, 0)
if 0 < ind < len(curve) - 1:
t_lo = get_lerp_time_2(curve[ind - 1], curve[ind], posG)
t_hi = get_lerp_time_2(curve[ind], curve[ind + 1], posG)
p_lo = VecSE2.lerp(curve[ind - 1].pos, curve[ind].pos, t_lo)
p_hi = VecSE2.lerp(curve[ind].pos, curve[ind + 1].pos, t_hi)
vec_lo = p_lo - posG
vec_hi = p_hi - posG
d_lo = VecE2.norm(VecE2.VecE2(vec_lo.x, vec_lo.y))
d_hi = VecE2.norm(VecE2.VecE2(vec_hi.x, vec_hi.y))
if d_lo < d_hi:
footpoint = p_lo
curveind = CurveIndex(ind - 1, t_lo)
else:
footpoint = p_hi
curveind = CurveIndex(ind, t_hi)
elif ind == 0:
t = get_lerp_time_2(curve[0], curve[1], posG)
footpoint = VecSE2.lerp(curve[0].pos, curve[1].pos, t)
curveind = CurveIndex(ind, t)
else: # ind == length(curve)
t = get_lerp_time_2(curve[-2], curve[-1], posG)
footpoint = VecSE2.lerp(curve[-2].pos, curve[-1].pos, t)
curveind = CurveIndex(ind - 1, t)
return get_curve_projection(posG, footpoint, curveind)
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 observe(lidar: LidarSensor, scene: Frame, roadway: Roadway, vehicle_index: int):
state_ego = scene[vehicle_index].state
egoid = scene[vehicle_index].id
ego_vel = VecE2.polar(state_ego.v, state_ego.posG.theta)
in_range_ids = set()
for i in range(scene.n):
veh = scene[i]
if veh.id != egoid:
a = state_ego.posG - veh.state.posG
distance = VecE2.norm(VecE2.VecE2(a.x, a.y))
# account for the length and width of the vehicle by considering
# the worst case where their maximum radius is aligned
distance = distance - math.hypot(veh.definition.length_ / 2., veh.definition.width_ / 2.)
if distance < lidar.max_range:
in_range_ids.add(veh.id)
# compute range and range_rate for each angle
for (i, angle) in enumerate(lidar.angles):
ray_angle = state_ego.posG.theta + angle
ray_vec = VecE2.polar(1.0, ray_angle)
ray = VecSE2.VecSE2(state_ego.posG.x, state_ego.posG.y, ray_angle)
range_ = lidar.max_range
range_rate = 0.0
for j in range(scene.n):
veh = scene[j]
# only consider the set of potentially in range vehicles
if veh.id in in_range_ids:
lidar.poly.set(to_oriented_bounding_box_2(lidar.poly, veh))
range2 = get_collision_time(ray, lidar.poly, 1.0) # TODO: continue finish here
if range2 and range2 < range_:
range_ = range2
relative_speed = VecE2.polar(veh.state.v, veh.state.posG.theta) - ego_vel
range_rate = VecE2.proj_(relative_speed, ray_vec)
lidar.ranges[i] = range_
lidar.range_rates[i] = range_rate
return lidar
def get_neighbor_fore_along_lane_1(scene: Frame,
roadway: Roadway,
tag_start: LaneTag,
s_base: float,
targetpoint_primary: str,
targetpoint_valid: str,
max_distance_fore: float = 250.0,
index_to_ignore: int = -1):
# targetpoint_primary: the reference point whose distance we want to minimize
# targetpoint_valid: the reference point, which if distance to is positive, we include the vehicle
best_ind = None
best_dist = max_distance_fore
tag_target = tag_start
dist_searched = 0.0
while dist_searched < max_distance_fore:
lane = roadway.get_by_tag(tag_target)
for i in range(scene.n):
veh = scene[i]
if i != index_to_ignore:
s_adjust = None
if veh.state.posF.roadind.tag == tag_target:
s_adjust = 0.0
elif is_between_segments_hi(veh.state.posF.roadind.ind, lane.curve) and \
is_in_entrances(roadway.get_by_tag(tag_target), veh.state.posF.roadind.tag):
distance_between_lanes = VecE2.norm(
VecE2.VecE2(
roadway.get_by_tag(tag_target).curve[0].pos -
roadway.get_by_tag(
veh.state.posF.roadind.tag).curve[-1].pos))
s_adjust = -(roadway.get_by_tag(
veh.state.posF.roadind.tag).curve[-1].s +
distance_between_lanes)
elif is_between_segments_lo(veh.state.posF.roadind.ind) and \
is_in_exits(roadway.get_by_tag(tag_target), veh.state.posF.roadind.tag):
distance_between_lanes = VecE2.norm(
VecE2.VecE2(
roadway.get_by_tag(tag_target).curve[-1].pos -
roadway.get_by_tag(
veh.state.posF.roadind.tag).curve[0].pos))
s_adjust = roadway.get_by_tag(
tag_target).curve[-1].s + distance_between_lanes
if s_adjust is not None:
s_valid = veh.state.posF.s + veh.get_targetpoint_delta(
targetpoint_valid) + s_adjust
dist_valid = s_valid - s_base + dist_searched
if dist_valid >= 0.0:
s_primary = veh.state.posF.s + veh.get_targetpoint_delta(
targetpoint_primary) + s_adjust
dist = s_primary - s_base + dist_searched
if dist < best_dist:
best_dist = dist
best_ind = i
if best_ind is not None:
break
if (not has_next(lane)) or (tag_target == tag_start
and dist_searched != 0.0):
# exit after visiting this lane a 2nd time
break
dist_searched += (lane.curve[-1].s - s_base)
s_base = -VecE2.norm(
VecE2.VecE2(lane.curve[-1].pos -
next_lane_point(lane, roadway).pos))
# negative distance between lanes
tag_target = next_lane(lane, roadway).tag
return NeighborLongitudinalResult(best_ind, best_dist)