carla-driver-interface 2.2.0__tar.gz → 2.3.1__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (34) hide show
  1. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/PKG-INFO +5 -3
  2. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/README.md +4 -2
  3. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/pyproject.toml +1 -1
  4. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/pyproject.toml.orig +1 -1
  5. carla_driver_interface-2.3.1/src/carla_driver_interface/hdmap.py +424 -0
  6. carla_driver_interface-2.2.0/src/carla_driver_interface/hdmap.py +0 -189
  7. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/LICENSE +0 -0
  8. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/__init__.py +0 -0
  9. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/__init__.py +0 -0
  10. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/carla_driver_pb2.py +0 -0
  11. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/carla_driver_pb2.pyi +0 -0
  12. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/carla_driver_pb2_grpc.py +0 -0
  13. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/common_pb2.py +0 -0
  14. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/common_pb2.pyi +0 -0
  15. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/egodriver_pb2.py +0 -0
  16. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/egodriver_pb2.pyi +0 -0
  17. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/egodriver_pb2_grpc.py +0 -0
  18. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/sensorsim_pb2.py +0 -0
  19. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/_proto/sensorsim_pb2.pyi +0 -0
  20. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/alpasim.py +0 -0
  21. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/cli.py +0 -0
  22. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/contract.py +0 -0
  23. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/driver.py +0 -0
  24. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/geometry.py +0 -0
  25. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/inprocess.py +0 -0
  26. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/policies/__init__.py +0 -0
  27. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/policies/constant_speed.py +0 -0
  28. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/policies/route_follower.py +0 -0
  29. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/polyline.py +0 -0
  30. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/protocol.py +0 -0
  31. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/py.typed +0 -0
  32. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/server.py +0 -0
  33. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/service.py +0 -0
  34. {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.1}/src/carla_driver_interface/testing.py +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: carla-driver-interface
3
- Version: 2.2.0
3
+ Version: 2.3.1
4
4
  Summary: Both ends of alpasim's egodriver gRPC contract: serve a driving policy, or drive one without CARLA
5
5
  Author: Masaya Kataoka
6
6
  Author-email: Masaya Kataoka <ms.kataoka@gmail.com>
@@ -73,8 +73,10 @@ Every sensor reaches a policy the same way: as a frame in `session.frame_history
73
73
  Beyond alpasim's cameras, a policy run by autoware_carla_scenario gets LiDAR sweeps (rig
74
74
  frame) that way and, with `map_dir` set to its copy of the runtime's `driver.map_dir`, the
75
75
  world's map as files (`ctx.map`) and every traffic light resolved into that map's own
76
- elements (`ctx.stop_lines()`, `carla_driver_interface.hdmap`). CARLA ground truth arrives
77
- in `ctx.renderer_data` (`None` when an alpasim runtime drives the policy).
76
+ elements (`ctx.stop_lines()`, `carla_driver_interface.hdmap`). That holds for a Lanelet2
77
+ map the runtime was given rather than converted (`driver.lanelet2_path`) too: its lights are
78
+ found on the map, as the first traffic-light element on or ahead of each stop point. CARLA
79
+ ground truth arrives in `ctx.renderer_data` (`None` when an alpasim runtime drives the policy).
78
80
 
79
81
  A policy can also work the ego's turn indicators: `DriveResult(..., turn_indicator=TurnIndicator.ENABLE_LEFT)`
80
82
  (`carla_driver_interface.driver.TurnIndicator`). The values are Autoware's
@@ -57,8 +57,10 @@ Every sensor reaches a policy the same way: as a frame in `session.frame_history
57
57
  Beyond alpasim's cameras, a policy run by autoware_carla_scenario gets LiDAR sweeps (rig
58
58
  frame) that way and, with `map_dir` set to its copy of the runtime's `driver.map_dir`, the
59
59
  world's map as files (`ctx.map`) and every traffic light resolved into that map's own
60
- elements (`ctx.stop_lines()`, `carla_driver_interface.hdmap`). CARLA ground truth arrives
61
- in `ctx.renderer_data` (`None` when an alpasim runtime drives the policy).
60
+ elements (`ctx.stop_lines()`, `carla_driver_interface.hdmap`). That holds for a Lanelet2
61
+ map the runtime was given rather than converted (`driver.lanelet2_path`) too: its lights are
62
+ found on the map, as the first traffic-light element on or ahead of each stop point. CARLA
63
+ ground truth arrives in `ctx.renderer_data` (`None` when an alpasim runtime drives the policy).
62
64
 
