Function to extract the full scene layout to be used as a full scene description to be given to the user :return: a dictionary describing the scene.
(carla_map)
| 22 | |
| 23 | |
| 24 | def get_scene_layout(carla_map): |
| 25 | """ |
| 26 | Function to extract the full scene layout to be used as a full scene description to be |
| 27 | given to the user |
| 28 | :return: a dictionary describing the scene. |
| 29 | """ |
| 30 | |
| 31 | def _lateral_shift(transform, shift): |
| 32 | transform.rotation.yaw += 90 |
| 33 | return transform.location + shift * transform.get_forward_vector() |
| 34 | |
| 35 | topology = [x[0] for x in carla_map.get_topology()] |
| 36 | topology = sorted(topology, key=lambda w: w.transform.location.z) |
| 37 | |
| 38 | # A road contains a list of lanes, a each lane contains a list of waypoints |
| 39 | map_dict = dict() |
| 40 | precision = 0.05 |
| 41 | for waypoint in topology: |
| 42 | waypoints = [waypoint] |
| 43 | nxt = waypoint.next(precision) |
| 44 | if len(nxt) > 0: |
| 45 | nxt = nxt[0] |
| 46 | while nxt.road_id == waypoint.road_id: |
| 47 | waypoints.append(nxt) |
| 48 | nxt = nxt.next(precision) |
| 49 | if len(nxt) > 0: |
| 50 | nxt = nxt[0] |
| 51 | else: |
| 52 | break |
| 53 | |
| 54 | left_marking = [_lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints] |
| 55 | right_marking = [_lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints] |
| 56 | |
| 57 | lane = { |
| 58 | "waypoints": waypoints, |
| 59 | "left_marking": left_marking, |
| 60 | "right_marking": right_marking |
| 61 | } |
| 62 | |
| 63 | if map_dict.get(waypoint.road_id) is None: |
| 64 | map_dict[waypoint.road_id] = {} |
| 65 | map_dict[waypoint.road_id][waypoint.lane_id] = lane |
| 66 | |
| 67 | # Generate waypoints graph |
| 68 | waypoints_graph = dict() |
| 69 | for road_key in map_dict: |
| 70 | for lane_key in map_dict[road_key]: |
| 71 | # List of waypoints |
| 72 | lane = map_dict[road_key][lane_key] |
| 73 | |
| 74 | for i in range(0, len(lane["waypoints"])): |
| 75 | next_ids = [w.id for w in lane["waypoints"][i + 1:len(lane["waypoints"])]] |
| 76 | |
| 77 | # Get left and right lane keys |
| 78 | left_lane_key = lane_key - 1 if lane_key - 1 != 0 else lane_key - 2 |
| 79 | right_lane_key = lane_key + 1 if lane_key + 1 != 0 else lane_key + 2 |
| 80 | |
| 81 | # Get left and right waypoint ids only if they are valid |
nothing calls this directly
no test coverage detected