OptiLine-Py 0.1.7__py3-none-any.whl
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.
- OptiLine/KinematicProfs.py +970 -0
- OptiLine/__init__.py +1 -0
- OptiLine/map_builder.py +790 -0
- OptiLine/opt_mintime.py +769 -0
- OptiLine/solvers.py +2342 -0
- OptiLine/utils.py +1656 -0
- optiline_py-0.1.7.dist-info/METADATA +35 -0
- optiline_py-0.1.7.dist-info/RECORD +9 -0
- optiline_py-0.1.7.dist-info/WHEEL +4 -0
OptiLine/map_builder.py
ADDED
|
@@ -0,0 +1,790 @@
|
|
|
1
|
+
"""
|
|
2
|
+
map_builder.py
|
|
3
|
+
==============
|
|
4
|
+
Generates random race-track maps and saves them in the same three-file
|
|
5
|
+
structure used by the 25 real-circuit maps shipped with OptiLine::
|
|
6
|
+
|
|
7
|
+
maps/<name>/
|
|
8
|
+
<name>_centerline.csv # x_m, y_m, w_tr_right_m, w_tr_left_m
|
|
9
|
+
<name>_map.png # 2000×2000 greyscale occupancy-grid image
|
|
10
|
+
<name>_map.yaml # ROS-style map metadata
|
|
11
|
+
|
|
12
|
+
Typical usage (class API)
|
|
13
|
+
-------------------------
|
|
14
|
+
>>> from OptiLine.map_builder import MapBuilder
|
|
15
|
+
>>> mb = MapBuilder(seed=42, maps_dir="maps/Zoo_Maps")
|
|
16
|
+
>>> mb.generate(n=10) # writes example_1 … example_10
|
|
17
|
+
|
|
18
|
+
>>> mb2 = MapBuilder(seed=0, variable_width=True)
|
|
19
|
+
>>> mb2.generate(n=5, start_index=26) # extends an existing dataset
|
|
20
|
+
|
|
21
|
+
Single-track generation without file I/O
|
|
22
|
+
-----------------------------------------
|
|
23
|
+
>>> track = mb.generate_track(style="circuit")
|
|
24
|
+
>>> track.keys() # {'xy', 'w_right', 'w_left', 'style'}
|
|
25
|
+
|
|
26
|
+
Functional API (unchanged for backward compatibility)
|
|
27
|
+
------------------------------------------------------
|
|
28
|
+
>>> from OptiLine.map_builder import generate_random_track, build_examples
|
|
29
|
+
"""
|
|
30
|
+
|
|
31
|
+
import os
|
|
32
|
+
import argparse
|
|
33
|
+
from collections import defaultdict
|
|
34
|
+
|
|
35
|
+
import numpy as np
|
|
36
|
+
from scipy.interpolate import CubicSpline
|
|
37
|
+
from scipy.ndimage import gaussian_filter1d
|
|
38
|
+
from PIL import Image, ImageDraw
|
|
39
|
+
|
|
40
|
+
# ---------------------------------------------------------------------------
|
|
41
|
+
# Public constants
|
|
42
|
+
# ---------------------------------------------------------------------------
|
|
43
|
+
DEFAULT_HALF_WIDTH = 1.10 # m – uniform half-width for tracks
|
|
44
|
+
POINT_SPACING = 0.40 # m – target arc-length step between CSV rows
|
|
45
|
+
IMAGE_SIZE = 2000 # px – output PNG resolution (square)
|
|
46
|
+
IMAGE_MARGIN_FRAC = 0.12 # fraction of image used as padding on each side
|
|
47
|
+
OCCUPIED_THRESH = 0.45
|
|
48
|
+
FREE_THRESH = 0.196
|
|
49
|
+
|
|
50
|
+
# Occupancy values (ROS convention, greyscale 0-255)
|
|
51
|
+
PIXEL_FREE = 255 # track surface
|
|
52
|
+
PIXEL_UNKNOWN = 205 # off-track / unknown area
|
|
53
|
+
PIXEL_OCCUPIED = 0 # track boundary walls
|
|
54
|
+
|
|
55
|
+
#: Named track archetypes available to the generator.
|
|
56
|
+
TRACK_STYLES = [
|
|
57
|
+
"oval_complex",
|
|
58
|
+
"circuit",
|
|
59
|
+
"circuit_complex",
|
|
60
|
+
"street_circuit",
|
|
61
|
+
"tilodrome",
|
|
62
|
+
]
|
|
63
|
+
|
|
64
|
+
# ---------------------------------------------------------------------------
|
|
65
|
+
# Per-style configuration (internal)
|
|
66
|
+
# ---------------------------------------------------------------------------
|
|
67
|
+
_STYLE_CFG = {
|
|
68
|
+
# n – (lo,hi) polygon vertex count
|
|
69
|
+
# asp – (lo,hi) bounding-box aspect ratio (ellipse axes width/height)
|
|
70
|
+
# hp/ch – probability of hairpin / chicane on each polygon edge
|
|
71
|
+
# plo/hi – push fraction for hairpin apex (fraction of mid-radius pushed in)
|
|
72
|
+
# bmp – chicane lateral bump fraction
|
|
73
|
+
# mhp – max hairpin count per track
|
|
74
|
+
# stiff – corner-stiffening distance from vertex (metres, pre-scale)
|
|
75
|
+
# conc – per-vertex probability of inward concavity
|
|
76
|
+
"oval_complex": dict(n=(4, 6), asp=(2.2, 3.5), hp=0.35, ch=0.18,
|
|
77
|
+
plo=0.50, phi=0.75, bmp=0.12, mhp=2,
|
|
78
|
+
stiff=5.5, conc=0.15),
|
|
79
|
+
"circuit": dict(n=(5, 8), asp=(1.4, 2.2), hp=0.30, ch=0.30,
|
|
80
|
+
plo=0.40, phi=0.68, bmp=0.14, mhp=4,
|
|
81
|
+
stiff=5.0, conc=0.25),
|
|
82
|
+
"circuit_complex": dict(n=(6, 9), asp=(1.1, 1.8), hp=0.35, ch=0.28,
|
|
83
|
+
plo=0.38, phi=0.62, bmp=0.12, mhp=5,
|
|
84
|
+
stiff=4.5, conc=0.30),
|
|
85
|
+
"street_circuit": dict(n=(6, 10), asp=(1.0, 1.5), hp=0.08, ch=0.50,
|
|
86
|
+
plo=0.25, phi=0.48, bmp=0.20, mhp=1,
|
|
87
|
+
stiff=3.5, conc=0.38),
|
|
88
|
+
"tilodrome": dict(n=(5, 7), asp=(1.5, 2.6), hp=0.30, ch=0.20,
|
|
89
|
+
plo=0.44, phi=0.72, bmp=0.10, mhp=3,
|
|
90
|
+
stiff=6.5, conc=0.20),
|
|
91
|
+
}
|
|
92
|
+
|
|
93
|
+
|
|
94
|
+
# ===========================================================================
|
|
95
|
+
# Low-level geometry helpers
|
|
96
|
+
# ===========================================================================
|
|
97
|
+
|
|
98
|
+
def _arc_lengths(pts: np.ndarray) -> np.ndarray:
|
|
99
|
+
"""Cumulative arc-length vector for an (N, 2) array of 2-D points."""
|
|
100
|
+
diffs = np.diff(pts, axis=0)
|
|
101
|
+
seg = np.hypot(diffs[:, 0], diffs[:, 1])
|
|
102
|
+
return np.concatenate([[0.0], np.cumsum(seg)])
|
|
103
|
+
|
|
104
|
+
|
|
105
|
+
def _resample_at_spacing(pts: np.ndarray, spacing: float) -> np.ndarray:
|
|
106
|
+
"""
|
|
107
|
+
Resample a closed 2-D polyline so consecutive points are ~*spacing* m
|
|
108
|
+
apart (arc-length parametrisation). Returns an open loop (first ≠ last).
|
|
109
|
+
"""
|
|
110
|
+
pts_c = np.vstack([pts, pts[0]])
|
|
111
|
+
s = _arc_lengths(pts_c)
|
|
112
|
+
total = s[-1]
|
|
113
|
+
n_pts = max(4, int(round(total / spacing)))
|
|
114
|
+
s_uni = np.linspace(0, total, n_pts + 1)[:-1]
|
|
115
|
+
x_rs = np.interp(s_uni, s, pts_c[:, 0])
|
|
116
|
+
y_rs = np.interp(s_uni, s, pts_c[:, 1])
|
|
117
|
+
return np.column_stack([x_rs, y_rs])
|
|
118
|
+
|
|
119
|
+
|
|
120
|
+
def _smooth_widths(n: int, w_min: float, w_max: float,
|
|
121
|
+
rng: np.random.Generator,
|
|
122
|
+
n_harmonics: int = 4) -> np.ndarray:
|
|
123
|
+
"""
|
|
124
|
+
Generate a smooth, periodic width profile with values in [w_min, w_max].
|
|
125
|
+
Built from a sum of low-frequency Fourier components.
|
|
126
|
+
"""
|
|
127
|
+
t = np.linspace(0, 2 * np.pi, n, endpoint=False)
|
|
128
|
+
profile = np.zeros(n)
|
|
129
|
+
for k in range(1, n_harmonics + 1):
|
|
130
|
+
amp = rng.uniform(0, 1.0 / k)
|
|
131
|
+
phase = rng.uniform(0, 2 * np.pi)
|
|
132
|
+
profile += amp * np.cos(k * t + phase)
|
|
133
|
+
profile = (profile - profile.min()) / (profile.max() - profile.min() + 1e-12)
|
|
134
|
+
return w_min + (w_max - w_min) * profile
|
|
135
|
+
|
|
136
|
+
|
|
137
|
+
# ===========================================================================
|
|
138
|
+
# Track generator: polygon backbone → corner stiffening → spline → check
|
|
139
|
+
# ===========================================================================
|
|
140
|
+
|
|
141
|
+
def _make_polygon(rng: np.random.Generator,
|
|
142
|
+
n: int, a: float, b: float,
|
|
143
|
+
concavity_prob: float = 0.25) -> np.ndarray:
|
|
144
|
+
"""
|
|
145
|
+
Return *n* vertices of an irregular polygon inscribed in an a×b ellipse.
|
|
146
|
+
|
|
147
|
+
* Angular spacing between vertices is jittered ±35 % of even spacing.
|
|
148
|
+
* Each vertex radius varies ±40 % of the base ellipse radius.
|
|
149
|
+
* With probability *concavity_prob*, a vertex is pushed further inward
|
|
150
|
+
(35–65 % of base radius), creating concavities that mimic real circuits.
|
|
151
|
+
"""
|
|
152
|
+
phi = rng.uniform(0.0, 2.0 * np.pi)
|
|
153
|
+
angs = np.linspace(0.0, 2.0 * np.pi, n, endpoint=False) + phi
|
|
154
|
+
angs += rng.uniform(-0.35, 0.35, n) * (2.0 * np.pi / n)
|
|
155
|
+
angs = np.sort(angs)
|
|
156
|
+
|
|
157
|
+
r = 1.0 + rng.uniform(-0.38, 0.38, n)
|
|
158
|
+
for i in range(n):
|
|
159
|
+
if rng.random() < concavity_prob:
|
|
160
|
+
r[i] *= rng.uniform(0.35, 0.65)
|
|
161
|
+
|
|
162
|
+
return np.column_stack([a * r * np.cos(angs), b * r * np.sin(angs)])
|
|
163
|
+
|
|
164
|
+
|
|
165
|
+
def _hairpin_pts(pt1: np.ndarray, pt2: np.ndarray,
|
|
166
|
+
rng: np.random.Generator,
|
|
167
|
+
push_lo: float, push_hi: float):
|
|
168
|
+
"""Two apex control points for a hairpin between *pt1* and *pt2*."""
|
|
169
|
+
mid = 0.5 * (pt1 + pt2)
|
|
170
|
+
r_mid = np.linalg.norm(mid)
|
|
171
|
+
if r_mid < 1.0:
|
|
172
|
+
return None
|
|
173
|
+
inward = -mid / r_mid
|
|
174
|
+
apex = mid + inward * r_mid * rng.uniform(push_lo, push_hi)
|
|
175
|
+
elen = np.linalg.norm(pt2 - pt1)
|
|
176
|
+
hw = min(elen * 0.07, 3.5)
|
|
177
|
+
ehat = (pt2 - pt1) / (elen + 1e-9)
|
|
178
|
+
return np.array([apex - ehat * hw, apex + ehat * hw])
|
|
179
|
+
|
|
180
|
+
|
|
181
|
+
def _chicane_pts(pt1: np.ndarray, pt2: np.ndarray,
|
|
182
|
+
rng: np.random.Generator,
|
|
183
|
+
bump_frac: float):
|
|
184
|
+
"""Two laterally offset control points (S-chicane) between *pt1* and *pt2*."""
|
|
185
|
+
edge = pt2 - pt1
|
|
186
|
+
elen = np.linalg.norm(edge)
|
|
187
|
+
if elen < 8.0:
|
|
188
|
+
return None
|
|
189
|
+
ehat = edge / elen
|
|
190
|
+
perp = np.array([-ehat[1], ehat[0]])
|
|
191
|
+
r_mid = np.linalg.norm(0.5 * (pt1 + pt2))
|
|
192
|
+
bump = r_mid * rng.uniform(0.05, bump_frac) * rng.choice([-1, 1])
|
|
193
|
+
c1 = pt1 + ehat * elen * 0.33 + perp * bump
|
|
194
|
+
c2 = pt1 + ehat * elen * 0.67 + perp * -bump
|
|
195
|
+
return np.array([c1, c2])
|
|
196
|
+
|
|
197
|
+
|
|
198
|
+
def _fit_periodic_spline(ctrl: np.ndarray):
|
|
199
|
+
"""
|
|
200
|
+
Fit a periodic CubicSpline through *ctrl* (N, 2) using cumulative chord
|
|
201
|
+
length. Returns (cs_x, cs_y, total_t).
|
|
202
|
+
"""
|
|
203
|
+
cp = np.vstack([ctrl, ctrl[0]])
|
|
204
|
+
d = np.diff(cp, axis=0)
|
|
205
|
+
chrd = np.maximum(np.hypot(d[:, 0], d[:, 1]), 1e-9)
|
|
206
|
+
t = np.concatenate([[0.0], np.cumsum(chrd)])
|
|
207
|
+
cs_x = CubicSpline(t, cp[:, 0], bc_type="periodic")
|
|
208
|
+
cs_y = CubicSpline(t, cp[:, 1], bc_type="periodic")
|
|
209
|
+
return cs_x, cs_y, float(t[-1])
|
|
210
|
+
|
|
211
|
+
|
|
212
|
+
def _has_self_intersection(pts: np.ndarray, clearance: float = 2.8) -> bool:
|
|
213
|
+
"""
|
|
214
|
+
Return True if any two non-adjacent track points are closer than
|
|
215
|
+
*clearance* metres (grid-hash check, O(N) average).
|
|
216
|
+
"""
|
|
217
|
+
n = len(pts)
|
|
218
|
+
skip = max(6, int(clearance / POINT_SPACING) + 3)
|
|
219
|
+
xmin, ymin = pts.min(axis=0)
|
|
220
|
+
c = clearance
|
|
221
|
+
grid = defaultdict(list)
|
|
222
|
+
for i in range(n):
|
|
223
|
+
gx = int((pts[i, 0] - xmin) / c)
|
|
224
|
+
gy = int((pts[i, 1] - ymin) / c)
|
|
225
|
+
grid[(gx, gy)].append(i)
|
|
226
|
+
|
|
227
|
+
for i in range(n):
|
|
228
|
+
gx = int((pts[i, 0] - xmin) / c)
|
|
229
|
+
gy = int((pts[i, 1] - ymin) / c)
|
|
230
|
+
for dgx in (-1, 0, 1):
|
|
231
|
+
for dgy in (-1, 0, 1):
|
|
232
|
+
for j in grid[(gx + dgx, gy + dgy)]:
|
|
233
|
+
gap = abs(i - j)
|
|
234
|
+
if skip < gap < n - skip:
|
|
235
|
+
if np.hypot(pts[i, 0] - pts[j, 0],
|
|
236
|
+
pts[i, 1] - pts[j, 1]) < clearance:
|
|
237
|
+
return True
|
|
238
|
+
return False
|
|
239
|
+
|
|
240
|
+
|
|
241
|
+
def _try_build_track(rng: np.random.Generator,
|
|
242
|
+
cfg: dict,
|
|
243
|
+
target_length: float) -> np.ndarray:
|
|
244
|
+
"""
|
|
245
|
+
Single attempt to build a track from *cfg*.
|
|
246
|
+
Returns an (N, 2) array or None if validation fails.
|
|
247
|
+
|
|
248
|
+
Pipeline
|
|
249
|
+
--------
|
|
250
|
+
1. Irregular polygon backbone with radial jitter and concavities.
|
|
251
|
+
2. 3-point corner stiffening (enter → vertex → leave) at every vertex.
|
|
252
|
+
3. Hairpin / chicane feature injection in straight sections.
|
|
253
|
+
4. Periodic CubicSpline guarantees exact closure.
|
|
254
|
+
5. Light Gaussian smoothing (σ ≈ 0.5 m) to suppress numerical oscillations.
|
|
255
|
+
6. Self-intersection rejection via grid-hash check.
|
|
256
|
+
"""
|
|
257
|
+
n = int(rng.integers(cfg["n"][0], cfg["n"][1] + 1))
|
|
258
|
+
asp = float(rng.uniform(*cfg["asp"]))
|
|
259
|
+
R = 50.0
|
|
260
|
+
a, b = R * np.sqrt(asp), R / np.sqrt(asp)
|
|
261
|
+
|
|
262
|
+
polygon = _make_polygon(rng, n, a, b, concavity_prob=cfg.get("conc", 0.25))
|
|
263
|
+
stiff = cfg.get("stiff", 5.0)
|
|
264
|
+
|
|
265
|
+
ctrl = []
|
|
266
|
+
n_hp = 0
|
|
267
|
+
for i in range(n):
|
|
268
|
+
p_prev = polygon[(i - 1) % n]
|
|
269
|
+
p1 = polygon[i]
|
|
270
|
+
p2 = polygon[(i + 1) % n]
|
|
271
|
+
|
|
272
|
+
d_in = p1 - p_prev; l_in = np.linalg.norm(d_in)
|
|
273
|
+
d_out = p2 - p1; l_out = np.linalg.norm(d_out)
|
|
274
|
+
|
|
275
|
+
s_in = min(stiff, l_in * 0.42) if l_in > 1e-6 else 0.0
|
|
276
|
+
s_out = min(stiff, l_out * 0.42) if l_out > 1e-6 else 0.0
|
|
277
|
+
|
|
278
|
+
if l_in > 1e-6: ctrl.append(p1 - (d_in / l_in) * s_in)
|
|
279
|
+
ctrl.append(p1)
|
|
280
|
+
if l_out > 1e-6: ctrl.append(p1 + (d_out / l_out) * s_out)
|
|
281
|
+
|
|
282
|
+
elen = float(l_out)
|
|
283
|
+
rv = float(rng.random())
|
|
284
|
+
if rv < cfg["hp"] and n_hp < cfg["mhp"] and elen > 12.0:
|
|
285
|
+
hp = _hairpin_pts(p1, p2, rng, cfg["plo"], cfg["phi"])
|
|
286
|
+
if hp is not None:
|
|
287
|
+
ctrl.extend(hp); n_hp += 1
|
|
288
|
+
elif rv < cfg["hp"] + cfg["ch"] and elen > 12.0:
|
|
289
|
+
ch = _chicane_pts(p1, p2, rng, cfg["bmp"])
|
|
290
|
+
if ch is not None:
|
|
291
|
+
ctrl.extend(ch)
|
|
292
|
+
|
|
293
|
+
ctrl = np.array(ctrl)
|
|
294
|
+
if len(ctrl) < 5:
|
|
295
|
+
return None
|
|
296
|
+
|
|
297
|
+
try:
|
|
298
|
+
cs_x, cs_y, total_t = _fit_periodic_spline(ctrl)
|
|
299
|
+
except Exception:
|
|
300
|
+
return None
|
|
301
|
+
|
|
302
|
+
t_d = np.linspace(0.0, total_t, 4000, endpoint=False)
|
|
303
|
+
dense = np.column_stack([cs_x(t_d), cs_y(t_d)])
|
|
304
|
+
|
|
305
|
+
approx = _arc_lengths(np.vstack([dense, dense[0]]))[-1]
|
|
306
|
+
if approx < 50.0:
|
|
307
|
+
return None
|
|
308
|
+
ctrl = ctrl * (target_length / approx)
|
|
309
|
+
|
|
310
|
+
try:
|
|
311
|
+
cs_x, cs_y, total_t = _fit_periodic_spline(ctrl)
|
|
312
|
+
except Exception:
|
|
313
|
+
return None
|
|
314
|
+
|
|
315
|
+
t_d = np.linspace(0.0, total_t, 4000, endpoint=False)
|
|
316
|
+
dense = np.column_stack([cs_x(t_d), cs_y(t_d)])
|
|
317
|
+
|
|
318
|
+
sig = max(1, int(0.5 / POINT_SPACING))
|
|
319
|
+
dense[:, 0] = gaussian_filter1d(dense[:, 0], sigma=sig, mode="wrap")
|
|
320
|
+
dense[:, 1] = gaussian_filter1d(dense[:, 1], sigma=sig, mode="wrap")
|
|
321
|
+
|
|
322
|
+
pts = _resample_at_spacing(dense, POINT_SPACING)
|
|
323
|
+
pts = pts - pts[0]
|
|
324
|
+
|
|
325
|
+
return None if _has_self_intersection(pts) else pts
|
|
326
|
+
|
|
327
|
+
|
|
328
|
+
# ===========================================================================
|
|
329
|
+
# Rendering helpers
|
|
330
|
+
# ===========================================================================
|
|
331
|
+
|
|
332
|
+
def _world_to_pixel(xy_world: np.ndarray,
|
|
333
|
+
origin: np.ndarray,
|
|
334
|
+
resolution: float,
|
|
335
|
+
img_height: int) -> np.ndarray:
|
|
336
|
+
"""World metres → image pixel coordinates (ROS convention)."""
|
|
337
|
+
px = (xy_world[:, 0] - origin[0]) / resolution
|
|
338
|
+
py = img_height - (xy_world[:, 1] - origin[1]) / resolution
|
|
339
|
+
return np.column_stack([px, py]).astype(int)
|
|
340
|
+
|
|
341
|
+
|
|
342
|
+
def render_map_png(xy: np.ndarray,
|
|
343
|
+
w_right: np.ndarray,
|
|
344
|
+
w_left: np.ndarray,
|
|
345
|
+
resolution: float,
|
|
346
|
+
origin: np.ndarray,
|
|
347
|
+
img_size: int = IMAGE_SIZE) -> Image.Image:
|
|
348
|
+
"""
|
|
349
|
+
Render an occupancy-grid PNG for a closed track.
|
|
350
|
+
|
|
351
|
+
Parameters
|
|
352
|
+
----------
|
|
353
|
+
xy : (N, 2) centrelinepoints in world metres
|
|
354
|
+
w_right : (N,) right half-widths in metres
|
|
355
|
+
w_left : (N,) left half-widths in metres
|
|
356
|
+
resolution : metres per pixel
|
|
357
|
+
origin : [x_world, y_world] of the bottom-left pixel
|
|
358
|
+
img_size : output image side length in pixels
|
|
359
|
+
|
|
360
|
+
Returns
|
|
361
|
+
-------
|
|
362
|
+
PIL.Image.Image
|
|
363
|
+
Greyscale occupancy-grid image (255 = free, 0 = wall, 205 = unknown).
|
|
364
|
+
"""
|
|
365
|
+
dp = np.vstack([np.diff(xy, axis=0), xy[0] - xy[-1]])
|
|
366
|
+
seg_len = np.hypot(dp[:, 0], dp[:, 1]) + 1e-12
|
|
367
|
+
nx = -dp[:, 1] / seg_len
|
|
368
|
+
ny = dp[:, 0] / seg_len
|
|
369
|
+
|
|
370
|
+
right_edge = xy - np.column_stack([nx * w_right, ny * w_right])
|
|
371
|
+
left_edge = xy + np.column_stack([nx * w_left, ny * w_left])
|
|
372
|
+
|
|
373
|
+
poly_world = np.vstack([left_edge, right_edge[::-1]])
|
|
374
|
+
poly_px = _world_to_pixel(poly_world, origin, resolution, img_size)
|
|
375
|
+
|
|
376
|
+
img = Image.new("L", (img_size, img_size), PIXEL_UNKNOWN)
|
|
377
|
+
draw = ImageDraw.Draw(img)
|
|
378
|
+
draw.polygon([tuple(p) for p in poly_px], fill=PIXEL_FREE)
|
|
379
|
+
|
|
380
|
+
left_px = _world_to_pixel(np.vstack([left_edge, left_edge[0:1]]),
|
|
381
|
+
origin, resolution, img_size)
|
|
382
|
+
right_px = _world_to_pixel(np.vstack([right_edge, right_edge[0:1]]),
|
|
383
|
+
origin, resolution, img_size)
|
|
384
|
+
draw.line([tuple(p) for p in left_px], fill=PIXEL_OCCUPIED, width=2)
|
|
385
|
+
draw.line([tuple(p) for p in right_px], fill=PIXEL_OCCUPIED, width=2)
|
|
386
|
+
return img
|
|
387
|
+
|
|
388
|
+
|
|
389
|
+
# ===========================================================================
|
|
390
|
+
# File writers
|
|
391
|
+
# ===========================================================================
|
|
392
|
+
|
|
393
|
+
def write_centerline_csv(path: str,
|
|
394
|
+
xy: np.ndarray,
|
|
395
|
+
w_right: np.ndarray,
|
|
396
|
+
w_left: np.ndarray) -> None:
|
|
397
|
+
"""Write the centrelineCSV in the standard OptiLine format."""
|
|
398
|
+
rows = np.column_stack([xy, w_right, w_left])
|
|
399
|
+
rows[0, :2] = 0.0 # force exact (0, 0) origin
|
|
400
|
+
with open(path, "w") as fh:
|
|
401
|
+
fh.write("# x_m, y_m, w_tr_right_m, w_tr_left_m\n")
|
|
402
|
+
for row in rows:
|
|
403
|
+
fh.write(f"{row[0]}, {row[1]}, {row[2]}, {row[3]}\n")
|
|
404
|
+
|
|
405
|
+
|
|
406
|
+
def write_map_yaml(path: str,
|
|
407
|
+
png_name: str,
|
|
408
|
+
resolution: float,
|
|
409
|
+
origin: np.ndarray) -> None:
|
|
410
|
+
"""Write the ROS-style map YAML file."""
|
|
411
|
+
with open(path, "w") as fh:
|
|
412
|
+
fh.write(f"image: {png_name}\n")
|
|
413
|
+
fh.write(f"resolution: {resolution:.5f}\n")
|
|
414
|
+
fh.write(f"origin: [{origin[0]:.15f},{origin[1]:.15f}, 0.000000]\n")
|
|
415
|
+
fh.write("negate: 0\n")
|
|
416
|
+
fh.write(f"occupied_thresh: {OCCUPIED_THRESH}\n")
|
|
417
|
+
fh.write(f"free_thresh: {FREE_THRESH}\n")
|
|
418
|
+
|
|
419
|
+
|
|
420
|
+
# ===========================================================================
|
|
421
|
+
# Functional public API
|
|
422
|
+
# ===========================================================================
|
|
423
|
+
|
|
424
|
+
def generate_random_track(
|
|
425
|
+
rng: np.random.Generator,
|
|
426
|
+
target_length: float = 450.0,
|
|
427
|
+
style: str = None,
|
|
428
|
+
variable_width: bool = False,
|
|
429
|
+
half_width_uniform: float = DEFAULT_HALF_WIDTH,
|
|
430
|
+
w_min: float = 0.8,
|
|
431
|
+
w_max: float = 3.0,
|
|
432
|
+
) -> dict:
|
|
433
|
+
"""
|
|
434
|
+
Generate a single random closed race-track (no file I/O).
|
|
435
|
+
|
|
436
|
+
Parameters
|
|
437
|
+
----------
|
|
438
|
+
rng : NumPy ``Generator`` (e.g. ``np.random.default_rng(42)``)
|
|
439
|
+
target_length : desired total track length in metres
|
|
440
|
+
style : one of :data:`TRACK_STYLES`, or *None* to pick randomly
|
|
441
|
+
variable_width : if True, half-widths vary smoothly along the track
|
|
442
|
+
half_width_uniform : uniform half-width (m) when *variable_width* is False
|
|
443
|
+
w_min / w_max : half-width bounds for variable-width mode
|
|
444
|
+
|
|
445
|
+
Returns
|
|
446
|
+
-------
|
|
447
|
+
dict
|
|
448
|
+
``{'xy': (N,2), 'w_right': (N,), 'w_left': (N,), 'style': str}``
|
|
449
|
+
"""
|
|
450
|
+
if style is None:
|
|
451
|
+
style = str(rng.choice(TRACK_STYLES))
|
|
452
|
+
cfg = _STYLE_CFG[style]
|
|
453
|
+
|
|
454
|
+
pts = None
|
|
455
|
+
for _ in range(30):
|
|
456
|
+
pts = _try_build_track(rng, cfg, target_length)
|
|
457
|
+
if pts is not None:
|
|
458
|
+
break
|
|
459
|
+
|
|
460
|
+
if pts is None: # absolute fallback: simple oval
|
|
461
|
+
t_fb = np.linspace(0.0, 2.0 * np.pi,
|
|
462
|
+
int(target_length / POINT_SPACING), endpoint=False)
|
|
463
|
+
R_fb = target_length / (2.0 * np.pi)
|
|
464
|
+
pts = np.column_stack([R_fb * np.cos(t_fb), R_fb * np.sin(t_fb)])
|
|
465
|
+
pts = pts - pts[0]
|
|
466
|
+
|
|
467
|
+
n_pts = len(pts)
|
|
468
|
+
if variable_width:
|
|
469
|
+
w_right = _smooth_widths(n_pts, w_min, w_max, rng)
|
|
470
|
+
w_left = _smooth_widths(n_pts, w_min, w_max, rng)
|
|
471
|
+
else:
|
|
472
|
+
w_right = np.full(n_pts, half_width_uniform)
|
|
473
|
+
w_left = np.full(n_pts, half_width_uniform)
|
|
474
|
+
|
|
475
|
+
return {"xy": pts, "w_right": w_right, "w_left": w_left, "style": style}
|
|
476
|
+
|
|
477
|
+
|
|
478
|
+
def build_example(
|
|
479
|
+
example_id: int,
|
|
480
|
+
maps_dir: str,
|
|
481
|
+
rng: np.random.Generator,
|
|
482
|
+
variable_width: bool = False,
|
|
483
|
+
verbose: bool = True,
|
|
484
|
+
) -> str:
|
|
485
|
+
"""
|
|
486
|
+
Generate one random track and write its three files to *maps_dir*.
|
|
487
|
+
|
|
488
|
+
Returns the path to the created folder.
|
|
489
|
+
"""
|
|
490
|
+
name = f"example_{example_id}"
|
|
491
|
+
folder = os.path.join(maps_dir, name)
|
|
492
|
+
os.makedirs(folder, exist_ok=True)
|
|
493
|
+
|
|
494
|
+
track = generate_random_track(rng=rng, variable_width=variable_width)
|
|
495
|
+
xy, w_right, w_left = track["xy"], track["w_right"], track["w_left"]
|
|
496
|
+
|
|
497
|
+
x_min, y_min = xy.min(axis=0)
|
|
498
|
+
x_max, y_max = xy.max(axis=0)
|
|
499
|
+
max_hw = max(w_right.max(), w_left.max())
|
|
500
|
+
x_min -= max_hw; x_max += max_hw
|
|
501
|
+
y_min -= max_hw; y_max += max_hw
|
|
502
|
+
|
|
503
|
+
span = max(x_max - x_min, y_max - y_min)
|
|
504
|
+
margin = span * IMAGE_MARGIN_FRAC
|
|
505
|
+
world_span = span + 2 * margin
|
|
506
|
+
origin = np.array([x_min - margin, y_min - margin])
|
|
507
|
+
resolution = world_span / IMAGE_SIZE
|
|
508
|
+
|
|
509
|
+
csv_path = os.path.join(folder, f"{name}_centerline.csv")
|
|
510
|
+
png_name = f"{name}_map.png"
|
|
511
|
+
png_path = os.path.join(folder, png_name)
|
|
512
|
+
yaml_path = os.path.join(folder, f"{name}_map.yaml")
|
|
513
|
+
|
|
514
|
+
write_centerline_csv(csv_path, xy, w_right, w_left)
|
|
515
|
+
render_map_png(xy, w_right, w_left, resolution, origin).save(png_path)
|
|
516
|
+
write_map_yaml(yaml_path, png_name, resolution, origin)
|
|
517
|
+
|
|
518
|
+
if verbose:
|
|
519
|
+
total_len = _arc_lengths(np.vstack([xy, xy[0]]))[-1]
|
|
520
|
+
print(
|
|
521
|
+
f" [{name}] pts={len(xy):4d} "
|
|
522
|
+
f"length={total_len:5.0f}m "
|
|
523
|
+
f"extent=({x_max-x_min:.0f}×{y_max-y_min:.0f})m "
|
|
524
|
+
f"resolution={resolution:.5f}m/px "
|
|
525
|
+
f"widths={'variable' if variable_width else f'{w_right[0]:.2f}m uniform'}"
|
|
526
|
+
)
|
|
527
|
+
return folder
|
|
528
|
+
|
|
529
|
+
|
|
530
|
+
def build_examples(
|
|
531
|
+
n: int = 5,
|
|
532
|
+
maps_dir: str = None,
|
|
533
|
+
seed: int = 42,
|
|
534
|
+
variable_width: bool = False,
|
|
535
|
+
start_index: int = 1,
|
|
536
|
+
) -> list:
|
|
537
|
+
"""
|
|
538
|
+
Generate *n* random tracks and save them under *maps_dir*.
|
|
539
|
+
|
|
540
|
+
Parameters
|
|
541
|
+
----------
|
|
542
|
+
n : number of examples to generate
|
|
543
|
+
maps_dir : target directory (auto-detected from project layout if None)
|
|
544
|
+
seed : random seed for reproducibility
|
|
545
|
+
variable_width : smoothly varying half-widths if True
|
|
546
|
+
start_index : first example index (1 → ``example_1``, ``example_2``, …)
|
|
547
|
+
|
|
548
|
+
Returns
|
|
549
|
+
-------
|
|
550
|
+
list of str
|
|
551
|
+
Paths to the created example folders.
|
|
552
|
+
"""
|
|
553
|
+
if maps_dir is None:
|
|
554
|
+
script_dir = os.path.dirname(os.path.abspath(__file__))
|
|
555
|
+
project_dir = os.path.dirname(os.path.dirname(script_dir)) # src/OptiLine → project root
|
|
556
|
+
candidate = os.path.join(project_dir, "maps", "Zoo_Maps")
|
|
557
|
+
if os.path.isdir(candidate):
|
|
558
|
+
maps_dir = candidate
|
|
559
|
+
elif os.path.isdir("maps"):
|
|
560
|
+
maps_dir = os.path.abspath("maps")
|
|
561
|
+
else:
|
|
562
|
+
raise FileNotFoundError(
|
|
563
|
+
"Cannot locate 'maps/' directory. "
|
|
564
|
+
"Pass maps_dir explicitly or run from the project root."
|
|
565
|
+
)
|
|
566
|
+
|
|
567
|
+
rng = np.random.default_rng(seed)
|
|
568
|
+
print(f"\nGenerating {n} map(s) → {maps_dir}")
|
|
569
|
+
print(f" seed={seed} variable_width={variable_width}\n")
|
|
570
|
+
|
|
571
|
+
folders = []
|
|
572
|
+
for i in range(n):
|
|
573
|
+
folder = build_example(
|
|
574
|
+
example_id=start_index + i,
|
|
575
|
+
maps_dir=maps_dir,
|
|
576
|
+
rng=rng,
|
|
577
|
+
variable_width=variable_width,
|
|
578
|
+
)
|
|
579
|
+
folders.append(folder)
|
|
580
|
+
|
|
581
|
+
print(f"\nDone. {n} example(s) written to {maps_dir}.")
|
|
582
|
+
return folders
|
|
583
|
+
|
|
584
|
+
|
|
585
|
+
# ===========================================================================
|
|
586
|
+
# MapBuilder class — stateful, object-oriented interface
|
|
587
|
+
# ===========================================================================
|
|
588
|
+
|
|
589
|
+
class MapBuilder:
|
|
590
|
+
"""
|
|
591
|
+
Stateful interface to the OptiLine map-generation pipeline.
|
|
592
|
+
|
|
593
|
+
Captures default settings (seed, output directory, width mode) so that
|
|
594
|
+
repeated calls to :meth:`generate` or :meth:`generate_track` do not
|
|
595
|
+
require re-specifying them.
|
|
596
|
+
|
|
597
|
+
Parameters
|
|
598
|
+
----------
|
|
599
|
+
seed : master random seed (int); passed to
|
|
600
|
+
``numpy.random.default_rng``.
|
|
601
|
+
maps_dir : path to the output directory where map folders are
|
|
602
|
+
created. Auto-detected from the project layout when
|
|
603
|
+
*None* (looks for ``maps/Zoo_Maps/`` relative to the
|
|
604
|
+
package root).
|
|
605
|
+
variable_width : if *True*, track half-widths vary smoothly along the
|
|
606
|
+
track (Fourier-based profile).
|
|
607
|
+
target_length : desired track length in metres (default 450 m).
|
|
608
|
+
half_width : uniform half-width in metres used when
|
|
609
|
+
*variable_width* is *False* (default 1.10 m).
|
|
610
|
+
w_min / w_max : half-width bounds for variable-width mode (metres).
|
|
611
|
+
|
|
612
|
+
Examples
|
|
613
|
+
--------
|
|
614
|
+
>>> mb = MapBuilder(seed=42, maps_dir="maps/Zoo_Maps")
|
|
615
|
+
>>> folders = mb.generate(n=10) # writes example_1 … example_10
|
|
616
|
+
>>> folders = mb.generate(n=5, start_index=11) # extends the dataset
|
|
617
|
+
|
|
618
|
+
>>> track = mb.generate_track(style="circuit")
|
|
619
|
+
>>> xy, w_r, w_l = track["xy"], track["w_right"], track["w_left"]
|
|
620
|
+
"""
|
|
621
|
+
|
|
622
|
+
def __init__(
|
|
623
|
+
self,
|
|
624
|
+
seed: int = 42,
|
|
625
|
+
maps_dir: str = None,
|
|
626
|
+
variable_width: bool = False,
|
|
627
|
+
target_length: float = 450.0,
|
|
628
|
+
half_width: float = DEFAULT_HALF_WIDTH,
|
|
629
|
+
w_min: float = 0.8,
|
|
630
|
+
w_max: float = 3.0,
|
|
631
|
+
):
|
|
632
|
+
self.seed = seed
|
|
633
|
+
self.maps_dir = maps_dir
|
|
634
|
+
self.variable_width = variable_width
|
|
635
|
+
self.target_length = target_length
|
|
636
|
+
self.half_width = half_width
|
|
637
|
+
self.w_min = w_min
|
|
638
|
+
self.w_max = w_max
|
|
639
|
+
self._rng = np.random.default_rng(seed)
|
|
640
|
+
|
|
641
|
+
# ------------------------------------------------------------------
|
|
642
|
+
def reset(self, seed: int = None) -> None:
|
|
643
|
+
"""
|
|
644
|
+
Reset the internal RNG.
|
|
645
|
+
|
|
646
|
+
Parameters
|
|
647
|
+
----------
|
|
648
|
+
seed : new seed; if *None*, the original seed passed to ``__init__``
|
|
649
|
+
is reused, making the sequence fully reproducible.
|
|
650
|
+
"""
|
|
651
|
+
self._rng = np.random.default_rng(
|
|
652
|
+
seed if seed is not None else self.seed
|
|
653
|
+
)
|
|
654
|
+
|
|
655
|
+
# ------------------------------------------------------------------
|
|
656
|
+
def generate_track(self, style: str = None) -> dict:
|
|
657
|
+
"""
|
|
658
|
+
Generate a single random track without writing any files.
|
|
659
|
+
|
|
660
|
+
Parameters
|
|
661
|
+
----------
|
|
662
|
+
style : one of :data:`TRACK_STYLES` or *None* (chosen randomly).
|
|
663
|
+
|
|
664
|
+
Returns
|
|
665
|
+
-------
|
|
666
|
+
dict
|
|
667
|
+
``{'xy': ndarray (N,2), 'w_right': ndarray (N,),
|
|
668
|
+
'w_left': ndarray (N,), 'style': str}``
|
|
669
|
+
"""
|
|
670
|
+
return generate_random_track(
|
|
671
|
+
rng=self._rng,
|
|
672
|
+
target_length=self.target_length,
|
|
673
|
+
style=style,
|
|
674
|
+
variable_width=self.variable_width,
|
|
675
|
+
half_width_uniform=self.half_width,
|
|
676
|
+
w_min=self.w_min,
|
|
677
|
+
w_max=self.w_max,
|
|
678
|
+
)
|
|
679
|
+
|
|
680
|
+
# ------------------------------------------------------------------
|
|
681
|
+
def generate(
|
|
682
|
+
self,
|
|
683
|
+
n: int = 1,
|
|
684
|
+
start_index: int = 1,
|
|
685
|
+
maps_dir: str = None,
|
|
686
|
+
verbose: bool = True,
|
|
687
|
+
) -> list:
|
|
688
|
+
"""
|
|
689
|
+
Generate *n* random tracks and write their files to disk.
|
|
690
|
+
|
|
691
|
+
Parameters
|
|
692
|
+
----------
|
|
693
|
+
n : number of maps to generate.
|
|
694
|
+
start_index : index of the first map (``example_<start_index>``).
|
|
695
|
+
maps_dir : override the instance-level output directory.
|
|
696
|
+
verbose : print a one-line summary for each generated map.
|
|
697
|
+
|
|
698
|
+
Returns
|
|
699
|
+
-------
|
|
700
|
+
list of str
|
|
701
|
+
Absolute paths to the created example folders.
|
|
702
|
+
"""
|
|
703
|
+
out_dir = maps_dir or self.maps_dir
|
|
704
|
+
if out_dir is None:
|
|
705
|
+
# Auto-detect project root: src/OptiLine/map_builder.py → project root
|
|
706
|
+
here = os.path.dirname(os.path.abspath(__file__))
|
|
707
|
+
project_dir = os.path.dirname(os.path.dirname(here))
|
|
708
|
+
candidate = os.path.join(project_dir, "maps", "Zoo_Maps")
|
|
709
|
+
if os.path.isdir(candidate):
|
|
710
|
+
out_dir = candidate
|
|
711
|
+
elif os.path.isdir("maps"):
|
|
712
|
+
out_dir = os.path.abspath("maps")
|
|
713
|
+
else:
|
|
714
|
+
raise FileNotFoundError(
|
|
715
|
+
"Cannot locate 'maps/' directory. "
|
|
716
|
+
"Set maps_dir in the constructor or pass it explicitly."
|
|
717
|
+
)
|
|
718
|
+
|
|
719
|
+
if verbose:
|
|
720
|
+
print(f"\nGenerating {n} map(s) → {out_dir}")
|
|
721
|
+
print(f" seed={self.seed} variable_width={self.variable_width}\n")
|
|
722
|
+
|
|
723
|
+
folders = []
|
|
724
|
+
for i in range(n):
|
|
725
|
+
folder = build_example(
|
|
726
|
+
example_id=start_index + i,
|
|
727
|
+
maps_dir=out_dir,
|
|
728
|
+
rng=self._rng,
|
|
729
|
+
variable_width=self.variable_width,
|
|
730
|
+
verbose=verbose,
|
|
731
|
+
)
|
|
732
|
+
folders.append(folder)
|
|
733
|
+
|
|
734
|
+
if verbose:
|
|
735
|
+
print(f"\nDone. {n} example(s) written to {out_dir}.")
|
|
736
|
+
return folders
|
|
737
|
+
|
|
738
|
+
# ------------------------------------------------------------------
|
|
739
|
+
@staticmethod
|
|
740
|
+
def render(xy: np.ndarray,
|
|
741
|
+
w_right: np.ndarray,
|
|
742
|
+
w_left: np.ndarray,
|
|
743
|
+
resolution: float,
|
|
744
|
+
origin: np.ndarray,
|
|
745
|
+
img_size: int = IMAGE_SIZE) -> Image.Image:
|
|
746
|
+
"""
|
|
747
|
+
Render an occupancy-grid PNG for a closed track.
|
|
748
|
+
|
|
749
|
+
Thin static wrapper around the module-level :func:`render_map_png`.
|
|
750
|
+
"""
|
|
751
|
+
return render_map_png(xy, w_right, w_left, resolution, origin, img_size)
|
|
752
|
+
|
|
753
|
+
# ------------------------------------------------------------------
|
|
754
|
+
def __repr__(self) -> str:
|
|
755
|
+
return (
|
|
756
|
+
f"MapBuilder(seed={self.seed}, variable_width={self.variable_width}, "
|
|
757
|
+
f"target_length={self.target_length}m, maps_dir={self.maps_dir!r})"
|
|
758
|
+
)
|
|
759
|
+
|
|
760
|
+
|
|
761
|
+
# ===========================================================================
|
|
762
|
+
# CLI entry-point (kept for direct script invocation)
|
|
763
|
+
# ===========================================================================
|
|
764
|
+
|
|
765
|
+
def _cli():
|
|
766
|
+
parser = argparse.ArgumentParser(
|
|
767
|
+
description="Generate random race-track maps for OptiLine."
|
|
768
|
+
)
|
|
769
|
+
parser.add_argument("--n", type=int, default=25,
|
|
770
|
+
help="Number of example maps to generate.")
|
|
771
|
+
parser.add_argument("--seed", type=int, default=7,
|
|
772
|
+
help="Random seed for reproducibility.")
|
|
773
|
+
parser.add_argument("--maps-dir", type=str, default=None,
|
|
774
|
+
help="Path to the output directory (auto-detected if omitted).")
|
|
775
|
+
parser.add_argument("--variable-width", action="store_true",
|
|
776
|
+
help="Generate smoothly varying track widths.")
|
|
777
|
+
parser.add_argument("--start-index", type=int, default=1,
|
|
778
|
+
help="Starting example index (default: 1).")
|
|
779
|
+
args = parser.parse_args()
|
|
780
|
+
|
|
781
|
+
mb = MapBuilder(
|
|
782
|
+
seed=args.seed,
|
|
783
|
+
maps_dir=args.maps_dir,
|
|
784
|
+
variable_width=args.variable_width,
|
|
785
|
+
)
|
|
786
|
+
mb.generate(n=args.n, start_index=args.start_index)
|
|
787
|
+
|
|
788
|
+
|
|
789
|
+
if __name__ == "__main__":
|
|
790
|
+
_cli()
|