carla-driver-interface 2.2.0__tar.gz → 2.3.0__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.
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/PKG-INFO +5 -3
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/README.md +4 -2
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/pyproject.toml +1 -1
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/pyproject.toml.orig +1 -1
- carla_driver_interface-2.3.0/src/carla_driver_interface/hdmap.py +410 -0
- carla_driver_interface-2.2.0/src/carla_driver_interface/hdmap.py +0 -189
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/LICENSE +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/__init__.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/__init__.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/carla_driver_pb2.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/carla_driver_pb2.pyi +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/carla_driver_pb2_grpc.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/common_pb2.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/common_pb2.pyi +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/egodriver_pb2.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/egodriver_pb2.pyi +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/egodriver_pb2_grpc.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/sensorsim_pb2.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/_proto/sensorsim_pb2.pyi +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/alpasim.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/cli.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/contract.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/driver.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/geometry.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/inprocess.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/policies/__init__.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/policies/constant_speed.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/policies/route_follower.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/polyline.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/protocol.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/py.typed +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/server.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/service.py +0 -0
- {carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/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.
|
|
3
|
+
Version: 2.3.0
|
|
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`).
|
|
77
|
-
|
|
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`).
|
|
61
|
-
|
|
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
|
|
@@ -0,0 +1,410 @@
|
|
|
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 left bound gives the direction; the right runs with it.
|
|
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
|
+
lanelet = rel.get("id", "")
|
|
270
|
+
rules = tuple(
|
|
271
|
+
m.get("ref", "")
|
|
272
|
+
for m in rel.iter("member")
|
|
273
|
+
if m.get("role") == "regulatory_element" and m.get("ref") in self.lights
|
|
274
|
+
)
|
|
275
|
+
centre = (left[[0, -1]] + right[[0, -1]]) / 2
|
|
276
|
+
length = 0.5 * (_length(left) + _length(right))
|
|
277
|
+
self.lanelets[lanelet] = (np.vstack([left, right[::-1]]), length, rules)
|
|
278
|
+
starts[_cell(centre[0])].append(lanelet)
|
|
279
|
+
ends[lanelet] = centre[1]
|
|
280
|
+
#: Each lanelet's outline's bounding box, ``(N, 4)``: min x, min y, max x, max y.
|
|
281
|
+
self._ids = list(self.lanelets)
|
|
282
|
+
self._boxes = np.array(
|
|
283
|
+
[
|
|
284
|
+
[*outline.min(axis=0), *outline.max(axis=0)]
|
|
285
|
+
for outline, _, _ in self.lanelets.values()
|
|
286
|
+
]
|
|
287
|
+
).reshape(-1, 4)
|
|
288
|
+
#: A light's stop point -> its lanelets and light elements; the points do
|
|
289
|
+
#: not move, and every step asks again.
|
|
290
|
+
self._resolved: dict[
|
|
291
|
+
tuple[float, float], tuple[tuple[str, ...], tuple[str, ...]]
|
|
292
|
+
] = {}
|
|
293
|
+
#: Lanelet id -> the lanelets that start where it ends.
|
|
294
|
+
self.successors: dict[str, list[str]] = {}
|
|
295
|
+
for lanelet, end in ends.items():
|
|
296
|
+
cx, cy = _cell(end)
|
|
297
|
+
near = [
|
|
298
|
+
other
|
|
299
|
+
for dx in (-1, 0, 1)
|
|
300
|
+
for dy in (-1, 0, 1)
|
|
301
|
+
for other in starts.get((cx + dx, cy + dy), ())
|
|
302
|
+
]
|
|
303
|
+
self.successors[lanelet] = [
|
|
304
|
+
other
|
|
305
|
+
for other in near
|
|
306
|
+
if other != lanelet
|
|
307
|
+
and np.linalg.norm(self._start(other) - end) < _JOIN_M
|
|
308
|
+
]
|
|
309
|
+
|
|
310
|
+
def _start(self, lanelet: str) -> np.ndarray:
|
|
311
|
+
outline = self.lanelets[lanelet][0]
|
|
312
|
+
return (outline[0] + outline[-1]) / 2
|
|
313
|
+
|
|
314
|
+
def lanelets_at(self, point: tuple[float, float]) -> list[str]:
|
|
315
|
+
"""The lanelets whose outline holds ``point``."""
|
|
316
|
+
x, y = point
|
|
317
|
+
b = self._boxes
|
|
318
|
+
near = np.flatnonzero(
|
|
319
|
+
(b[:, 0] <= x) & (x <= b[:, 2]) & (b[:, 1] <= y) & (y <= b[:, 3])
|
|
320
|
+
)
|
|
321
|
+
return [
|
|
322
|
+
self._ids[i] for i in near if _inside(point, self.lanelets[self._ids[i]][0])
|
|
323
|
+
]
|
|
324
|
+
|
|
325
|
+
def rules_ahead(self, lanelets: Iterable[str]) -> list[str]:
|
|
326
|
+
"""The light elements of the first lanelets on or ahead that carry one.
|
|
327
|
+
|
|
328
|
+
Each branch stops at its first lanelet with a light, or once
|
|
329
|
+
``_LIGHT_SEARCH_M`` of lanelets past ``lanelets`` lie behind it: a stop
|
|
330
|
+
point sits near the end of its own lanelet, however long that is.
|
|
331
|
+
"""
|
|
332
|
+
found: dict[str, None] = {}
|
|
333
|
+
start = set(lanelets)
|
|
334
|
+
# (lanelet, metres of lanelets between the start's end and it)
|
|
335
|
+
queue = [(lanelet, 0.0) for lanelet in start]
|
|
336
|
+
seen = set()
|
|
337
|
+
while queue:
|
|
338
|
+
lanelet, travelled = queue.pop(0)
|
|
339
|
+
if lanelet in seen:
|
|
340
|
+
continue
|
|
341
|
+
seen.add(lanelet)
|
|
342
|
+
_, length, rules = self.lanelets[lanelet]
|
|
343
|
+
if rules:
|
|
344
|
+
found.update(dict.fromkeys(rules))
|
|
345
|
+
continue
|
|
346
|
+
ahead = travelled if lanelet in start else travelled + length
|
|
347
|
+
if ahead <= _LIGHT_SEARCH_M:
|
|
348
|
+
queue += [(nxt, ahead) for nxt in self.successors[lanelet]]
|
|
349
|
+
return list(found)
|
|
350
|
+
|
|
351
|
+
def stop_lines(self, light: TrafficLight) -> list[StopLine]:
|
|
352
|
+
"""``light``'s stop points, named by this map's lanelets and elements."""
|
|
353
|
+
points = []
|
|
354
|
+
rules: dict[str, None] = {}
|
|
355
|
+
for point in light.stop_points:
|
|
356
|
+
p = (point.position_local.x, point.position_local.y)
|
|
357
|
+
if p not in self._resolved:
|
|
358
|
+
lanes = tuple(self.lanelets_at(p))
|
|
359
|
+
self._resolved[p] = (lanes, tuple(self.rules_ahead(lanes)))
|
|
360
|
+
lanes, ahead = self._resolved[p]
|
|
361
|
+
rules.update(dict.fromkeys(ahead))
|
|
362
|
+
points.append((point, lanes))
|
|
363
|
+
rule_ids = tuple(rules)
|
|
364
|
+
light_ids = tuple(
|
|
365
|
+
dict.fromkeys(ref for rule in rule_ids for ref in self.lights[rule])
|
|
366
|
+
)
|
|
367
|
+
return [
|
|
368
|
+
StopLine(
|
|
369
|
+
traffic_light_id=light.opendrive_id,
|
|
370
|
+
state=light.state,
|
|
371
|
+
position_local=np.array(
|
|
372
|
+
[
|
|
373
|
+
point.position_local.x,
|
|
374
|
+
point.position_local.y,
|
|
375
|
+
point.position_local.z,
|
|
376
|
+
],
|
|
377
|
+
dtype=np.float64,
|
|
378
|
+
),
|
|
379
|
+
road_id=point.road_id,
|
|
380
|
+
section_id=point.section_id,
|
|
381
|
+
lane_id=point.lane_id,
|
|
382
|
+
lane_ids=lanes,
|
|
383
|
+
rule_ids=rule_ids,
|
|
384
|
+
light_ids=light_ids,
|
|
385
|
+
)
|
|
386
|
+
for point, lanes in points
|
|
387
|
+
]
|
|
388
|
+
|
|
389
|
+
|
|
390
|
+
def _tags(element: ET.Element) -> dict[str, str]:
|
|
391
|
+
return {t.get("k", ""): t.get("v", "") for t in element.iter("tag")}
|
|
392
|
+
|
|
393
|
+
|
|
394
|
+
def _length(points: np.ndarray) -> float:
|
|
395
|
+
return float(np.linalg.norm(np.diff(points, axis=0), axis=1).sum())
|
|
396
|
+
|
|
397
|
+
|
|
398
|
+
def _cell(point: np.ndarray) -> tuple[int, int]:
|
|
399
|
+
return (math.floor(point[0]), math.floor(point[1]))
|
|
400
|
+
|
|
401
|
+
|
|
402
|
+
def _inside(point: tuple[float, float], outline: np.ndarray) -> bool:
|
|
403
|
+
"""Even-odd test of ``point`` against the closed polygon ``outline``."""
|
|
404
|
+
x, y = point
|
|
405
|
+
xs, ys = outline[:, 0], outline[:, 1]
|
|
406
|
+
xn, yn = np.roll(xs, -1), np.roll(ys, -1)
|
|
407
|
+
crosses = (ys > y) != (yn > y)
|
|
408
|
+
with np.errstate(divide="ignore", invalid="ignore"):
|
|
409
|
+
at = xs + (y - ys) * (xn - xs) / (yn - ys)
|
|
410
|
+
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
|
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/__init__.py
RENAMED
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/alpasim.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/cli.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/contract.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/driver.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/geometry.py
RENAMED
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/polyline.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/protocol.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/py.typed
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/server.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/service.py
RENAMED
|
File without changes
|
{carla_driver_interface-2.2.0 → carla_driver_interface-2.3.0}/src/carla_driver_interface/testing.py
RENAMED
|
File without changes
|