get area of a given traffic light adapted from "carla-simulator/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py"
(traffic_light, carla_map)
| 5 | |
| 6 | |
| 7 | def _get_traffic_light_waypoints(traffic_light, carla_map): |
| 8 | """ |
| 9 | get area of a given traffic light |
| 10 | adapted from "carla-simulator/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py" |
| 11 | """ |
| 12 | base_transform = traffic_light.get_transform() |
| 13 | tv_loc = traffic_light.trigger_volume.location |
| 14 | tv_ext = traffic_light.trigger_volume.extent |
| 15 | |
| 16 | # Discretize the trigger box into points |
| 17 | x_values = np.arange(-0.9 * tv_ext.x, 0.9 * tv_ext.x, 1.0) # 0.9 to avoid crossing to adjacent lanes |
| 18 | area = [] |
| 19 | for x in x_values: |
| 20 | point_location = base_transform.transform(tv_loc + carla.Location(x=x)) |
| 21 | area.append(point_location) |
| 22 | |
| 23 | # Get the waypoints of these points, removing duplicates |
| 24 | ini_wps = [] |
| 25 | for pt in area: |
| 26 | wpx = carla_map.get_waypoint(pt) |
| 27 | # As x_values are arranged in order, only the last one has to be checked |
| 28 | if not ini_wps or ini_wps[-1].road_id != wpx.road_id or ini_wps[-1].lane_id != wpx.lane_id: |
| 29 | ini_wps.append(wpx) |
| 30 | |
| 31 | # Leaderboard: Advance them until the intersection |
| 32 | stopline_wps = [] |
| 33 | stopline_vertices = [] |
| 34 | junction_wps = [] |
| 35 | for wpx in ini_wps: |
| 36 | # Below: just use trigger volume, otherwise it's on the zebra lines. |
| 37 | # stopline_wps.append(wpx) |
| 38 | # vec_forward = wpx.transform.get_forward_vector() |
| 39 | # vec_right = carla.Vector3D(x=-vec_forward.y, y=vec_forward.x, z=0) |
| 40 | |
| 41 | # loc_left = wpx.transform.location - 0.4 * wpx.lane_width * vec_right |
| 42 | # loc_right = wpx.transform.location + 0.4 * wpx.lane_width * vec_right |
| 43 | # stopline_vertices.append([loc_left, loc_right]) |
| 44 | |
| 45 | while not wpx.is_intersection: |
| 46 | next_wp = wpx.next(0.5)[0] |
| 47 | if next_wp and not next_wp.is_intersection: |
| 48 | wpx = next_wp |
| 49 | else: |
| 50 | break |
| 51 | junction_wps.append(wpx) |
| 52 | |
| 53 | stopline_wps.append(wpx) |
| 54 | vec_forward = wpx.transform.get_forward_vector() |
| 55 | vec_right = carla.Vector3D(x=-vec_forward.y, y=vec_forward.x, z=0) |
| 56 | |
| 57 | loc_left = wpx.transform.location - 0.4 * wpx.lane_width * vec_right |
| 58 | loc_right = wpx.transform.location + 0.4 * wpx.lane_width * vec_right |
| 59 | stopline_vertices.append([loc_left, loc_right]) |
| 60 | |
| 61 | # all paths at junction for this traffic light |
| 62 | junction_paths = [] |
| 63 | path_wps = [] |
| 64 | wp_queue = deque(junction_wps) |
no test coverage detected