crgutils 0.1.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.
- crgutils-0.1.0/PKG-INFO +173 -0
- crgutils-0.1.0/README.md +163 -0
- crgutils-0.1.0/pyproject.toml +32 -0
- crgutils-0.1.0/src/crgutils/__init__.py +38 -0
- crgutils-0.1.0/src/crgutils/_check.py +102 -0
- crgutils-0.1.0/src/crgutils/_contact_point.py +167 -0
- crgutils-0.1.0/src/crgutils/_convenience.py +96 -0
- crgutils-0.1.0/src/crgutils/_dataset.py +192 -0
- crgutils-0.1.0/src/crgutils/_eval.py +541 -0
- crgutils-0.1.0/src/crgutils/_loader.py +29 -0
- crgutils-0.1.0/src/crgutils/_loader_impl.py +732 -0
- crgutils-0.1.0/src/crgutils/_modifiers.py +345 -0
- crgutils-0.1.0/src/crgutils/_types.py +54 -0
- crgutils-0.1.0/src/crgutils/py.typed +0 -0
crgutils-0.1.0/PKG-INFO
ADDED
|
@@ -0,0 +1,173 @@
|
|
|
1
|
+
Metadata-Version: 2.3
|
|
2
|
+
Name: crgutils
|
|
3
|
+
Version: 0.1.0
|
|
4
|
+
Summary: Add your description here
|
|
5
|
+
Author: coder
|
|
6
|
+
Author-email: coder <coder@timeintegral.ai>
|
|
7
|
+
Requires-Dist: numpy>=1.26
|
|
8
|
+
Requires-Python: >=3.11
|
|
9
|
+
Description-Content-Type: text/markdown
|
|
10
|
+
|
|
11
|
+
# crgutils
|
|
12
|
+
|
|
13
|
+
`crgutils` is a Python library for loading, validating, and evaluating ASAM OpenCRG road surface files.
|
|
14
|
+
|
|
15
|
+
Supported formats and capabilities:
|
|
16
|
+
|
|
17
|
+
- ASCII `.crg` inputs in `LRFI` and `LDFI` formats
|
|
18
|
+
- Binary `.crg` inputs in `KRBI` and `KDBI` formats
|
|
19
|
+
- Road-space queries `(u, v) → z`, `(u, v) → (x, y)`, `(u, v) → (phi, kappa)`
|
|
20
|
+
- World-space queries `(x, y) → z`, `(x, y) → (u, v)`, `(x, y) → (phi, kappa)`
|
|
21
|
+
- All five border modes: `NONE`, `EX_ZERO`, `EX_KEEP`, `REPEAT`, `REFLECT`
|
|
22
|
+
- Header-defined evaluation options and pending runtime modifiers
|
|
23
|
+
|
|
24
|
+
## Install
|
|
25
|
+
|
|
26
|
+
Core package:
|
|
27
|
+
|
|
28
|
+
```bash
|
|
29
|
+
uv sync
|
|
30
|
+
```
|
|
31
|
+
|
|
32
|
+
Core package with test dependencies:
|
|
33
|
+
|
|
34
|
+
```bash
|
|
35
|
+
uv sync --extra dev
|
|
36
|
+
```
|
|
37
|
+
|
|
38
|
+
If you also want the demo workspace members, especially the visualizer's extra dependencies:
|
|
39
|
+
|
|
40
|
+
```bash
|
|
41
|
+
uv sync --all-packages
|
|
42
|
+
```
|
|
43
|
+
|
|
44
|
+
Python 3.11+ is required.
|
|
45
|
+
|
|
46
|
+
## Quick Start
|
|
47
|
+
|
|
48
|
+
```python
|
|
49
|
+
import crgutils
|
|
50
|
+
|
|
51
|
+
ds = crgutils.read("samples/crg-txt/handmade_straight.crg") # load + validate + apply modifiers
|
|
52
|
+
cp = ds.create_contact_point() # default options
|
|
53
|
+
|
|
54
|
+
z = cp.eval_uv_to_z(10.0, 0.0) # elevation at road (u=10, v=0)
|
|
55
|
+
x, y = cp.eval_uv_to_xy(10.0, 0.5) # road → world coords
|
|
56
|
+
u, v = cp.eval_xy_to_uv(x, y) # world → road coords
|
|
57
|
+
phi, kappa = cp.eval_uv_to_pk(10.0, 0.0) # heading and curvature
|
|
58
|
+
```
|
|
59
|
+
|
|
60
|
+
Override evaluation options when creating the contact point:
|
|
61
|
+
|
|
62
|
+
```python
|
|
63
|
+
cp = ds.create_contact_point(border_mode_u=crgutils.BorderMode.REPEAT)
|
|
64
|
+
```
|
|
65
|
+
|
|
66
|
+
## API Surface
|
|
67
|
+
|
|
68
|
+
| Symbol | Purpose |
|
|
69
|
+
|---|---|
|
|
70
|
+
| `crgutils.read(path, **kwargs)` | Load, validate, and prepare a `.crg` file; returns `CRGDataset` |
|
|
71
|
+
| `dataset.create_contact_point(**options)` | Create a `ContactPoint` evaluation context |
|
|
72
|
+
| `crgutils.ContactPoint(dataset, **options)` | Stateful evaluation context (direct constructor) |
|
|
73
|
+
|
|
74
|
+
Exported types:
|
|
75
|
+
|
|
76
|
+
| Type | Description |
|
|
77
|
+
|---|---|
|
|
78
|
+
| `CRGDataset` | Parsed road surface (arrays + metadata) |
|
|
79
|
+
| `EvalOptions` | Frozen dataclass for evaluation parameters |
|
|
80
|
+
| `BorderMode` | `NONE / EX_ZERO / EX_KEEP / REPEAT / REFLECT` |
|
|
81
|
+
| `RefLineContinue` | `EXTRAPOLATE / CLOSE_TRACK` |
|
|
82
|
+
| `CurvMode` | `LATERAL / REF_LINE` |
|
|
83
|
+
| `GridNaNMode` | `KEEP / SET_ZERO / KEEP_LAST` |
|
|
84
|
+
|
|
85
|
+
### `ContactPoint` evaluation methods
|
|
86
|
+
|
|
87
|
+
```python
|
|
88
|
+
cp.eval_uv_to_z(u, v) → float # elevation from road coords
|
|
89
|
+
cp.eval_xy_to_z(x, y) → float # elevation from world coords
|
|
90
|
+
cp.eval_uv_to_xy(u, v) → (float, float) # road → world
|
|
91
|
+
cp.eval_xy_to_uv(x, y) → (float, float) # world → road
|
|
92
|
+
cp.eval_uv_to_pk(u, v) → (float, float) # heading, curvature
|
|
93
|
+
cp.eval_xy_to_pk(x, y) → (float, float) # heading, curvature
|
|
94
|
+
cp.eval_uv_to_z_grid(u, v) → np.ndarray # vectorised elevation
|
|
95
|
+
cp.eval_xy_to_z_grid(x, y) → np.ndarray # vectorised elevation
|
|
96
|
+
cp.with_options(**overrides) → ContactPoint # new context, shared dataset
|
|
97
|
+
```
|
|
98
|
+
|
|
99
|
+
## Sample Data
|
|
100
|
+
|
|
101
|
+
Bundled sample files live in:
|
|
102
|
+
|
|
103
|
+
```text
|
|
104
|
+
samples/crg-txt/ ASCII CRG files (straight, curved, banked, sloped, circle, …)
|
|
105
|
+
samples/crg-bin/ Binary CRG files (Belgian block cobblestone, country road, …)
|
|
106
|
+
```
|
|
107
|
+
|
|
108
|
+
Both the demo scripts and the test suite load from these directories.
|
|
109
|
+
|
|
110
|
+
## Demos
|
|
111
|
+
|
|
112
|
+
The repository has two demo areas:
|
|
113
|
+
|
|
114
|
+
- `demo/c-api/` contains Python ports of the upstream OpenCRG C API demos
|
|
115
|
+
- `demo/viz/` contains interactive visualization tools with their own dependencies
|
|
116
|
+
|
|
117
|
+
### C-API Demo Ports
|
|
118
|
+
|
|
119
|
+
```bash
|
|
120
|
+
uv run python demo/c-api/simple.py # xy→z on a 2D grid
|
|
121
|
+
uv run python demo/c-api/reader.py # header / channel / road info
|
|
122
|
+
uv run python demo/c-api/curvature.py # curvature profile
|
|
123
|
+
uv run python demo/c-api/eval_z.py # uv→z, round-trip z, pk
|
|
124
|
+
uv run python demo/c-api/eval_xy_uv.py # uv→xy→uv round-trips
|
|
125
|
+
uv run python demo/c-api/eval_options.py # 29 option/modifier tests
|
|
126
|
+
```
|
|
127
|
+
|
|
128
|
+
Each script defaults to a file in `samples/crg-txt/` and accepts an explicit path:
|
|
129
|
+
|
|
130
|
+
```bash
|
|
131
|
+
uv run python demo/c-api/reader.py samples/crg-bin/belgian_block.crg
|
|
132
|
+
uv run python demo/c-api/eval_xy_uv.py samples/crg-txt/handmade_curved.crg
|
|
133
|
+
uv run python demo/c-api/eval_options.py -t 5 # run only test 5
|
|
134
|
+
```
|
|
135
|
+
|
|
136
|
+
Pass `--help` to any script for usage details.
|
|
137
|
+
|
|
138
|
+
### Visualizer
|
|
139
|
+
|
|
140
|
+
The visualizer lives in the `viz` workspace member and includes both a marimo app and a notebook:
|
|
141
|
+
|
|
142
|
+
```bash
|
|
143
|
+
uv sync --package viz
|
|
144
|
+
uv run --package viz marimo edit demo/viz/crg_viz.py
|
|
145
|
+
uv run --package viz jupyter notebook demo/viz/render.ipynb
|
|
146
|
+
```
|
|
147
|
+
|
|
148
|
+
The marimo app lets you browse bundled sample files or point it at any local `.crg` file.
|
|
149
|
+
|
|
150
|
+
## Tests
|
|
151
|
+
|
|
152
|
+
```bash
|
|
153
|
+
uv run pytest
|
|
154
|
+
```
|
|
155
|
+
|
|
156
|
+
The test suite covers the loader, all evaluation functions, all five border modes,
|
|
157
|
+
modifiers, `check()`, `ContactPoint` option inheritance, and round-trip accuracy.
|
|
158
|
+
|
|
159
|
+
## Repository Layout
|
|
160
|
+
|
|
161
|
+
```text
|
|
162
|
+
src/crgutils/ library source
|
|
163
|
+
samples/ bundled CRG sample files (crg-txt/ and crg-bin/)
|
|
164
|
+
demo/c-api/ Python ports of the OpenCRG C API demos
|
|
165
|
+
demo/viz/ visualization app and notebook
|
|
166
|
+
tests/ pytest test suite
|
|
167
|
+
```
|
|
168
|
+
|
|
169
|
+
## Notes
|
|
170
|
+
|
|
171
|
+
- The binary Belgian-block sample contains all-NaN edge channels that produce expected
|
|
172
|
+
`RuntimeWarning`s from NumPy when computing array summaries; the file still loads and
|
|
173
|
+
is fully covered by tests.
|
crgutils-0.1.0/README.md
ADDED
|
@@ -0,0 +1,163 @@
|
|
|
1
|
+
# crgutils
|
|
2
|
+
|
|
3
|
+
`crgutils` is a Python library for loading, validating, and evaluating ASAM OpenCRG road surface files.
|
|
4
|
+
|
|
5
|
+
Supported formats and capabilities:
|
|
6
|
+
|
|
7
|
+
- ASCII `.crg` inputs in `LRFI` and `LDFI` formats
|
|
8
|
+
- Binary `.crg` inputs in `KRBI` and `KDBI` formats
|
|
9
|
+
- Road-space queries `(u, v) → z`, `(u, v) → (x, y)`, `(u, v) → (phi, kappa)`
|
|
10
|
+
- World-space queries `(x, y) → z`, `(x, y) → (u, v)`, `(x, y) → (phi, kappa)`
|
|
11
|
+
- All five border modes: `NONE`, `EX_ZERO`, `EX_KEEP`, `REPEAT`, `REFLECT`
|
|
12
|
+
- Header-defined evaluation options and pending runtime modifiers
|
|
13
|
+
|
|
14
|
+
## Install
|
|
15
|
+
|
|
16
|
+
Core package:
|
|
17
|
+
|
|
18
|
+
```bash
|
|
19
|
+
uv sync
|
|
20
|
+
```
|
|
21
|
+
|
|
22
|
+
Core package with test dependencies:
|
|
23
|
+
|
|
24
|
+
```bash
|
|
25
|
+
uv sync --extra dev
|
|
26
|
+
```
|
|
27
|
+
|
|
28
|
+
If you also want the demo workspace members, especially the visualizer's extra dependencies:
|
|
29
|
+
|
|
30
|
+
```bash
|
|
31
|
+
uv sync --all-packages
|
|
32
|
+
```
|
|
33
|
+
|
|
34
|
+
Python 3.11+ is required.
|
|
35
|
+
|
|
36
|
+
## Quick Start
|
|
37
|
+
|
|
38
|
+
```python
|
|
39
|
+
import crgutils
|
|
40
|
+
|
|
41
|
+
ds = crgutils.read("samples/crg-txt/handmade_straight.crg") # load + validate + apply modifiers
|
|
42
|
+
cp = ds.create_contact_point() # default options
|
|
43
|
+
|
|
44
|
+
z = cp.eval_uv_to_z(10.0, 0.0) # elevation at road (u=10, v=0)
|
|
45
|
+
x, y = cp.eval_uv_to_xy(10.0, 0.5) # road → world coords
|
|
46
|
+
u, v = cp.eval_xy_to_uv(x, y) # world → road coords
|
|
47
|
+
phi, kappa = cp.eval_uv_to_pk(10.0, 0.0) # heading and curvature
|
|
48
|
+
```
|
|
49
|
+
|
|
50
|
+
Override evaluation options when creating the contact point:
|
|
51
|
+
|
|
52
|
+
```python
|
|
53
|
+
cp = ds.create_contact_point(border_mode_u=crgutils.BorderMode.REPEAT)
|
|
54
|
+
```
|
|
55
|
+
|
|
56
|
+
## API Surface
|
|
57
|
+
|
|
58
|
+
| Symbol | Purpose |
|
|
59
|
+
|---|---|
|
|
60
|
+
| `crgutils.read(path, **kwargs)` | Load, validate, and prepare a `.crg` file; returns `CRGDataset` |
|
|
61
|
+
| `dataset.create_contact_point(**options)` | Create a `ContactPoint` evaluation context |
|
|
62
|
+
| `crgutils.ContactPoint(dataset, **options)` | Stateful evaluation context (direct constructor) |
|
|
63
|
+
|
|
64
|
+
Exported types:
|
|
65
|
+
|
|
66
|
+
| Type | Description |
|
|
67
|
+
|---|---|
|
|
68
|
+
| `CRGDataset` | Parsed road surface (arrays + metadata) |
|
|
69
|
+
| `EvalOptions` | Frozen dataclass for evaluation parameters |
|
|
70
|
+
| `BorderMode` | `NONE / EX_ZERO / EX_KEEP / REPEAT / REFLECT` |
|
|
71
|
+
| `RefLineContinue` | `EXTRAPOLATE / CLOSE_TRACK` |
|
|
72
|
+
| `CurvMode` | `LATERAL / REF_LINE` |
|
|
73
|
+
| `GridNaNMode` | `KEEP / SET_ZERO / KEEP_LAST` |
|
|
74
|
+
|
|
75
|
+
### `ContactPoint` evaluation methods
|
|
76
|
+
|
|
77
|
+
```python
|
|
78
|
+
cp.eval_uv_to_z(u, v) → float # elevation from road coords
|
|
79
|
+
cp.eval_xy_to_z(x, y) → float # elevation from world coords
|
|
80
|
+
cp.eval_uv_to_xy(u, v) → (float, float) # road → world
|
|
81
|
+
cp.eval_xy_to_uv(x, y) → (float, float) # world → road
|
|
82
|
+
cp.eval_uv_to_pk(u, v) → (float, float) # heading, curvature
|
|
83
|
+
cp.eval_xy_to_pk(x, y) → (float, float) # heading, curvature
|
|
84
|
+
cp.eval_uv_to_z_grid(u, v) → np.ndarray # vectorised elevation
|
|
85
|
+
cp.eval_xy_to_z_grid(x, y) → np.ndarray # vectorised elevation
|
|
86
|
+
cp.with_options(**overrides) → ContactPoint # new context, shared dataset
|
|
87
|
+
```
|
|
88
|
+
|
|
89
|
+
## Sample Data
|
|
90
|
+
|
|
91
|
+
Bundled sample files live in:
|
|
92
|
+
|
|
93
|
+
```text
|
|
94
|
+
samples/crg-txt/ ASCII CRG files (straight, curved, banked, sloped, circle, …)
|
|
95
|
+
samples/crg-bin/ Binary CRG files (Belgian block cobblestone, country road, …)
|
|
96
|
+
```
|
|
97
|
+
|
|
98
|
+
Both the demo scripts and the test suite load from these directories.
|
|
99
|
+
|
|
100
|
+
## Demos
|
|
101
|
+
|
|
102
|
+
The repository has two demo areas:
|
|
103
|
+
|
|
104
|
+
- `demo/c-api/` contains Python ports of the upstream OpenCRG C API demos
|
|
105
|
+
- `demo/viz/` contains interactive visualization tools with their own dependencies
|
|
106
|
+
|
|
107
|
+
### C-API Demo Ports
|
|
108
|
+
|
|
109
|
+
```bash
|
|
110
|
+
uv run python demo/c-api/simple.py # xy→z on a 2D grid
|
|
111
|
+
uv run python demo/c-api/reader.py # header / channel / road info
|
|
112
|
+
uv run python demo/c-api/curvature.py # curvature profile
|
|
113
|
+
uv run python demo/c-api/eval_z.py # uv→z, round-trip z, pk
|
|
114
|
+
uv run python demo/c-api/eval_xy_uv.py # uv→xy→uv round-trips
|
|
115
|
+
uv run python demo/c-api/eval_options.py # 29 option/modifier tests
|
|
116
|
+
```
|
|
117
|
+
|
|
118
|
+
Each script defaults to a file in `samples/crg-txt/` and accepts an explicit path:
|
|
119
|
+
|
|
120
|
+
```bash
|
|
121
|
+
uv run python demo/c-api/reader.py samples/crg-bin/belgian_block.crg
|
|
122
|
+
uv run python demo/c-api/eval_xy_uv.py samples/crg-txt/handmade_curved.crg
|
|
123
|
+
uv run python demo/c-api/eval_options.py -t 5 # run only test 5
|
|
124
|
+
```
|
|
125
|
+
|
|
126
|
+
Pass `--help` to any script for usage details.
|
|
127
|
+
|
|
128
|
+
### Visualizer
|
|
129
|
+
|
|
130
|
+
The visualizer lives in the `viz` workspace member and includes both a marimo app and a notebook:
|
|
131
|
+
|
|
132
|
+
```bash
|
|
133
|
+
uv sync --package viz
|
|
134
|
+
uv run --package viz marimo edit demo/viz/crg_viz.py
|
|
135
|
+
uv run --package viz jupyter notebook demo/viz/render.ipynb
|
|
136
|
+
```
|
|
137
|
+
|
|
138
|
+
The marimo app lets you browse bundled sample files or point it at any local `.crg` file.
|
|
139
|
+
|
|
140
|
+
## Tests
|
|
141
|
+
|
|
142
|
+
```bash
|
|
143
|
+
uv run pytest
|
|
144
|
+
```
|
|
145
|
+
|
|
146
|
+
The test suite covers the loader, all evaluation functions, all five border modes,
|
|
147
|
+
modifiers, `check()`, `ContactPoint` option inheritance, and round-trip accuracy.
|
|
148
|
+
|
|
149
|
+
## Repository Layout
|
|
150
|
+
|
|
151
|
+
```text
|
|
152
|
+
src/crgutils/ library source
|
|
153
|
+
samples/ bundled CRG sample files (crg-txt/ and crg-bin/)
|
|
154
|
+
demo/c-api/ Python ports of the OpenCRG C API demos
|
|
155
|
+
demo/viz/ visualization app and notebook
|
|
156
|
+
tests/ pytest test suite
|
|
157
|
+
```
|
|
158
|
+
|
|
159
|
+
## Notes
|
|
160
|
+
|
|
161
|
+
- The binary Belgian-block sample contains all-NaN edge channels that produce expected
|
|
162
|
+
`RuntimeWarning`s from NumPy when computing array summaries; the file still loads and
|
|
163
|
+
is fully covered by tests.
|
|
@@ -0,0 +1,32 @@
|
|
|
1
|
+
[project]
|
|
2
|
+
name = "crgutils"
|
|
3
|
+
version = "0.1.0"
|
|
4
|
+
description = "Add your description here"
|
|
5
|
+
readme = "README.md"
|
|
6
|
+
authors = [
|
|
7
|
+
{ name = "coder", email = "coder@timeintegral.ai" }
|
|
8
|
+
]
|
|
9
|
+
requires-python = ">=3.11"
|
|
10
|
+
dependencies = ["numpy>=1.26"]
|
|
11
|
+
|
|
12
|
+
[dependency-groups]
|
|
13
|
+
dev = [
|
|
14
|
+
"bump-my-version>=1.3.0",
|
|
15
|
+
"pytest>=8.0",
|
|
16
|
+
"pytest-cov",
|
|
17
|
+
"ruff>=0.15.10",
|
|
18
|
+
]
|
|
19
|
+
|
|
20
|
+
[build-system]
|
|
21
|
+
requires = ["uv_build>=0.11.2,<0.12.0"]
|
|
22
|
+
build-backend = "uv_build"
|
|
23
|
+
|
|
24
|
+
[tool.uv.workspace]
|
|
25
|
+
members = [
|
|
26
|
+
"demo/viz",
|
|
27
|
+
"demo/c-api",
|
|
28
|
+
".",
|
|
29
|
+
]
|
|
30
|
+
|
|
31
|
+
[tool.uv]
|
|
32
|
+
default-groups = ["dev"]
|
|
@@ -0,0 +1,38 @@
|
|
|
1
|
+
"""crgutils — Python library for reading and evaluating OpenCRG road surface files.
|
|
2
|
+
|
|
3
|
+
Quick start
|
|
4
|
+
-----------
|
|
5
|
+
>>> import crgutils
|
|
6
|
+
>>> ds = crgutils.read("road.crg") # load + validate + apply modifiers
|
|
7
|
+
>>> cp = ds.create_contact_point()
|
|
8
|
+
>>> z = cp.eval_uv_to_z(10.0, 0.0) # elevation at (u=10, v=0)
|
|
9
|
+
>>> x, y = cp.eval_uv_to_xy(10.0, 0.5) # road → world coords
|
|
10
|
+
>>> u, v = cp.eval_xy_to_uv(x, y) # world → road coords
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
from ._convenience import read
|
|
14
|
+
from ._dataset import CRGDataset
|
|
15
|
+
from ._contact_point import ContactPoint
|
|
16
|
+
from ._types import BorderMode, RefLineContinue, CurvMode, GridNaNMode, EvalOptions
|
|
17
|
+
|
|
18
|
+
# Lower-level building blocks — available for advanced use but not part of the
|
|
19
|
+
# primary public API.
|
|
20
|
+
from ._loader import load # noqa: F401
|
|
21
|
+
from ._check import check # noqa: F401
|
|
22
|
+
from ._modifiers import apply_modifiers # noqa: F401
|
|
23
|
+
|
|
24
|
+
__version__ = "0.1.0"
|
|
25
|
+
|
|
26
|
+
__all__ = [
|
|
27
|
+
# High-level interface
|
|
28
|
+
"read",
|
|
29
|
+
"CRGDataset",
|
|
30
|
+
"ContactPoint",
|
|
31
|
+
# Types / enums
|
|
32
|
+
"BorderMode",
|
|
33
|
+
"RefLineContinue",
|
|
34
|
+
"CurvMode",
|
|
35
|
+
"GridNaNMode",
|
|
36
|
+
"EvalOptions",
|
|
37
|
+
"__version__",
|
|
38
|
+
]
|
|
@@ -0,0 +1,102 @@
|
|
|
1
|
+
"""Validate a CRGDataset for consistency and accuracy.
|
|
2
|
+
|
|
3
|
+
Port of ``crgCheck()`` / ``crgCheckOpts()`` / ``crgCheckData()`` from
|
|
4
|
+
crgLoader.c / crgMgr.c.
|
|
5
|
+
"""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
import math
|
|
10
|
+
import warnings
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
|
|
14
|
+
from ._dataset import CRGDataset
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
def check(dataset: CRGDataset, *, raise_on_error: bool = False) -> bool:
|
|
18
|
+
"""Validate *dataset* for internal consistency.
|
|
19
|
+
|
|
20
|
+
Parameters
|
|
21
|
+
----------
|
|
22
|
+
dataset:
|
|
23
|
+
The dataset to validate.
|
|
24
|
+
raise_on_error:
|
|
25
|
+
If True, raise a ``ValueError`` instead of returning False.
|
|
26
|
+
|
|
27
|
+
Returns
|
|
28
|
+
-------
|
|
29
|
+
bool
|
|
30
|
+
``True`` if all checks pass.
|
|
31
|
+
"""
|
|
32
|
+
errors: list[str] = []
|
|
33
|
+
warnings_: list[str] = []
|
|
34
|
+
|
|
35
|
+
# --- basic shape checks ---
|
|
36
|
+
if dataset.n_u < 2:
|
|
37
|
+
errors.append(f"u axis has fewer than 2 points ({dataset.n_u})")
|
|
38
|
+
if dataset.n_v < 1:
|
|
39
|
+
errors.append(f"v axis has no points ({dataset.n_v})")
|
|
40
|
+
if dataset.z.shape != (dataset.n_v, dataset.n_u):
|
|
41
|
+
errors.append(
|
|
42
|
+
f"z shape {dataset.z.shape} does not match (n_v={dataset.n_v}, n_u={dataset.n_u})"
|
|
43
|
+
)
|
|
44
|
+
|
|
45
|
+
# --- u increment consistency ---
|
|
46
|
+
if dataset.n_u >= 2:
|
|
47
|
+
computed_inc = (dataset.u_max - dataset.u_min) / (dataset.n_u - 1)
|
|
48
|
+
if abs(computed_inc - dataset.u_inc) > 1e-6 * max(1.0, abs(dataset.u_inc)):
|
|
49
|
+
warnings_.append(
|
|
50
|
+
f"u_inc mismatch: stored={dataset.u_inc:.6f}, computed={computed_inc:.6f}"
|
|
51
|
+
)
|
|
52
|
+
|
|
53
|
+
# --- v monotonicity ---
|
|
54
|
+
if dataset.n_v >= 2:
|
|
55
|
+
diffs = np.diff(dataset.v.astype(float))
|
|
56
|
+
if not np.all(diffs > 0):
|
|
57
|
+
errors.append("v array is not strictly increasing")
|
|
58
|
+
|
|
59
|
+
# --- phi continuity (warn on large jumps) ---
|
|
60
|
+
if dataset.n_u >= 2:
|
|
61
|
+
phi_unwrapped = np.unwrap(dataset.phi.astype(float))
|
|
62
|
+
dphi = np.abs(np.diff(phi_unwrapped))
|
|
63
|
+
max_dphi = float(dphi.max()) if len(dphi) else 0.0
|
|
64
|
+
if max_dphi > math.pi / 2:
|
|
65
|
+
warnings_.append(
|
|
66
|
+
f"Large heading jump detected: max |Δphi| = {math.degrees(max_dphi):.1f}°"
|
|
67
|
+
)
|
|
68
|
+
|
|
69
|
+
# --- NaN fraction in z grid ---
|
|
70
|
+
nan_count = int(np.isnan(dataset.z.astype(float)).sum())
|
|
71
|
+
total = dataset.z.size
|
|
72
|
+
if nan_count > 0:
|
|
73
|
+
frac = nan_count / total
|
|
74
|
+
if frac > 0.5:
|
|
75
|
+
errors.append(f"More than 50% of z values are NaN ({nan_count}/{total})")
|
|
76
|
+
elif frac > 0.1:
|
|
77
|
+
warnings_.append(f"{nan_count}/{total} z values are NaN ({100*frac:.1f}%)")
|
|
78
|
+
|
|
79
|
+
# --- reference line x/y length vs u length ---
|
|
80
|
+
if dataset.n_u >= 2:
|
|
81
|
+
dx = np.diff(dataset.x.astype(float))
|
|
82
|
+
dy = np.diff(dataset.y.astype(float))
|
|
83
|
+
seg_lengths = np.sqrt(dx ** 2 + dy ** 2)
|
|
84
|
+
total_xy = float(seg_lengths.sum())
|
|
85
|
+
total_u = dataset.u_max - dataset.u_min
|
|
86
|
+
if total_u > 0 and abs(total_xy - total_u) / total_u > 0.05:
|
|
87
|
+
warnings_.append(
|
|
88
|
+
f"Reference line arc length {total_xy:.3f} m differs from "
|
|
89
|
+
f"u range {total_u:.3f} m by more than 5%"
|
|
90
|
+
)
|
|
91
|
+
|
|
92
|
+
for w in warnings_:
|
|
93
|
+
warnings.warn(f"CRGDataset check: {w}", UserWarning, stacklevel=2)
|
|
94
|
+
|
|
95
|
+
if errors:
|
|
96
|
+
msg = "CRGDataset validation failed:\n" + "\n".join(f" - {e}" for e in errors)
|
|
97
|
+
if raise_on_error:
|
|
98
|
+
raise ValueError(msg)
|
|
99
|
+
warnings.warn(msg, UserWarning, stacklevel=2)
|
|
100
|
+
return False
|
|
101
|
+
|
|
102
|
+
return True
|
|
@@ -0,0 +1,167 @@
|
|
|
1
|
+
"""ContactPoint: stateful evaluation context for a CRGDataset."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from collections import deque
|
|
6
|
+
from typing import Any
|
|
7
|
+
|
|
8
|
+
import numpy as np
|
|
9
|
+
|
|
10
|
+
from ._dataset import CRGDataset
|
|
11
|
+
from ._types import EvalOptions
|
|
12
|
+
from ._eval import (
|
|
13
|
+
_eval_uv_to_z,
|
|
14
|
+
_eval_uv_to_xy,
|
|
15
|
+
_xy_to_uv,
|
|
16
|
+
_eval_uv_to_pk,
|
|
17
|
+
)
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
class ContactPoint:
|
|
21
|
+
"""Evaluation context for a :class:`CRGDataset`.
|
|
22
|
+
|
|
23
|
+
A ``ContactPoint`` binds a dataset to a set of :class:`EvalOptions` and
|
|
24
|
+
maintains a small history deque that accelerates successive nearby queries
|
|
25
|
+
(warm start for the xy→uv search).
|
|
26
|
+
|
|
27
|
+
Multiple ``ContactPoint`` instances on the same dataset are safe to use
|
|
28
|
+
independently (each has its own history and options).
|
|
29
|
+
|
|
30
|
+
Parameters
|
|
31
|
+
----------
|
|
32
|
+
dataset:
|
|
33
|
+
The CRG dataset to evaluate against.
|
|
34
|
+
**options:
|
|
35
|
+
Keyword arguments that override :class:`EvalOptions` defaults.
|
|
36
|
+
Keys must be valid :class:`EvalOptions` field names.
|
|
37
|
+
|
|
38
|
+
Examples
|
|
39
|
+
--------
|
|
40
|
+
>>> import crgutils
|
|
41
|
+
>>> ds = crgutils.load("road.crg")
|
|
42
|
+
>>> cp = crgutils.ContactPoint(ds)
|
|
43
|
+
>>> z = cp.eval_uv_to_z(5.0, 0.0)
|
|
44
|
+
>>> x, y = cp.eval_uv_to_xy(5.0, 0.5)
|
|
45
|
+
>>> u, v = cp.eval_xy_to_uv(x, y)
|
|
46
|
+
"""
|
|
47
|
+
|
|
48
|
+
def __init__(self, dataset: CRGDataset, **options: Any) -> None:
|
|
49
|
+
self._ds = dataset
|
|
50
|
+
# Merge dataset-level default options with caller overrides
|
|
51
|
+
merged = {**dataset.default_options, **options}
|
|
52
|
+
self._options = EvalOptions(**merged)
|
|
53
|
+
self._history: deque[tuple[float, float, int]] = deque(
|
|
54
|
+
maxlen=self._options.history_size
|
|
55
|
+
)
|
|
56
|
+
|
|
57
|
+
# Pre-seed history if ref_line_search_u is set
|
|
58
|
+
if self._options.ref_line_search_u is not None:
|
|
59
|
+
su = self._options.ref_line_search_u
|
|
60
|
+
idx = int((su - dataset.u_min) / dataset.u_inc)
|
|
61
|
+
idx = max(1, min(idx, dataset.n_u - 1))
|
|
62
|
+
x_seed, y_seed = _eval_uv_to_xy(dataset, self._options, su, 0.0)
|
|
63
|
+
self._history.appendleft((x_seed, y_seed, idx))
|
|
64
|
+
|
|
65
|
+
# ------------------------------------------------------------------
|
|
66
|
+
# Factory
|
|
67
|
+
# ------------------------------------------------------------------
|
|
68
|
+
|
|
69
|
+
def with_options(self, **overrides: Any) -> "ContactPoint":
|
|
70
|
+
"""Return a new ``ContactPoint`` with option overrides applied.
|
|
71
|
+
|
|
72
|
+
The new instance shares the same dataset but gets a fresh history.
|
|
73
|
+
"""
|
|
74
|
+
from dataclasses import asdict
|
|
75
|
+
current = asdict(self._options)
|
|
76
|
+
current.update(overrides)
|
|
77
|
+
return ContactPoint(self._ds, **current)
|
|
78
|
+
|
|
79
|
+
# ------------------------------------------------------------------
|
|
80
|
+
# Properties
|
|
81
|
+
# ------------------------------------------------------------------
|
|
82
|
+
|
|
83
|
+
@property
|
|
84
|
+
def options(self) -> EvalOptions:
|
|
85
|
+
return self._options
|
|
86
|
+
|
|
87
|
+
@property
|
|
88
|
+
def dataset(self) -> CRGDataset:
|
|
89
|
+
return self._ds
|
|
90
|
+
|
|
91
|
+
# ------------------------------------------------------------------
|
|
92
|
+
# Primary evaluation methods
|
|
93
|
+
# ------------------------------------------------------------------
|
|
94
|
+
|
|
95
|
+
def eval_uv_to_z(self, u: float, v: float) -> float:
|
|
96
|
+
"""Bilinear elevation at road coordinate (u, v) [m]."""
|
|
97
|
+
return _eval_uv_to_z(self._ds, self._options, u, v)
|
|
98
|
+
|
|
99
|
+
def eval_xy_to_z(self, x: float, y: float) -> float:
|
|
100
|
+
"""Elevation at Cartesian position (x, y) [m]."""
|
|
101
|
+
u, v = _xy_to_uv(self._ds, self._options, x, y, self._history)
|
|
102
|
+
return _eval_uv_to_z(self._ds, self._options, u, v)
|
|
103
|
+
|
|
104
|
+
def eval_uv_to_xy(self, u: float, v: float) -> tuple[float, float]:
|
|
105
|
+
"""Convert road (u, v) to Cartesian (x, y) [m]."""
|
|
106
|
+
return _eval_uv_to_xy(self._ds, self._options, u, v)
|
|
107
|
+
|
|
108
|
+
def eval_xy_to_uv(self, x: float, y: float) -> tuple[float, float]:
|
|
109
|
+
"""Convert Cartesian (x, y) to road (u, v) [m]."""
|
|
110
|
+
return _xy_to_uv(self._ds, self._options, x, y, self._history)
|
|
111
|
+
|
|
112
|
+
def eval_uv_to_pk(self, u: float, v: float) -> tuple[float, float]:
|
|
113
|
+
"""Heading [rad] and curvature [1/m] at road (u, v)."""
|
|
114
|
+
return _eval_uv_to_pk(self._ds, self._options, u, v)
|
|
115
|
+
|
|
116
|
+
def eval_xy_to_pk(self, x: float, y: float) -> tuple[float, float]:
|
|
117
|
+
"""Heading [rad] and curvature [1/m] at Cartesian (x, y)."""
|
|
118
|
+
u, v = _xy_to_uv(self._ds, self._options, x, y, self._history)
|
|
119
|
+
return _eval_uv_to_pk(self._ds, self._options, u, v)
|
|
120
|
+
|
|
121
|
+
# ------------------------------------------------------------------
|
|
122
|
+
# Vectorised convenience methods
|
|
123
|
+
# ------------------------------------------------------------------
|
|
124
|
+
|
|
125
|
+
def eval_uv_to_z_grid(
|
|
126
|
+
self,
|
|
127
|
+
u: "np.ndarray",
|
|
128
|
+
v: "np.ndarray",
|
|
129
|
+
) -> "np.ndarray":
|
|
130
|
+
"""Vectorised elevation evaluation over arrays of (u, v) pairs.
|
|
131
|
+
|
|
132
|
+
Parameters
|
|
133
|
+
----------
|
|
134
|
+
u, v:
|
|
135
|
+
1-D arrays of matching length or broadcastable shapes.
|
|
136
|
+
|
|
137
|
+
Returns
|
|
138
|
+
-------
|
|
139
|
+
np.ndarray
|
|
140
|
+
Elevation values, same shape as broadcast(u, v).
|
|
141
|
+
"""
|
|
142
|
+
u_arr = np.asarray(u, dtype=float).ravel()
|
|
143
|
+
v_arr = np.asarray(v, dtype=float).ravel()
|
|
144
|
+
z_arr = np.empty(len(u_arr), dtype=float)
|
|
145
|
+
for i in range(len(u_arr)):
|
|
146
|
+
z_arr[i] = _eval_uv_to_z(self._ds, self._options, float(u_arr[i]), float(v_arr[i]))
|
|
147
|
+
return z_arr
|
|
148
|
+
|
|
149
|
+
def eval_xy_to_z_grid(
|
|
150
|
+
self,
|
|
151
|
+
x: "np.ndarray",
|
|
152
|
+
y: "np.ndarray",
|
|
153
|
+
) -> "np.ndarray":
|
|
154
|
+
"""Vectorised elevation evaluation over arrays of (x, y) pairs."""
|
|
155
|
+
x_arr = np.asarray(x, dtype=float).ravel()
|
|
156
|
+
y_arr = np.asarray(y, dtype=float).ravel()
|
|
157
|
+
z_arr = np.empty(len(x_arr), dtype=float)
|
|
158
|
+
for i in range(len(x_arr)):
|
|
159
|
+
z_arr[i] = self.eval_xy_to_z(float(x_arr[i]), float(y_arr[i]))
|
|
160
|
+
return z_arr
|
|
161
|
+
|
|
162
|
+
def __repr__(self) -> str:
|
|
163
|
+
return (
|
|
164
|
+
f"ContactPoint(dataset={self._ds.source_file!r}, "
|
|
165
|
+
f"border_mode_u={self._options.border_mode_u.name}, "
|
|
166
|
+
f"border_mode_v={self._options.border_mode_v.name})"
|
|
167
|
+
)
|