Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- class AbstractQualitativeTrafficLightConverter:
- DISTANCE_TO_IGNORE_IN_M = 100
- def __init__(self, hd_map: QualitativeHDMap):
- self._hd_map = hd_map
- self._tracked_traffic_lights = dict()
- def _get_qualitative_traffic_light(self, module_name, timestamp_sec, secuence_num,
- color, my_position, traffic_light_position) -> QualitativeTrafficLight:
- distance_in_m = self._get_distance_in_m(traffic_light_position, my_position)
- return QualitativeTrafficLight(module_name = module_name,
- timestamp_sec = timestamp_sec,
- secuence_num = secuence_num,
- color = color,
- relative_distance = self._get_relative_distance(distance_in_m),
- distance_in_m = distance_in_m)
- def get_qualitative_traffic_lights(self, quantitative_traffic_lights: list,
- localization) -> Iterable[QualitativeTrafficLight]:
- filtered_traffic_lights = []
- for traffic_light in quantitative_traffic_lights:
- traffic_light_position = traffic_light.position.get_components()
- my_position = localization.pose.position.get_components()
- if self._get_distance_in_m(traffic_light_position, my_position) < self.DISTANCE_TO_IGNORE_IN_M:
- qualitative_traffic_light = self._get_qualitative_traffic_light(traffic_light.header.get('module_name'),
- traffic_light.header.get('timestamp_sec'),
- traffic_light.header.get('secuence_num'),
- traffic_light.traffic_light_obstacle.color,
- my_position,
- traffic_light_position)
- filtered_traffic_lights.append(qualitative_traffic_light)
- return filtered_traffic_lights
- def _get_distance_in_m(self, traffic_light_position: list, my_position: list) -> float:
- delta_position = numpy.subtract(my_position[:2], traffic_light_position[:2])
- return phisics.get_scalar_distance(delta_position)
- def _get_relative_distance(self, distance_in_m: float) -> RelativeDistance:
- if distance_in_m < 2:
- return RelativeDistance.VERY_CLOSE
- elif distance_in_m < 5:
- return RelativeDistance.CLOSE
- elif distance_in_m < 20:
- return RelativeDistance.MEDIUM
- elif distance_in_m < 50:
- return RelativeDistance.FAR
- else:
- return RelativeDistance.VERY_FAR
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement