def linear_project_perception(description, prev_perception):
"""
Get perception from linear projection of description
"""
perception = PerceptionObstacle()
perception = prev_perception
perception.timestamp = time.time()
if "trace" not in description:
return perception
trace = description["trace"]
prev_point = (prev_perception.position.x, prev_perception.position.y,
prev_perception.position.z)
delta_s = description["speed"] * _s_delta_t
for i in range(1, len(trace)):
if on_segment(trace[i - 1], trace[i], prev_point):
dist = distance(trace[i - 1], trace[i])
delta_s += distance(trace[i - 1], prev_point)
while dist < delta_s:
delta_s -= dist
i += 1
if i < len(trace):
dist = distance(trace[i - 1], trace[i])
else:
return init_perception(description)
ratio = delta_s / dist
perception.position.CopyFrom(
get_point(trace[i - 1], trace[i], ratio))
perception.theta = math.atan2(trace[i][1] - trace[i - 1][1],
trace[i][0] - trace[i - 1][0])
perception.ClearField("polygon_point")
perception.polygon_point.extend(
generate_polygon(perception.position, perception.theta,
perception.length, perception.width))
return perception
return perception
def init_perception(description):
"""
Create perception from description
"""
perception = PerceptionObstacle()
perception.id = description["id"]
perception.position.x = description["position"][0]
perception.position.y = description["position"][1]
perception.position.z = description["position"][2]
perception.theta = description["theta"]
perception.velocity.CopyFrom(get_velocity(
description["theta"], description["speed"]))
perception.length = description["length"]
perception.width = description["width"]
perception.height = description["height"]
perception.polygon_point.extend(generate_polygon(perception.position,
perception.theta,
perception.length,
perception.width))
perception.tracking_time = description["tracking_time"]
perception.type = PerceptionObstacle.Type.Value(description["type"])
perception.timestamp = cyber_time.Time.now().to_sec()
return perception