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 linear_project_perception(description, prev_perception):
"""get perception from linear projection of description"""
perception = PerceptionObstacle()
perception = prev_perception
perception.timestamp = rospy.get_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))
# for point in generate_polygon(perception.position, perception.theta,
# perception.length, perception.width):
# p = perception.polygon_point.add()
# p.x = point[0]
# p.y = point[1]
# p.z = point[2]
perception.tracking_time = description["tracking_time"]
perception.type = PerceptionObstacle.Type.Value(description["type"])
perception.timestamp = rospy.get_time()
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
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))
# for point in generate_polygon(perception.position, perception.theta,
# perception.length, perception.width):
# p = perception.polygon_point.add()
# p.x = point[0]
# p.y = point[1]
# p.z = point[2]
perception.tracking_time = description["tracking_time"]
perception.type = PerceptionObstacle.Type.Value(description["type"])
perception.timestamp=rospy.get_time()
return perception