Advertisement
Guest User

mis meurtos

a guest
May 22nd, 2018
65
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.23 KB | None | 0 0
  1. class AbstractQualitativeTrafficLightConverter:
  2.     DISTANCE_TO_IGNORE_IN_M = 100
  3.  
  4.     def __init__(self, hd_map: QualitativeHDMap):
  5.         self._hd_map = hd_map
  6.         self._tracked_traffic_lights = dict()
  7.        
  8.     def _get_qualitative_traffic_light(self, module_name, timestamp_sec, secuence_num,
  9.                                         color, my_position, traffic_light_position) -> QualitativeTrafficLight:
  10.         distance_in_m = self._get_distance_in_m(traffic_light_position, my_position)
  11.         return QualitativeTrafficLight(module_name = module_name,
  12.                                         timestamp_sec = timestamp_sec,
  13.                                         secuence_num = secuence_num,
  14.                                         color = color,
  15.                                         relative_distance = self._get_relative_distance(distance_in_m),
  16.                                         distance_in_m = distance_in_m)
  17.    
  18.     def get_qualitative_traffic_lights(self, quantitative_traffic_lights: list,
  19.                                        localization) -> Iterable[QualitativeTrafficLight]:
  20.         filtered_traffic_lights = []
  21.         for traffic_light in quantitative_traffic_lights:
  22.             traffic_light_position = traffic_light.position.get_components()
  23.             my_position = localization.pose.position.get_components()
  24.             if self._get_distance_in_m(traffic_light_position, my_position) < self.DISTANCE_TO_IGNORE_IN_M:
  25.                 qualitative_traffic_light = self._get_qualitative_traffic_light(traffic_light.header.get('module_name'),
  26.                                                                                 traffic_light.header.get('timestamp_sec'),
  27.                                                                                 traffic_light.header.get('secuence_num'),
  28.                                                                                 traffic_light.traffic_light_obstacle.color,
  29.                                                                                 my_position,
  30.                                                                                 traffic_light_position)
  31.                 filtered_traffic_lights.append(qualitative_traffic_light)
  32.         return filtered_traffic_lights
  33.    
  34.     def _get_distance_in_m(self, traffic_light_position: list, my_position: list) -> float:
  35.         delta_position = numpy.subtract(my_position[:2], traffic_light_position[:2])
  36.         return phisics.get_scalar_distance(delta_position)
  37.        
  38.     def _get_relative_distance(self, distance_in_m: float) -> RelativeDistance:
  39.         if distance_in_m < 2:
  40.             return RelativeDistance.VERY_CLOSE
  41.         elif distance_in_m < 5:
  42.             return RelativeDistance.CLOSE
  43.         elif distance_in_m < 20:
  44.             return RelativeDistance.MEDIUM
  45.         elif distance_in_m < 50:
  46.             return RelativeDistance.FAR
  47.         else:
  48.             return RelativeDistance.VERY_FAR
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement