def read_boundaries(filepath_boundaries: str):
fp = open(filepath_boundaries, 'r')
lines = fp.readlines()
fp.close()
for i, line in enumerate(lines):
lines[i] = line.strip()
assert lines[0] == 'BOUNDARIES'
n_boundaries = int(lines[1])
assert n_boundaries >= 0
retval = [] # Array{Vector{VecE2}}
line_index = 1
for i in range(n_boundaries):
line_index += 1
assert lines[line_index] == "BOUNDARY {}".format(i + 1)
line_index += 1
npts = int(lines[line_index])
line = [] # Array{VecE2}
for j in range(npts):
line_index += 1
matches = re.findall(FLOATING_POINT_REGEX, lines[line_index])
x = float(matches[0]) * METERS_PER_FOOT
y = float(matches[1]) * METERS_PER_FOOT
line.append(VecE2.VecE2(x, y))
retval.append(line)
return retval
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 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 proj_2(posG: VecSE2.VecSE2, roadway: Roadway):
best_dist2 = math.inf
best_proj = RoadProjection(
CurvePt.CurveProjection(CurvePt.CurveIndex(-1, -1), None, None),
NULL_LANETAG)
for seg in roadway.segments:
for lane in seg.lanes:
roadproj = proj_1(posG, lane, roadway,
move_along_curves=False) # return RoadProjection
targetlane = roadway.get_by_tag(roadproj.tag) # return Lane
footpoint = targetlane.get_by_ind_roadway(roadproj.curveproj.ind,
roadway)
vec = posG - footpoint.pos
dist2 = VecE2.normsquared(VecE2.VecE2(vec.x, vec.y))
# print(best_dist2, dist2, roadproj.curveproj.ind.i)
if dist2 < best_dist2:
best_dist2 = dist2
best_proj = roadproj
return best_proj
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 to_oriented_bounding_box_1(retval: ConvexPolygon, center: VecSE2.VecSE2,
len: float, wid: float):
assert len > 0
assert wid > 0
assert center.theta is not None
assert center.x is not None
assert center.y is not None
x = VecE2.polar(len / 2, center.theta)
y = VecE2.polar(wid / 2, center.theta + math.pi / 2)
C = VecSE2.convert(center)
retval.pts[0] = x - y + C
retval.pts[1] = x + y + C
retval.pts[2] = -x + y + C
retval.pts[3] = -x - y + C
retval.npts = 4
retval.set(ensure_pts_sorted_by_min_polar_angle(retval))
return retval
def get_intersection_time(A: Projectile, seg: LineSegment):
o = VecE2.VecE2(A.pos.x, A.pos.y)
v_1 = o - seg.A
v_2 = seg.B - seg.A
v_3 = VecE2.polar(1.0, A.pos.theta + math.pi / 2)
denom = geom.dot_product(v_2, v_3)
if not math.isclose(denom, 0.0, abs_tol=1e-10):
d_1 = geom.cross_product(v_2,
v_1) / denom # time for projectile (0 ≤ t₁)
t_2 = geom.dot_product(v_1,
v_3) / denom # time for segment (0 ≤ t₂ ≤ 1)
if 0.0 <= d_1 and 0.0 <= t_2 <= 1.0:
return d_1 / A.v
else:
# denom is zero if the segment and the projectile are parallel
# only collide if they are perfectly aligned
if geom.are_collinear(A.pos, seg.A, seg.B):
dist_a = VecE2.normsquared(seg.A - o)
dist_b = VecE2.normsquared(seg.B - o)
return math.sqrt(min(dist_a, dist_b)) / A.v
return math.inf
def get_lerp_time_unclamped_1(A: VecE2.VecE2, B: VecE2.VecE2, Q: VecE2.VecE2):
a = Q - A
# A.show()
# B.show()
b = B - A
c = VecE2.proj(a, b, VecE2.VecE2)
if b.x != 0.0:
t = c.x / b.x
elif b.y != 0.0:
t = c.y / b.y
else:
t = 0.0 # no lerping to be done
return t
def read_centerlines(filepath_centerlines: str):
fp = open(filepath_centerlines, 'r')
lines = fp.readlines()
fp.close()
for i, line in enumerate(lines):
lines[i] = line.strip()
assert lines[0] == 'CENTERLINES'
n_centerlines = int(lines[1])
assert n_centerlines >= 0
line_index = 1
retval = {}
for i in range(n_centerlines):
line_index += 1
assert lines[line_index] == "CENTERLINE"
line_index += 1
name = lines[line_index]
line_index += 1
npts = int(lines[line_index])
line = []
for j in range(npts):
line_index += 1
matches = re.findall(FLOATING_POINT_REGEX, lines[line_index])
x = float(matches[0]) * METERS_PER_FOOT
y = float(matches[1]) * METERS_PER_FOOT
line.append(VecE2.VecE2(x, y))
centerline = []
theta = (line[1] - line[0]).atan()
centerline.append(
CurvePt.CurvePt(VecSE2.VecSE2(line[0].x, line[0].y, theta), 0.0))
for j in range(1, npts - 1):
s = centerline[j - 1].s + (line[j] - line[j - 1]).hypot()
theta = ((line[j] - line[j - 1]).atan() +
(line[j + 1] - line[j]).atan()) / 2 # mean angle
centerline.append(
CurvePt.CurvePt(VecSE2.VecSE2(line[j].x, line[j].y, theta), s))
s = centerline[npts - 2].s + (line[npts - 1] - line[npts - 2]).hypot()
theta = (line[npts - 1] - line[npts - 2]).atan()
centerline.append(
CurvePt.CurvePt(
VecSE2.VecSE2(line[npts - 1].x, line[npts - 1].y, theta), s))
retval[name] = centerline
return retval
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)
def index_closest_to_point(curve: list,
target: VecSE2.VecSE2): # curve: list(CurvePt)
"""
index_closest_to_point(curve::Curve, target::posG(VecSE2))
returns the curve index closest to the point described by `target`.
`target` must be [x, y].
"""
a = 1
b = len(curve)
c = div(a + b, 2)
assert (len(curve) >= b)
sqdist_a = curve[a - 1].pos - target
sqdist_b = curve[b - 1].pos - target
sqdist_c = curve[c - 1].pos - target
# sqdist_a.show()
# sqdist_b.show()
# sqdist_c.show()
sqdist_a = VecE2.normsquared(VecE2.VecE2(sqdist_a.x, sqdist_a.y))
sqdist_b = VecE2.normsquared(VecE2.VecE2(sqdist_b.x, sqdist_b.y))
sqdist_c = VecE2.normsquared(VecE2.VecE2(sqdist_c.x, sqdist_c.y))
# print(target.x, target.y, sqdist_a, sqdist_b, sqdist_c)
# curve[a - 1].pos.show()
# curve[b - 1].pos.show()
# curve[c - 1].pos.show()
while True:
if b == a:
return a - 1
elif b == a + 1:
return (b - 1) if sqdist_b < sqdist_a else (a - 1)
elif c == a + 1 and c == b - 1:
if sqdist_a < sqdist_b and sqdist_a < sqdist_c:
return a - 1
elif sqdist_b < sqdist_a and sqdist_b < sqdist_c:
return b - 1
else:
return c - 1
left = div(a + c, 2)
sqdist_l = curve[left - 1].pos - target
sqdist_l = VecE2.normsquared(VecE2.VecE2(sqdist_l.x, sqdist_l.y))
right = div(c + b, 2)
sqdist_r = curve[right - 1].pos - target
sqdist_r = VecE2.normsquared(VecE2.VecE2(sqdist_r.x, sqdist_r.y))
if sqdist_l < sqdist_r:
b = c
sqdist_b = sqdist_c
c = left
sqdist_c = sqdist_l
else:
a = c
sqdist_a = sqdist_c
c = right
sqdist_c = sqdist_r
raise OverflowError("index_closest_to_point reached unreachable statement")
def is_potentially_colliding(A: Vehicle, B: Vehicle):
vec = A.state.posG - B.state.posG
delta_square = VecE2.normsquared(VecE2.VecE2(vec.x, vec.y))
r_a = _bounding_radius(A)
r_b = _bounding_radius(B)
return delta_square <= r_a * r_a + 2 * r_a * r_b + r_b * r_b
def is_colliding_2(P: ConvexPolygon, Q: ConvexPolygon, temp: ConvexPolygon):
temp.set(minkowski_difference(temp, P, Q))
return in_poly(VecE2.VecE2(0, 0), temp)
def integrate(centerline_fn: str,
boundary_fn: str,
dist_threshold_lane_connect: float = 2.0,
desired_distance_between_curve_samples: float = 1.0):
'''
:param centerline_path: center line file path
:param boundary_path: boundary file path
:param dist_threshold_lane_connect: [m]
:param desired_distance_between_curve_samples: [m]
:return:
'''
centerline_path = os.path.join(DIR, "../data/", centerline_fn)
boundary_path = os.path.join(DIR, "../data/", boundary_fn)
input_params = RoadwayInputParams(filepath_boundaries=boundary_path,
filepath_centerlines=centerline_path)
roadway_data = read_roadway(input_params)
print("Finish loading centerlines and boundaries.")
lane_pts_dict = dict()
for (handle_int, lane) in enumerate(roadway_data.centerlines):
segid = get_segid(lane)
N = len(lane)
pts = [None for _ in range(N)] # VecE2 list
for i in range(N):
x = lane[i].pos.x
y = lane[i].pos.y
pts[i] = VecE2.VecE2(x, y)
laneid = 1
for tag in lane_pts_dict.keys():
if tag.segment == segid:
laneid += 1
lane_pts_dict[roadway.LaneTag(segid, laneid)] = pts
###################################################
# Shift pts to connect to previous / next pts
lane_next_dict = dict()
lane_prev_dict = dict()
for (tag, pts) in lane_pts_dict.items():
# see if can connect to next
best_tag = roadway.NULL_LANETAG
best_ind = -1
best_sq_dist = dist_threshold_lane_connect
for (tag2, pts2) in lane_pts_dict.items():
if tag2.segment != tag.segment:
for (ind, pt) in enumerate(pts2):
sq_dist = VecE2.normsquared(VecE2.VecE2(pt - pts[-1]))
if sq_dist < best_sq_dist:
best_sq_dist = sq_dist
best_ind = ind
best_tag = tag2
if best_tag != roadway.NULL_LANETAG:
# remove our last pt and set next to pt to their pt
pts.pop()
lane_next_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
if best_ind == 0: # set connect prev as well
lane_prev_dict[best_tag] = (pts[-1], tag)
for (tag, pts) in lane_pts_dict.items():
# see if can connect to prev
if tag not in lane_prev_dict.keys():
best_tag = roadway.NULL_LANETAG
best_ind = -1
best_sq_dist = dist_threshold_lane_connect
for (tag2, pts2) in lane_pts_dict.items():
if tag2.segment != tag.segment:
for (ind, pt) in enumerate(pts2):
sq_dist = VecE2.normsquared(VecE2.VecE2(pt - pts[0]))
if sq_dist < best_sq_dist:
best_sq_dist = sq_dist
best_ind = ind
best_tag = tag2
if best_tag != roadway.NULL_LANETAG:
lane_prev_dict[tag] = (lane_pts_dict[best_tag][best_ind],
best_tag)
###################################################
# Build the roadway
retval = roadway.Roadway()
for (tag, pts) in lane_pts_dict.items():
if not retval.has_segment(tag.segment):
retval.segments.append(roadway.RoadSegment(tag.segment))
lane_new_dict = dict() # old -> new tag
for seg in retval.segments:
# pull lanetags for this seg
lanetags = [] # LaneTag
for tag in lane_pts_dict.keys():
if tag.segment == seg.id:
lanetags.append(tag)
# sort the lanes such that the rightmost lane is lane 1
# do this by taking the first lane,
# then project each lane's midpoint to the perpendicular at the midpoint
assert len(lanetags) != 0
proj_positions = [None for _ in range(len(lanetags))] # list of float
first_lane_pts = lane_pts_dict[lanetags[0]]
n = len(first_lane_pts)
lo = first_lane_pts[n // 2 - 1]
hi = first_lane_pts[n // 2]
midpt_orig = (lo + hi) / 2
dir = VecE2.polar(
1.0, (hi - lo).atan() +
math.pi / 2) # direction perpendicular (left) of lane
for (i, tag) in enumerate(lanetags):
pts = lane_pts_dict[tag]
n = len(pts)
midpt = (pts[n // 2 - 1] + pts[n // 2]) / 2
proj_positions[i] = VecE2.proj_(midpt - midpt_orig, dir)
for (i, j) in enumerate(
sorted(range(len(proj_positions)),
key=proj_positions.__getitem__)):
tag = lanetags[j]
boundary_left = roadway.LaneBoundary("solid", "white") if i == len(proj_positions) - 1 \
else roadway.LaneBoundary("broken", "white")
boundary_right = roadway.LaneBoundary("solid", "white") if i == 0 \
else roadway.LaneBoundary("broken", "white")
pts = lane_pts_dict[tag]
pt_matrix = np.zeros((2, len(pts)))
for (k, P) in enumerate(pts):
pt_matrix[0, k] = P.x
pt_matrix[1, k] = P.y
print("fitting curve ", len(pts), " ")
curve = _fit_curve(pt_matrix,
desired_distance_between_curve_samples)
tag_new = roadway.LaneTag(seg.id, len(seg.lanes) + 1)
lane = roadway.Lane(tag_new,
curve,
boundary_left=boundary_left,
boundary_right=boundary_right)
seg.lanes.append(lane)
lane_new_dict[tag] = tag_new
###################################################
# Connect the lanes
for (tag_old, tup) in lane_next_dict.items():
next_pt, next_tag_old = tup
lane = retval.get_by_tag(lane_new_dict[tag_old])
next_tag_new = lane_new_dict[next_tag_old]
dest = retval.get_by_tag(next_tag_new)
roadproj = roadway.proj_1(VecSE2.VecSE2(next_pt, 0.0), dest, retval)
print("connecting {} to {}".format(lane.tag, dest.tag))
cindS = CurvePt.curveindex_end(lane.curve)
cindD = roadproj.curveproj.ind
if cindD == CurvePt.CURVEINDEX_START: # a standard connection
lane, dest = roadway.connect(lane, dest)
# remove any similar connection from lane_prev_dict
if next_tag_old in lane_prev_dict.keys(
) and lane_prev_dict[next_tag_old][1] == tag_old:
lane_prev_dict.pop(next_tag_old)
else:
lane.exits.insert(
0,
roadway.LaneConnection(True, cindS,
roadway.RoadIndex(cindD, dest.tag)))
dest.entrances.append(
roadway.LaneConnection(False, cindD,
roadway.RoadIndex(cindS, lane.tag)))
for (tag_old, tup) in lane_prev_dict.items():
prev_pt, prev_tag_old = tup
lane = retval.get_by_tag(lane_new_dict[tag_old])
prev_tag_new = lane_new_dict[prev_tag_old]
prev = retval.get_by_tag(prev_tag_new)
roadproj = roadway.proj_1(VecSE2.VecSE2(prev_pt, 0.0), prev, retval)
print("connecting {} from {}".format(lane.tag, prev.tag))
cindS = roadproj.curveproj.ind
cindD = CurvePt.CURVEINDEX_START
if cindS == CurvePt.curveindex_end(
prev.curve): # a standard connection
assert roadway.has_prev(prev)
prev, lane = roadway.connect(prev, lane)
else:
# a standard connection
prev.exits.append(
roadway.LaneConnection(True, cindS,
roadway.RoadIndex(cindD, lane.tag)))
lane.entrances.insert(
0,
roadway.LaneConnection(False, cindD,
roadway.RoadIndex(cindS, prev.tag)))
retval = convert_curves_feet_to_meters(retval)
def get_footpoint(self):
return self.state.posG.add_E2(
VecE2.polar(
self.state.posF.t,
self.state.posG.theta - self.state.posF.phi - math.pi / 2))