63
65
  A policy can also work the ego's turn indicators: `DriveResult(..., turn_indicator=TurnIndicator.ENABLE_LEFT)`
64
66
  (`carla_driver_interface.driver.TurnIndicator`). The values are Autoware's
@@ -1,6 +1,6 @@
1
1
  [project]
2
2
  name = "carla-driver-interface"
3
- version = "2.2.0"
3
+ version = "2.3.1"
4
4
  description = "Both ends of alpasim's egodriver gRPC contract: serve a driving policy, or drive one without CARLA"
5
5
  readme = "README.md"
6
6
  license = "Apache-2.0"
@@ -1,6 +1,6 @@
1
1
  [project]
2
2
  name = "carla-driver-interface"
3
- version = "2.2.0"
3
+ version = "2.3.1"
4
4
  description = "Both ends of alpasim's egodriver gRPC contract: serve a driving policy, or drive one without CARLA"
5
5
  readme = "README.md"
6
6
  license = "Apache-2.0"
@@ -0,0 +1,424 @@
1
+ """Read a map set written by a runtime's map export.
2
+
3
+ The CARLA runtime in autoware_carla_scenario
4
+ (https://github.com/autowarefoundation/autoware_carla_scenario) writes these sets with
5
+ ``autoware_carla_scenario.driver.hdmap_export`` into its ``driver.map_dir``.
6
+
7
+ The driver half: no roadgen, no CARLA. A :class:`MapFiles` opens one set in
8
+ the format a driver reads and turns the traffic lights each step carries --
9
+ named by OpenDRIVE -- into :class:`StopLine` records named by that format's
10
+ own elements, so a driver never has to know OpenDRIVE was involved.
11
+
12
+ The translation runs through two roadgen traces the export writes beside the
13
+ map: the read trace, from each OpenDRIVE lane (``lane:<road>/<section>/<lane>``)
14
+ and signal (``signal:<id>``) to the IR element it became, and the format's
15
+ trace, from each IR element to the elements of the format it became. The IR
16
+ itself says which rules name a light.
17
+
18
+ A Lanelet2 map given to the runtime rather than converted
19
+ (``driver.lanelet2_path``) comes with an empty trace: nothing relates it to the
20
+ OpenDRIVE. Its lights are found on the map instead (:class:`_GivenLanelet2`):
21
+ each stop point lies on a lanelet, and the light is the traffic-light
22
+ regulatory element of the first lanelet on or ahead of it that carries one.
23
+ Its ``local_x`` / ``local_y`` must be the ``local`` frame.
24
+ """
25
+
26
+ from __future__ import annotations
27
+
28
+ import json
29
+ import math
30
+ import xml.etree.ElementTree as ET
31
+ from collections import defaultdict
32
+ from collections.abc import Iterable
33
+ from dataclasses import dataclass
34
+ from pathlib import Path
35
+
36
+ import numpy as np
37
+
38
+ from .protocol import TrafficLight, TrafficLightState
39
+
40
+ __all__ = ["MapFiles", "StopLine"]
41
+
42
+ _MANIFEST_FILE = "manifest.json"
43
+
44
+ #: The trace role of the element a format makes of an IR lane, where the
45
+ #: format gives a lane several (Lanelet2 also traces its boundaries and centre
46
+ #: line to it). A format not listed keeps every element traced to the lane.
47
+ _LANE_ROLES = {"lanelet2": "lanelet"}
48
+
49
+ #: How far ahead of a stop point a given Lanelet2 map is searched for its light.
50
+ #: CARLA puts a light's stop points at its trigger volume, a few metres short of
51
+ #: the painted line; the light's element may hang off the junction lanes past it.
52
+ _LIGHT_SEARCH_M = 50.0
53
+ #: Lanelets join where one's end and the next's start are this close (m).
54
+ _JOIN_M = 0.3
55
+
56
+
57
+ @dataclass(frozen=True)
58
+ class StopLine:
59
+ """Where to stop for one traffic light on one lane, and the light's state.
60
+
61
+ The ``*_ids`` name elements of the driver's map format, as that format
62
+ numbers them -- for Lanelet2, lanelet, regulatory element and traffic-light
63
+ line string ids.
64
+ """
65
+
66
+ #: The light's OpenDRIVE signal id.
67
+ traffic_light_id: str
68
+ state: TrafficLightState
69
+ #: The stop point on the lane's centre line, ``(3,)``, ``local`` frame.
70
+ position_local: np.ndarray
71
+ #: The lane the point lies on, as OpenDRIVE names it.
72
+ road_id: int
73
+ section_id: int
74
+ lane_id: int
75
+ #: The lane the stop point lies on -- the approach to the line -- in the
76
+ #: map format; empty when the format has no such lane.
77
+ lane_ids: tuple[str, ...]
78
+ #: The format's rule elements for this light (Lanelet2: regulatory
79
+ #: elements). They hang off the lanes the light governs, which for a
80
+ #: CARLA town are the junction lanes past the line, not ``lane_ids``.
81
+ rule_ids: tuple[str, ...]
82
+ #: The format's elements for the light itself (Lanelet2: its line strings).
83
+ light_ids: tuple[str, ...]
84
+
85
+
86
+ class MapFiles:
87
+ """One map set, opened in one format.
88
+
89
+ Args:
90
+ directory: The set's directory, ``<map_dir>/<map_id>``.
91
+ format: A format the set was written in, e.g. ``"lanelet2"``.
92
+ """
93
+
94
+ def __init__(self, directory: str | Path, format: str = "lanelet2") -> None:
95
+ self.directory = Path(directory)
96
+ manifest_path = self.directory / _MANIFEST_FILE
97
+ try:
98
+ manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
99
+ except FileNotFoundError:
100
+ raise FileNotFoundError(
101
+ f"no map set at {self.directory} (no {_MANIFEST_FILE}); the runtime writes "
102
+ "one per map under driver.map_dir -- is the driver's map_dir a copy "
103
+ "of the runtime's?"
104
+ ) from None
105
+ written = manifest.get("formats", {})
106
+ if format not in written:
107
+ raise ValueError(
108
+ f"map {manifest.get('map_id')!r} was not written as {format!r}, only as "
109
+ f"{sorted(written)}; add it to driver.map_formats"
110
+ )
111
+ self.map_id: str = manifest["map_id"]
112
+ self.map_name: str = manifest.get("map_name", "")
113
+ self.format = format
114
+ #: The map file (or, for ClipGT, directory) in ``format``.
115
+ self.path = self.directory / written[format]["path"]
116
+ #: The OpenDRIVE the set was converted from.
117
+ self.opendrive_path = self.directory / manifest["source"]
118
+
119
+ ir = json.loads((self.directory / manifest["ir"]).read_text(encoding="utf-8"))
120
+ trace = json.loads(
121
+ (self.directory / written[format]["trace"]).read_text(encoding="utf-8")
122
+ )
123
+ read = json.loads(
124
+ (self.directory / manifest["read_trace"]).read_text(encoding="utf-8")
125
+ )
126
+ self._from_opendrive = _read_trace(read)
127
+ self._rules_by_object = _ir_rules_by_object(ir)
128
+ self._refs = _trace_refs(trace)
129
+ self._lane_role = _LANE_ROLES.get(format)
130
+ #: A Lanelet2 map given as it is: its lights are found on the map.
131
+ self._given = (
132
+ _GivenLanelet2(self.path)
133
+ if format == "lanelet2" and written[format].get("provided")
134
+ else None
135
+ )
136
+
137
+ @classmethod
138
+ def open(
139
+ cls, map_dir: str | Path, map_id: str, format: str = "lanelet2"
140
+ ) -> MapFiles:
141
+ """The set ``map_id`` under ``map_dir``."""
142
+ return cls(Path(map_dir) / map_id, format)
143
+
144
+ def stop_lines(self, lights: Iterable[TrafficLight]) -> list[StopLine]:
145
+ """Each light's stop points as stop lines in this format, in arrival order."""
146
+ stop_lines = []
147
+ for light in lights:
148
+ if self._given is not None:
149
+ stop_lines += self._given.stop_lines(light)
150
+ continue
151
+ objects = self._from_opendrive.get(f"signal:{light.opendrive_id}", ())
152
+ rules = dict.fromkeys(
153
+ r for obj in objects for r in self._rules_by_object.get(obj, ())
154
+ )
155
+ rule_ids = tuple(ref for rule in rules for ref in self._ids(rule))
156
+ light_ids = tuple(ref for obj in objects for ref in self._ids(obj))
157
+ for point in light.stop_points:
158
+ lanes = self._from_opendrive.get(
159
+ f"lane:{point.road_id}/{point.section_id}/{point.lane_id}", ()
160
+ )
161
+ p = point.position_local
162
+ stop_lines.append(
163
+ StopLine(
164
+ traffic_light_id=light.opendrive_id,
165
+ state=light.state,
166
+ position_local=np.array([p.x, p.y, p.z], dtype=np.float64),
167
+ road_id=point.road_id,
168
+ section_id=point.section_id,
169
+ lane_id=point.lane_id,
170
+ lane_ids=tuple(
171
+ ref
172
+ for lane in lanes
173
+ for ref in self._ids(lane, self._lane_role)
174
+ ),
175
+ rule_ids=rule_ids,
176
+ light_ids=light_ids,
177
+ )
178
+ )
179
+ return stop_lines
180
+
181
+ def _ids(self, ir_id: str, role: str | None = None) -> tuple[str, ...]:
182
+ """The format's ids for one IR element, optionally of one role only."""
183
+ return tuple(
184
+ ref
185
+ for ref, ref_role in self._refs.get(ir_id, ())
186
+ if role is None or ref_role == role
187
+ )
188
+
189
+
190
+ def _read_trace(trace: dict) -> dict[str, list[str]]:
191
+ """OpenDRIVE element (``signal:943``, ``lane:21/0/-1``) -> the IR elements it became."""
192
+ became: dict[str, list[str]] = defaultdict(list)
193
+ for link in trace.get("links", ()):
194
+ if link["ir"] not in became[link["ref"]]:
195
+ became[link["ref"]].append(link["ir"])
196
+ return became
197
+
198
+
199
+ def _ir_rules_by_object(ir: dict) -> dict[str, list[str]]:
200
+ """IR object id -> the IR rules that name it, each once."""
201
+ rules: dict[str, list[str]] = defaultdict(list)
202
+ for rule in ir.get("rules", ()):
203
+ for obj in dict.fromkeys(rule.get("objects", ())):
204
+ rules[obj].append(rule["id"])
205
+ return rules
206
+
207
+
208
+ def _trace_refs(trace: dict) -> dict[str, list[tuple[str, str | None]]]:
209
+ """IR id -> ``(format id, role)`` for each element it became, ``kind:`` stripped."""
210
+ refs: dict[str, list[tuple[str, str | None]]] = defaultdict(list)
211
+ for link in trace.get("links", ()):
212
+ ref = str(link["ref"]).split(":", 1)[-1]
213
+ refs[link["ir"]].append((ref, link.get("role")))
214
+ return refs
215
+
216
+
217
+ class _GivenLanelet2:
218
+ """A Lanelet2 map's lanelets and traffic lights, read for resolving lights.
219
+
220
+ Only what that takes: each lanelet's outline, which lanelets follow it and
221
+ its traffic-light regulatory elements (with the line strings they refer to).
222
+ """
223
+
224
+ def __init__(self, path: Path) -> None:
225
+ root = ET.parse(path).getroot()
226
+ xy: dict[str, tuple[float, float]] = {}
227
+ for node in root.iter("node"):
228
+ tags = _tags(node)
229
+ if "local_x" in tags and "local_y" in tags:
230
+ xy[node.get("id", "")] = (
231
+ float(tags["local_x"]),
232
+ float(tags["local_y"]),
233
+ )
234
+ ways = {
235
+ way.get("id", ""): [nd.get("ref", "") for nd in way.iter("nd")]
236
+ for way in root.iter("way")
237
+ }
238
+ relations = list(root.iter("relation"))
239
+ #: Traffic-light element id -> the line strings it refers to (the lights).
240
+ self.lights: dict[str, tuple[str, ...]] = {
241
+ rel.get("id", ""): tuple(
242
+ m.get("ref", "")
243
+ for m in rel.iter("member")
244
+ if m.get("role") == "refers"
245
+ )
246
+ for rel in relations
247
+ if _tags(rel).get("subtype") == "traffic_light"
248
+ }
249
+ #: Lanelet id -> (outline ``(N, 2)``, centre-line length, its light elements).
250
+ self.lanelets: dict[str, tuple[np.ndarray, float, tuple[str, ...]]] = {}
251
+ starts: dict[tuple[int, int], list[str]] = defaultdict(list)
252
+ ends: dict[str, np.ndarray] = {}
253
+ for rel in relations:
254
+ if _tags(rel).get("type") != "lanelet":
255
+ continue
256
+ members = {m.get("role"): m.get("ref", "") for m in rel.iter("member")}
257
+ try:
258
+ left = np.array([xy[n] for n in ways[members["left"]]])
259
+ right = np.array([xy[n] for n in ways[members["right"]]])
260
+ except KeyError:
261
+ continue # a lanelet without both bounds, or off the local frame
262
+ # The right bound runs with the left.
263
+ if np.linalg.norm(left[0] - right[-1]) + np.linalg.norm(
264
+ left[-1] - right[0]
265
+ ) < np.linalg.norm(left[0] - right[0]) + np.linalg.norm(
266
+ left[-1] - right[-1]
267
+ ):
268
+ right = right[::-1]
269
+ # And the lanelet runs the way Lanelet2 reads it: with its left bound
270
+ # on the left, whichever order the ways are written in. A writer that
271
+ # shares one way between lanes running opposite ways -- roadgen does --
272
+ # writes a backward lane's bounds against its travel. Left bound on,
273
+ # right bound back, the outline turns clockwise when the left bound is
274
+ # on the left.
275
+ if _signed_area(np.vstack([left, right[::-1]])) > 0:
276
+ left, right = left[::-1], right[::-1]
277
+ lanelet = rel.get("id", "")
278
+ rules = tuple(
279
+ m.get("ref", "")
280
+ for m in rel.iter("member")
281
+ if m.get("role") == "regulatory_element" and m.get("ref") in self.lights
282
+ )
283
+ centre = (left[[0, -1]] + right[[0, -1]]) / 2
284
+ length = 0.5 * (_length(left) + _length(right))
285
+ self.lanelets[lanelet] = (np.vstack([left, right[::-1]]), length, rules)
286
+ starts[_cell(centre[0])].append(lanelet)
287
+ ends[lanelet] = centre[1]
288
+ #: Each lanelet's outline's bounding box, ``(N, 4)``: min x, min y, max x, max y.
289
+ self._ids = list(self.lanelets)
290
+ self._boxes = np.array(
291
+ [
292
+ [*outline.min(axis=0), *outline.max(axis=0)]
293
+ for outline, _, _ in self.lanelets.values()
294
+ ]
295
+ ).reshape(-1, 4)
296
+ #: A light's stop point -> its lanelets and light elements; the points do
297
+ #: not move, and every step asks again.
298
+ self._resolved: dict[
299
+ tuple[float, float], tuple[tuple[str, ...], tuple[str, ...]]
300
+ ] = {}
301
+ #: Lanelet id -> the lanelets that start where it ends.
302
+ self.successors: dict[str, list[str]] = {}
303
+ for lanelet, end in ends.items():
304
+ cx, cy = _cell(end)
305
+ near = [
306
+ other
307
+ for dx in (-1, 0, 1)
308
+ for dy in (-1, 0, 1)
309
+ for other in starts.get((cx + dx, cy + dy), ())
310
+ ]
311
+ self.successors[lanelet] = [
312
+ other
313
+ for other in near
314
+ if other != lanelet
315
+ and np.linalg.norm(self._start(other) - end) < _JOIN_M
316
+ ]
317
+
318
+ def _start(self, lanelet: str) -> np.ndarray:
319
+ outline = self.lanelets[lanelet][0]
320
+ return (outline[0] + outline[-1]) / 2
321
+
322
+ def lanelets_at(self, point: tuple[float, float]) -> list[str]:
323
+ """The lanelets whose outline holds ``point``."""
324
+ x, y = point
325
+ b = self._boxes
326
+ near = np.flatnonzero(
327
+ (b[:, 0] <= x) & (x <= b[:, 2]) & (b[:, 1] <= y) & (y <= b[:, 3])
328
+ )
329
+ return [
330
+ self._ids[i] for i in near if _inside(point, self.lanelets[self._ids[i]][0])
331
+ ]
332
+
333
+ def rules_ahead(self, lanelets: Iterable[str]) -> list[str]:
334
+ """The light elements of the first lanelets on or ahead that carry one.
335
+
336
+ Each branch stops at its first lanelet with a light, or once
337
+ ``_LIGHT_SEARCH_M`` of lanelets past ``lanelets`` lie behind it: a stop
338
+ point sits near the end of its own lanelet, however long that is.
339
+ """
340
+ found: dict[str, None] = {}
341
+ start = set(lanelets)
342
+ # (lanelet, metres of lanelets between the start's end and it)
343
+ queue = [(lanelet, 0.0) for lanelet in start]
344
+ seen = set()
345
+ while queue:
346
+ lanelet, travelled = queue.pop(0)
347
+ if lanelet in seen:
348
+ continue
349
+ seen.add(lanelet)
350
+ _, length, rules = self.lanelets[lanelet]
351
+ if rules:
352
+ found.update(dict.fromkeys(rules))
353
+ continue
354
+ ahead = travelled if lanelet in start else travelled + length
355
+ if ahead <= _LIGHT_SEARCH_M:
356
+ queue += [(nxt, ahead) for nxt in self.successors[lanelet]]
357
+ return list(found)
358
+
359
+ def stop_lines(self, light: TrafficLight) -> list[StopLine]:
360
+ """``light``'s stop points, named by this map's lanelets and elements."""
361
+ points = []
362
+ rules: dict[str, None] = {}
363
+ for point in light.stop_points:
364
+ p = (point.position_local.x, point.position_local.y)
365
+ if p not in self._resolved:
366
+ lanes = tuple(self.lanelets_at(p))
367
+ self._resolved[p] = (lanes, tuple(self.rules_ahead(lanes)))
368
+ lanes, ahead = self._resolved[p]
369
+ rules.update(dict.fromkeys(ahead))
370
+ points.append((point, lanes))
371
+ rule_ids = tuple(rules)
372
+ light_ids = tuple(
373
+ dict.fromkeys(ref for rule in rule_ids for ref in self.lights[rule])
374
+ )
375
+ return [
376
+ StopLine(
377
+ traffic_light_id=light.opendrive_id,
378
+ state=light.state,
379
+ position_local=np.array(
380
+ [
381
+ point.position_local.x,
382
+ point.position_local.y,
383
+ point.position_local.z,
384
+ ],
385
+ dtype=np.float64,
386
+ ),
387
+ road_id=point.road_id,
388
+ section_id=point.section_id,
389
+ lane_id=point.lane_id,
390
+ lane_ids=lanes,
391
+ rule_ids=rule_ids,
392
+ light_ids=light_ids,
393
+ )
394
+ for point, lanes in points
395
+ ]
396
+
397
+
398
+ def _tags(element: ET.Element) -> dict[str, str]:
399
+ return {t.get("k", ""): t.get("v", "") for t in element.iter("tag")}
400
+
401
+
402
+ def _length(points: np.ndarray) -> float:
403
+ return float(np.linalg.norm(np.diff(points, axis=0), axis=1).sum())
404
+
405
+
406
+ def _signed_area(polygon: np.ndarray) -> float:
407
+ """The shoelace area of a closed ``(N, 2)`` outline: positive anticlockwise."""
408
+ x, y = polygon[:, 0], polygon[:, 1]
409
+ return 0.5 * float(np.dot(x, np.roll(y, -1)) - np.dot(np.roll(x, -1), y))
410
+
411
+
412
+ def _cell(point: np.ndarray) -> tuple[int, int]:
413
+ return (math.floor(point[0]), math.floor(point[1]))
414
+
415
+
416
+ def _inside(point: tuple[float, float], outline: np.ndarray) -> bool:
417
+ """Even-odd test of ``point`` against the closed polygon ``outline``."""
418
+ x, y = point
419
+ xs, ys = outline[:, 0], outline[:, 1]
420
+ xn, yn = np.roll(xs, -1), np.roll(ys, -1)
421
+ crosses = (ys > y) != (yn > y)
422
+ with np.errstate(divide="ignore", invalid="ignore"):
423
+ at = xs + (y - ys) * (xn - xs) / (yn - ys)
424
+ return bool(np.count_nonzero(crosses & (x < at)) % 2)
@@ -1,189 +0,0 @@
1
- """Read a map set written by a runtime's map export.
2
-
3
- The CARLA runtime in autoware_carla_scenario
4
- (https://github.com/autowarefoundation/autoware_carla_scenario) writes these sets with
5
- ``autoware_carla_scenario.driver.hdmap_export`` into its ``driver.map_dir``.
6
-
7
- The driver half: no roadgen, no CARLA. A :class:`MapFiles` opens one set in
8
- the format a driver reads and turns the traffic lights each step carries --
9
- named by OpenDRIVE -- into :class:`StopLine` records named by that format's
10
- own elements, so a driver never has to know OpenDRIVE was involved.
11
-
12
- The translation runs through two roadgen traces the export writes beside the
13
- map: the read trace, from each OpenDRIVE lane (``lane:<road>/<section>/<lane>``)
14
- and signal (``signal:<id>``) to the IR element it became, and the format's
15
- trace, from each IR element to the elements of the format it became. The IR
16
- itself says which rules name a light.
17
- """
18
-
19
- from __future__ import annotations
20
-
21
- import json
22
- from collections import defaultdict
23
- from collections.abc import Iterable
24
- from dataclasses import dataclass
25
- from pathlib import Path
26
-
27
- import numpy as np
28
-
29
- from .protocol import TrafficLight, TrafficLightState
30
-
31
- __all__ = ["MapFiles", "StopLine"]
32
-
33
- _MANIFEST_FILE = "manifest.json"
34
-
35
- #: The trace role of the element a format makes of an IR lane, where the
36
- #: format gives a lane several (Lanelet2 also traces its boundaries and centre
37
- #: line to it). A format not listed keeps every element traced to the lane.
38
- _LANE_ROLES = {"lanelet2": "lanelet"}
39
-
40
-
41
- @dataclass(frozen=True)
42
- class StopLine:
43
- """Where to stop for one traffic light on one lane, and the light's state.
44
-
45
- The ``*_ids`` name elements of the driver's map format, as that format
46
- numbers them -- for Lanelet2, lanelet, regulatory element and traffic-light
47
- line string ids.
48
- """
49
-
50
- #: The light's OpenDRIVE signal id.
51
- traffic_light_id: str
52
- state: TrafficLightState
53
- #: The stop point on the lane's centre line, ``(3,)``, ``local`` frame.
54
- position_local: np.ndarray
55
- #: The lane the point lies on, as OpenDRIVE names it.
56
- road_id: int
57
- section_id: int
58
- lane_id: int
59
- #: The lane the stop point lies on -- the approach to the line -- in the
60
- #: map format; empty when the format has no such lane.
61
- lane_ids: tuple[str, ...]
62
- #: The format's rule elements for this light (Lanelet2: regulatory
63
- #: elements). They hang off the lanes the light governs, which for a
64
- #: CARLA town are the junction lanes past the line, not ``lane_ids``.
65
- rule_ids: tuple[str, ...]
66
- #: The format's elements for the light itself (Lanelet2: its line strings).
67
- light_ids: tuple[str, ...]
68
-
69
-
70
- class MapFiles:
71
- """One map set, opened in one format.
72
-
73
- Args:
74
- directory: The set's directory, ``<map_dir>/<map_id>``.
75
- format: A format the set was written in, e.g. ``"lanelet2"``.
76
- """
77
-
78
- def __init__(self, directory: str | Path, format: str = "lanelet2") -> None:
79
- self.directory = Path(directory)
80
- manifest_path = self.directory / _MANIFEST_FILE
81
- try:
82
- manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
83
- except FileNotFoundError:
84
- raise FileNotFoundError(
85
- f"no map set at {self.directory} (no {_MANIFEST_FILE}); the runtime writes "
86
- "one per map under driver.map_dir -- is the driver's map_dir a copy "
87
- "of the runtime's?"
88
- ) from None
89
- written = manifest.get("formats", {})
90
- if format not in written:
91
- raise ValueError(
92
- f"map {manifest.get('map_id')!r} was not written as {format!r}, only as "
93
- f"{sorted(written)}; add it to driver.map_formats"
94
- )
95
- self.map_id: str = manifest["map_id"]
96
- self.map_name: str = manifest.get("map_name", "")
97
- self.format = format
98
- #: The map file (or, for ClipGT, directory) in ``format``.
99
- self.path = self.directory / written[format]["path"]
100
- #: The OpenDRIVE the set was converted from.
101
- self.opendrive_path = self.directory / manifest["source"]
102
-
103
- ir = json.loads((self.directory / manifest["ir"]).read_text(encoding="utf-8"))
104
- trace = json.loads(
105
- (self.directory / written[format]["trace"]).read_text(encoding="utf-8")
106
- )
107
- read = json.loads(
108
- (self.directory / manifest["read_trace"]).read_text(encoding="utf-8")
109
- )
110
- self._from_opendrive = _read_trace(read)
111
- self._rules_by_object = _ir_rules_by_object(ir)
112
- self._refs = _trace_refs(trace)
113
- self._lane_role = _LANE_ROLES.get(format)
114
-
115
- @classmethod
116
- def open(
117
- cls, map_dir: str | Path, map_id: str, format: str = "lanelet2"
118
- ) -> MapFiles:
119
- """The set ``map_id`` under ``map_dir``."""
120
- return cls(Path(map_dir) / map_id, format)
121
-
122
- def stop_lines(self, lights: Iterable[TrafficLight]) -> list[StopLine]:
123
- """Each light's stop points as stop lines in this format, in arrival order."""
124
- stop_lines = []
125
- for light in lights:
126
- objects = self._from_opendrive.get(f"signal:{light.opendrive_id}", ())
127
- rules = dict.fromkeys(
128
- r for obj in objects for r in self._rules_by_object.get(obj, ())
129
- )
130
- rule_ids = tuple(ref for rule in rules for ref in self._ids(rule))
131
- light_ids = tuple(ref for obj in objects for ref in self._ids(obj))
132
- for point in light.stop_points:
133
- lanes = self._from_opendrive.get(
134
- f"lane:{point.road_id}/{point.section_id}/{point.lane_id}", ()
135
- )
136
- p = point.position_local
137
- stop_lines.append(
138
- StopLine(
139
- traffic_light_id=light.opendrive_id,
140
- state=light.state,
141
- position_local=np.array([p.x, p.y, p.z], dtype=np.float64),
142
- road_id=point.road_id,
143
- section_id=point.section_id,
144
- lane_id=point.lane_id,
145
- lane_ids=tuple(
146
- ref
147
- for lane in lanes
148
- for ref in self._ids(lane, self._lane_role)
149
- ),
150
- rule_ids=rule_ids,
151
- light_ids=light_ids,
152
- )
153
- )
154
- return stop_lines
155
-
156
- def _ids(self, ir_id: str, role: str | None = None) -> tuple[str, ...]:
157
- """The format's ids for one IR element, optionally of one role only."""
158
- return tuple(
159
- ref
160
- for ref, ref_role in self._refs.get(ir_id, ())
161
- if role is None or ref_role == role
162
- )
163
-
164
-
165
- def _read_trace(trace: dict) -> dict[str, list[str]]:
166
- """OpenDRIVE element (``signal:943``, ``lane:21/0/-1``) -> the IR elements it became."""
167
- became: dict[str, list[str]] = defaultdict(list)
168
- for link in trace.get("links", ()):
169
- if link["ir"] not in became[link["ref"]]:
170
- became[link["ref"]].append(link["ir"])
171
- return became
172
-
173
-
174
- def _ir_rules_by_object(ir: dict) -> dict[str, list[str]]:
175
- """IR object id -> the IR rules that name it, each once."""
176
- rules: dict[str, list[str]] = defaultdict(list)
177
- for rule in ir.get("rules", ()):
178
- for obj in dict.fromkeys(rule.get("objects", ())):
179
- rules[obj].append(rule["id"])
180
- return rules
181
-
182
-
183
- def _trace_refs(trace: dict) -> dict[str, list[tuple[str, str | None]]]:
184
- """IR id -> ``(format id, role)`` for each element it became, ``kind:`` stripped."""
185
- refs: dict[str, list[tuple[str, str | None]]] = defaultdict(list)
186
- for link in trace.get("links", ()):
187
- ref = str(link["ref"]).split(":", 1)[-1]
188
- refs[link["ir"]].append((ref, link.get("role")))
189
- return refs