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 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)) # 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 get_cyber_obstacle_msg(self): obstacle = PerceptionObstacle() obstacle.id = self.get_global_id() transform = self.carla_actor.get_transform() velocity = self.carla_actor.get_velocity() obstacle.position.x = transform.location.x obstacle.position.y = -transform.location.y obstacle.position.z = transform.location.z obstacle.theta = -math.radians(transform.rotation.yaw) obstacle.velocity.x = velocity.x obstacle.velocity.y = -velocity.y obstacle.velocity.z = velocity.z obstacle.length = self.carla_actor.bounding_box.extent.x * 2.0 obstacle.width = self.carla_actor.bounding_box.extent.y * 2.0 obstacle.height = self.carla_actor.bounding_box.extent.z * 2.0 return obstacle
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