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 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 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))
