colregs-planner 0.1.0__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.
- colregs_planner/__init__.py +22 -0
- colregs_planner/ais/__init__.py +0 -0
- colregs_planner/ais/cleaning.py +166 -0
- colregs_planner/ais/windows.py +52 -0
- colregs_planner/colregs_graph/__init__.py +0 -0
- colregs_planner/colregs_graph/colregs_classifier.py +418 -0
- colregs_planner/colregs_graph/colregs_graph.py +115 -0
- colregs_planner/motion_models/__init__.py +1 -0
- colregs_planner/motion_models/adapters.py +386 -0
- colregs_planner/motion_models/ct_model.py +191 -0
- colregs_planner/motion_models/milliampere_model.py +344 -0
- colregs_planner/motion_models/vikenes_model.py +404 -0
- colregs_planner/nominal/__init__.py +19 -0
- colregs_planner/nominal/indexer.py +166 -0
- colregs_planner/nominal/library.py +144 -0
- colregs_planner/nominal/retriever.py +216 -0
- colregs_planner/planner_api/__init__.py +40 -0
- colregs_planner/planner_api/dp_planner.py +320 -0
- colregs_planner/planner_api/manoeuvre_search.py +519 -0
- colregs_planner/planner_api/manoeuvres.py +48 -0
- colregs_planner/planner_api/schema.py +208 -0
- colregs_planner/planner_api/service.py +687 -0
- colregs_planner/planner_api/tools.py +218 -0
- colregs_planner/policies/__init__.py +1 -0
- colregs_planner/policies/constant.py +70 -0
- colregs_planner/policies/cto.py +452 -0
- colregs_planner/policies/debug_types.py +79 -0
- colregs_planner/policies/predictor_dp.py +52 -0
- colregs_planner/policies/search_dp.py +93 -0
- colregs_planner/policies/vo_analytic.py +777 -0
- colregs_planner/policies/vo_common.py +387 -0
- colregs_planner/policies/vo_kuwata.py +553 -0
- colregs_planner/prediction/__init__.py +1 -0
- colregs_planner/prediction/parallel.py +203 -0
- colregs_planner/prediction/predictor.py +693 -0
- colregs_planner/prediction/responding_filter.py +217 -0
- colregs_planner/prediction/vo_rollout.py +621 -0
- colregs_planner/scenario_config.py +165 -0
- colregs_planner/seachart/__init__.py +0 -0
- colregs_planner/seachart/loader.py +292 -0
- colregs_planner/vessel/__init__.py +2 -0
- colregs_planner/vessel/base.py +85 -0
- colregs_planner/vessel/encounter.py +225 -0
- colregs_planner/vessel/state.py +170 -0
- colregs_planner/visualization/__init__.py +1 -0
- colregs_planner/visualization/live_viz.py +2088 -0
- colregs_planner/visualization/map_background.py +131 -0
- colregs_planner/visualization/multi_panel.py +540 -0
- colregs_planner-0.1.0.dist-info/METADATA +471 -0
- colregs_planner-0.1.0.dist-info/RECORD +53 -0
- colregs_planner-0.1.0.dist-info/WHEEL +5 -0
- colregs_planner-0.1.0.dist-info/licenses/LICENSE +21 -0
- colregs_planner-0.1.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,22 @@
|
|
|
1
|
+
"""colregs_planner: COLREGs-aware multi-vessel trajectory prediction and
|
|
2
|
+
own-ship manoeuvre planning.
|
|
3
|
+
|
|
4
|
+
from colregs_planner import ColregsDPPlanner, Route # DP reference for an own ship
|
|
5
|
+
from colregs_planner import PredictionService # predictions for a planner
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from importlib.metadata import PackageNotFoundError, version
|
|
9
|
+
|
|
10
|
+
from colregs_planner.planner_api import (
|
|
11
|
+
ColregsDPPlanner,
|
|
12
|
+
DPReference,
|
|
13
|
+
PredictionService,
|
|
14
|
+
Route,
|
|
15
|
+
)
|
|
16
|
+
|
|
17
|
+
try:
|
|
18
|
+
__version__ = version("colregs-planner")
|
|
19
|
+
except PackageNotFoundError: # running from a source tree that is not installed
|
|
20
|
+
__version__ = "0.0.0"
|
|
21
|
+
|
|
22
|
+
__all__ = ["ColregsDPPlanner", "DPReference", "PredictionService", "Route", "__version__"]
|
|
File without changes
|
|
@@ -0,0 +1,166 @@
|
|
|
1
|
+
"""colregs_planner/ais/cleaning.py
|
|
2
|
+
|
|
3
|
+
Raw Kystverket AIS parquet -> cleaned, uniformly-resampled per-voyage tracks.
|
|
4
|
+
Shared between scripts/process_raw_ais.py (the encounter-extraction pipeline)
|
|
5
|
+
and colregs_planner/nominal/indexer.py (the nominal track-library builder) so both stay
|
|
6
|
+
in sync without one importing the other as a script.
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import math
|
|
12
|
+
from pathlib import Path
|
|
13
|
+
|
|
14
|
+
import numpy as np
|
|
15
|
+
import pandas as pd
|
|
16
|
+
|
|
17
|
+
_COG_UNAVAILABLE = 360.0
|
|
18
|
+
_HDG_UNAVAILABLE = 511
|
|
19
|
+
_ROT_UNAVAILABLE = -128
|
|
20
|
+
# Default area (lat_min, lat_max, lon_min, lon_max): the Oslofjord study area.
|
|
21
|
+
DEFAULT_BOUNDS: tuple[float, float, float, float] = (59.0, 59.95, 10.2, 10.85)
|
|
22
|
+
|
|
23
|
+
|
|
24
|
+
# ---------------------------------------------------------------------------
|
|
25
|
+
# Load & clean one daily parquet file
|
|
26
|
+
# ---------------------------------------------------------------------------
|
|
27
|
+
|
|
28
|
+
def load_and_clean(
|
|
29
|
+
path: Path,
|
|
30
|
+
bounds: tuple[float, float, float, float] = DEFAULT_BOUNDS,
|
|
31
|
+
) -> pd.DataFrame:
|
|
32
|
+
"""Load one daily parquet file, keeping valid rows inside *bounds*
|
|
33
|
+
(lat_min, lat_max, lon_min, lon_max) [deg]."""
|
|
34
|
+
lat_min, lat_max, lon_min, lon_max = bounds
|
|
35
|
+
df = pd.read_parquet(path)
|
|
36
|
+
df["sog"] = pd.to_numeric(df["speed_over_ground"], errors="coerce")
|
|
37
|
+
df["cog"] = pd.to_numeric(df["course_over_ground"], errors="coerce")
|
|
38
|
+
df.drop(columns=["speed_over_ground", "course_over_ground", "geometry"],
|
|
39
|
+
errors="ignore", inplace=True)
|
|
40
|
+
df.rename(columns={"date_time_utc": "timestamp"}, inplace=True)
|
|
41
|
+
df.loc[df["cog"] >= _COG_UNAVAILABLE, "cog"] = float("nan")
|
|
42
|
+
df.loc[df["true_heading"] == _HDG_UNAVAILABLE, "true_heading"] = float("nan")
|
|
43
|
+
df.loc[df["rate_of_turn"] == _ROT_UNAVAILABLE, "rate_of_turn"] = float("nan")
|
|
44
|
+
if df["timestamp"].dt.tz is not None:
|
|
45
|
+
df["timestamp"] = df["timestamp"].dt.tz_localize(None)
|
|
46
|
+
mmsi = df["mmsi"].astype(int)
|
|
47
|
+
df = df[(mmsi >= 100_000_000) & (mmsi <= 999_999_999)].copy()
|
|
48
|
+
df = df[df["latitude"].between(lat_min, lat_max) &
|
|
49
|
+
df["longitude"].between(lon_min, lon_max)].copy()
|
|
50
|
+
df = df[df["sog"].between(0.0, 50.0)].copy()
|
|
51
|
+
df.sort_values("timestamp", inplace=True)
|
|
52
|
+
df.drop_duplicates(subset=["mmsi", "timestamp"], keep="first", inplace=True)
|
|
53
|
+
return df.reset_index(drop=True)
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
# ---------------------------------------------------------------------------
|
|
57
|
+
# Circular COG interpolation
|
|
58
|
+
# ---------------------------------------------------------------------------
|
|
59
|
+
|
|
60
|
+
def _interp_circular_deg(t_new: np.ndarray, t_old: np.ndarray,
|
|
61
|
+
angles_deg: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
62
|
+
cos_v = np.cos(np.radians(angles_deg))
|
|
63
|
+
sin_v = np.sin(np.radians(angles_deg))
|
|
64
|
+
ci = np.interp(t_new, t_old, cos_v)
|
|
65
|
+
si = np.interp(t_new, t_old, sin_v)
|
|
66
|
+
norms = np.hypot(ci, si)
|
|
67
|
+
norms[norms < 1e-9] = 1.0
|
|
68
|
+
return ci / norms, si / norms
|
|
69
|
+
|
|
70
|
+
|
|
71
|
+
def _fill_nan_nearest(vals: np.ndarray) -> np.ndarray:
|
|
72
|
+
"""Forward/backward fill NaN using nearest valid value."""
|
|
73
|
+
nans = np.isnan(vals)
|
|
74
|
+
if not nans.any() or nans.all():
|
|
75
|
+
return vals
|
|
76
|
+
idx = np.arange(len(vals))
|
|
77
|
+
valid_idx = idx[~nans]
|
|
78
|
+
fill_pos = np.searchsorted(valid_idx, idx[nans])
|
|
79
|
+
fill_pos = np.clip(fill_pos, 0, len(valid_idx) - 1)
|
|
80
|
+
vals = vals.copy()
|
|
81
|
+
vals[nans] = vals[valid_idx[fill_pos]]
|
|
82
|
+
return vals
|
|
83
|
+
|
|
84
|
+
|
|
85
|
+
# ---------------------------------------------------------------------------
|
|
86
|
+
# Trajectory segmentation and resampling
|
|
87
|
+
# ---------------------------------------------------------------------------
|
|
88
|
+
|
|
89
|
+
def resample_track(
|
|
90
|
+
track: pd.DataFrame,
|
|
91
|
+
dt: float,
|
|
92
|
+
gap_threshold: float,
|
|
93
|
+
min_segment_obs: int,
|
|
94
|
+
) -> list[pd.DataFrame]:
|
|
95
|
+
track = track.sort_values("timestamp").reset_index(drop=True)
|
|
96
|
+
ts_unix = (track["timestamp"] - pd.Timestamp("1970-01-01")).dt.total_seconds().values
|
|
97
|
+
diffs = np.diff(ts_unix)
|
|
98
|
+
split_at = list(np.where(diffs > gap_threshold)[0] + 1)
|
|
99
|
+
seg_starts = [0] + split_at
|
|
100
|
+
seg_ends = split_at + [len(track)]
|
|
101
|
+
mmsi_val = int(track["mmsi"].iloc[0])
|
|
102
|
+
result: list[pd.DataFrame] = []
|
|
103
|
+
|
|
104
|
+
for s0, s1 in zip(seg_starts, seg_ends):
|
|
105
|
+
seg = track.iloc[s0:s1].reset_index(drop=True)
|
|
106
|
+
if len(seg) < min_segment_obs:
|
|
107
|
+
continue
|
|
108
|
+
ts_old = ts_unix[s0:s1]
|
|
109
|
+
span = ts_old[-1] - ts_old[0]
|
|
110
|
+
if span < dt:
|
|
111
|
+
continue
|
|
112
|
+
t_grid = np.arange(ts_old[0], ts_old[-1] + 1e-9, dt)
|
|
113
|
+
out: dict = {}
|
|
114
|
+
out["timestamp"] = pd.to_datetime(t_grid, unit="s")
|
|
115
|
+
out["mmsi"] = mmsi_val
|
|
116
|
+
|
|
117
|
+
for col in ["latitude", "longitude", "sog"]:
|
|
118
|
+
vals = seg[col].to_numpy(dtype=float, na_value=float("nan"))
|
|
119
|
+
vals = _fill_nan_nearest(vals)
|
|
120
|
+
out[col] = np.interp(t_grid, ts_old, vals)
|
|
121
|
+
|
|
122
|
+
# COG circular interpolation
|
|
123
|
+
cog_vals = seg["cog"].to_numpy(dtype=float, na_value=float("nan"))
|
|
124
|
+
if np.isnan(cog_vals).all():
|
|
125
|
+
cog_vals = np.zeros(len(seg))
|
|
126
|
+
else:
|
|
127
|
+
cog_vals = _fill_nan_nearest(cog_vals)
|
|
128
|
+
ci, si = _interp_circular_deg(t_grid, ts_old, cog_vals)
|
|
129
|
+
out["cos_hdg"] = ci
|
|
130
|
+
out["sin_hdg"] = si
|
|
131
|
+
out["cog"] = np.degrees(np.arctan2(si, ci)) % 360
|
|
132
|
+
|
|
133
|
+
hdg_rad = np.arctan2(si, ci)
|
|
134
|
+
dh = np.diff(hdg_rad)
|
|
135
|
+
dh = (dh + math.pi) % (2 * math.pi) - math.pi
|
|
136
|
+
out["heading_rate"] = np.concatenate([[0.0], dh / dt])
|
|
137
|
+
|
|
138
|
+
if "true_heading" in seg.columns:
|
|
139
|
+
th = seg["true_heading"].to_numpy(dtype=float, na_value=float("nan"))
|
|
140
|
+
if not np.isnan(th).all():
|
|
141
|
+
th = _fill_nan_nearest(th)
|
|
142
|
+
th_ci, th_si = _interp_circular_deg(t_grid, ts_old, th)
|
|
143
|
+
out["true_heading"] = np.degrees(np.arctan2(th_si, th_ci)) % 360
|
|
144
|
+
else:
|
|
145
|
+
out["true_heading"] = np.full(len(t_grid), float("nan"))
|
|
146
|
+
|
|
147
|
+
for col in ["status", "maneuvre", "msg_type"]:
|
|
148
|
+
if col in seg.columns:
|
|
149
|
+
out[col] = np.interp(
|
|
150
|
+
t_grid, ts_old,
|
|
151
|
+
seg[col].to_numpy(dtype=float, na_value=0.0)
|
|
152
|
+
)
|
|
153
|
+
if "rate_of_turn" in seg.columns:
|
|
154
|
+
vals = seg["rate_of_turn"].to_numpy(dtype=float, na_value=float("nan"))
|
|
155
|
+
vals = _fill_nan_nearest(vals) if not np.isnan(vals).all() else np.zeros_like(vals)
|
|
156
|
+
out["rate_of_turn"] = np.interp(t_grid, ts_old, vals)
|
|
157
|
+
for col in ["data_source", "ais_class"]:
|
|
158
|
+
if col in seg.columns:
|
|
159
|
+
out[col] = seg[col].iloc[0]
|
|
160
|
+
|
|
161
|
+
original_ts = set(np.round(ts_old / dt).astype(int))
|
|
162
|
+
grid_r = np.round(t_grid / dt).astype(int)
|
|
163
|
+
out["interpolated"] = ~np.isin(grid_r, list(original_ts))
|
|
164
|
+
result.append(pd.DataFrame(out))
|
|
165
|
+
|
|
166
|
+
return result
|
|
@@ -0,0 +1,52 @@
|
|
|
1
|
+
"""colregs_planner/ais/windows.py
|
|
2
|
+
|
|
3
|
+
Helpers for the encounter windows written by scripts/process_raw_ais.py.
|
|
4
|
+
"""
|
|
5
|
+
|
|
6
|
+
from __future__ import annotations
|
|
7
|
+
|
|
8
|
+
import math
|
|
9
|
+
|
|
10
|
+
from colregs_planner.vessel.state import VesselState
|
|
11
|
+
|
|
12
|
+
_KNOTS_TO_MS = 0.5144
|
|
13
|
+
|
|
14
|
+
|
|
15
|
+
def window_to_frames(w: dict) -> list[dict[str, VesselState]]:
|
|
16
|
+
"""Convert a window to a per-timestep list of {str(mmsi): VesselState}."""
|
|
17
|
+
mmsi_list = sorted(w["vessels"].keys())
|
|
18
|
+
n_steps = len(next(iter(w["vessels"].values())))
|
|
19
|
+
frames: list[dict[str, VesselState]] = []
|
|
20
|
+
for t in range(n_steps):
|
|
21
|
+
frame: dict[str, VesselState] = {}
|
|
22
|
+
for mmsi in mmsi_list:
|
|
23
|
+
df = w["vessels"][mmsi]
|
|
24
|
+
if t >= len(df):
|
|
25
|
+
continue
|
|
26
|
+
row = df.iloc[t]
|
|
27
|
+
frame[str(mmsi)] = VesselState(
|
|
28
|
+
north=float(row["north"]),
|
|
29
|
+
east=float(row["east"]),
|
|
30
|
+
psi=math.atan2(float(row["sin_hdg"]), float(row["cos_hdg"])),
|
|
31
|
+
speed=float(row["sog"]) * _KNOTS_TO_MS,
|
|
32
|
+
r=float(row.get("heading_rate", 0.0)),
|
|
33
|
+
)
|
|
34
|
+
if frame:
|
|
35
|
+
frames.append(frame)
|
|
36
|
+
return frames
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
def traj_limits(
|
|
40
|
+
frames: list[dict[str, VesselState]], padding: float = 300.0
|
|
41
|
+
) -> tuple[tuple[float, float], tuple[float, float]]:
|
|
42
|
+
"""Square (east, north) plot limits around every position in frames."""
|
|
43
|
+
all_n = [s.north for f in frames for s in f.values()]
|
|
44
|
+
all_e = [s.east for f in frames for s in f.values()]
|
|
45
|
+
n_min, n_max = min(all_n) - padding, max(all_n) + padding
|
|
46
|
+
e_min, e_max = min(all_e) - padding, max(all_e) + padding
|
|
47
|
+
nr, er = n_max - n_min, e_max - e_min
|
|
48
|
+
if nr > er:
|
|
49
|
+
d = (nr - er) / 2; e_min -= d; e_max += d
|
|
50
|
+
else:
|
|
51
|
+
d = (er - nr) / 2; n_min -= d; n_max += d
|
|
52
|
+
return (e_min, e_max), (n_min, n_max)
|
|
File without changes
|
|
@@ -0,0 +1,418 @@
|
|
|
1
|
+
"""
|
|
2
|
+
colregs_classifier.py
|
|
3
|
+
|
|
4
|
+
Observation-driven COLREGs scenario classifier.
|
|
5
|
+
|
|
6
|
+
Produces a ColregsGraph from a dict of observed VesselStates, using the
|
|
7
|
+
Temporally Aware Geometric COLREG Scenario Classification (TAG-CSC) approach
|
|
8
|
+
of Gleeson et al. (2024), adapted for the symmetric multi-vessel graph
|
|
9
|
+
representation used in this project.
|
|
10
|
+
|
|
11
|
+
Classification pipeline (per unordered pair {a, b}):
|
|
12
|
+
1. DCPA gate — pairs with DCPA > dcpa_max get no edge
|
|
13
|
+
2. Geometric lookup — (psi, beta) region table → EncounterType
|
|
14
|
+
3. Temporal vote — forward-project to CPA, take priority-weighted mode
|
|
15
|
+
4. Hysteresis — maintain active scenario until vessels are d_u apart
|
|
16
|
+
5. Role assignment — map EncounterType to (role_a, role_b) via ENCOUNTER_ROLES
|
|
17
|
+
|
|
18
|
+
Coordinate convention: NED, heading in radians CW from North.
|
|
19
|
+
|
|
20
|
+
Usage
|
|
21
|
+
-----
|
|
22
|
+
classifier = ColregsClassifier()
|
|
23
|
+
while True:
|
|
24
|
+
states: dict[int, VesselState] = get_observations()
|
|
25
|
+
graph: ColregsGraph = classifier.update(states)
|
|
26
|
+
"""
|
|
27
|
+
|
|
28
|
+
from __future__ import annotations
|
|
29
|
+
|
|
30
|
+
import math
|
|
31
|
+
from bisect import bisect_right
|
|
32
|
+
from dataclasses import dataclass, field
|
|
33
|
+
from typing import Optional
|
|
34
|
+
|
|
35
|
+
from colregs_planner.vessel.state import VesselState
|
|
36
|
+
from colregs_planner.vessel.encounter import EncounterType
|
|
37
|
+
from colregs_planner.colregs_graph.colregs_graph import ColregsGraph, ColregsEdge, ENCOUNTER_ROLES
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
# ---------------------------------------------------------------------------
|
|
41
|
+
# Geometric region table (Gleeson et al. 2024, Table 1)
|
|
42
|
+
#
|
|
43
|
+
# Regions are defined in terms of:
|
|
44
|
+
# psi = theta_TS - theta_OS (relative course, degrees)
|
|
45
|
+
# beta = bearing_to_TS - theta_OS (relative bearing, degrees)
|
|
46
|
+
#
|
|
47
|
+
# Each entry is a list of (psi_min, psi_max, beta_min, beta_max) rectangles.
|
|
48
|
+
# Membership: psi_min <= psi < psi_max AND beta_min <= beta < beta_max.
|
|
49
|
+
# The scenario label is always from vessel_a's perspective (a = OS, b = TS).
|
|
50
|
+
# ---------------------------------------------------------------------------
|
|
51
|
+
|
|
52
|
+
_REGIONS: dict[EncounterType, list[tuple[float, float, float, float]]] = {
|
|
53
|
+
EncounterType.HEAD_ON: [
|
|
54
|
+
( 135, 180, -45, 45),
|
|
55
|
+
(-180, -135, -45, 45),
|
|
56
|
+
],
|
|
57
|
+
EncounterType.CROSSING_STARBOARD: [ # vessel_a gives way
|
|
58
|
+
(-180, -64, 45, 180),
|
|
59
|
+
( -64, 0, 67.5, 112.5),
|
|
60
|
+
(-135, -64, -30, 45),
|
|
61
|
+
],
|
|
62
|
+
EncounterType.CROSSING_PORT: [ # vessel_a stands on
|
|
63
|
+
( 66, 180, -180, -45),
|
|
64
|
+
( 0, 66, -112.5, -67.5),
|
|
65
|
+
( 66, 135, -45, 30),
|
|
66
|
+
],
|
|
67
|
+
EncounterType.OVERTAKING_AHEAD: [ # vessel_a is overtaking (gives way)
|
|
68
|
+
(-65, 66, -67.5, 67.5),
|
|
69
|
+
],
|
|
70
|
+
EncounterType.OVERTAKING_ASTERN: [ # vessel_a is being overtaken (stands on)
|
|
71
|
+
(-65, 66, -180, -112.5),
|
|
72
|
+
(-65, 66, 112.5, 180),
|
|
73
|
+
],
|
|
74
|
+
}
|
|
75
|
+
|
|
76
|
+
# Priority for tie-breaking in temporal vote (lower = higher priority).
|
|
77
|
+
# Give-way scenarios are higher priority: failing to give way is more dangerous.
|
|
78
|
+
# (Gleeson et al. 2024, Table 2)
|
|
79
|
+
_PRIORITY: dict[EncounterType, int] = {
|
|
80
|
+
EncounterType.CROSSING_STARBOARD: 1,
|
|
81
|
+
EncounterType.OVERTAKING_AHEAD: 2,
|
|
82
|
+
EncounterType.HEAD_ON: 3,
|
|
83
|
+
EncounterType.CROSSING_PORT: 4,
|
|
84
|
+
EncounterType.OVERTAKING_ASTERN: 5,
|
|
85
|
+
}
|
|
86
|
+
|
|
87
|
+
|
|
88
|
+
# ---------------------------------------------------------------------------
|
|
89
|
+
# Region lookup table
|
|
90
|
+
#
|
|
91
|
+
# Every rectangle edge in _REGIONS is a breakpoint, so the (psi, beta) plane
|
|
92
|
+
# splits into a grid of cells that each lie entirely inside or outside every
|
|
93
|
+
# region. bisect_right on the sorted breakpoints gives the cell index exactly
|
|
94
|
+
# (half-open [min, max) matches the region membership rule), and the table
|
|
95
|
+
# holds the first-matching scenario per cell, preserving _REGIONS order as the
|
|
96
|
+
# tie-break where rectangles overlap. Index 0 is the cell below all edges.
|
|
97
|
+
# ---------------------------------------------------------------------------
|
|
98
|
+
|
|
99
|
+
_PSI_EDGES: list[float] = sorted({e for rs in _REGIONS.values() for r in rs for e in r[:2]})
|
|
100
|
+
_BETA_EDGES: list[float] = sorted({e for rs in _REGIONS.values() for r in rs for e in r[2:]})
|
|
101
|
+
|
|
102
|
+
|
|
103
|
+
def _region_lookup(psi_deg: float, beta_deg: float) -> Optional[EncounterType]:
|
|
104
|
+
"""First scenario in _REGIONS whose rectangles contain (psi, beta), or None."""
|
|
105
|
+
for scenario, rects in _REGIONS.items():
|
|
106
|
+
for psi_min, psi_max, beta_min, beta_max in rects:
|
|
107
|
+
if psi_min <= psi_deg < psi_max and beta_min <= beta_deg < beta_max:
|
|
108
|
+
return scenario
|
|
109
|
+
return None
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
def _build_table() -> list[list[Optional[EncounterType]]]:
|
|
113
|
+
# Each cell is sampled at its lower-left corner, which lies inside the
|
|
114
|
+
# cell; the out-of-range cells (index 0) map to None.
|
|
115
|
+
return [
|
|
116
|
+
[_region_lookup(p, b) if i and j else None
|
|
117
|
+
for j, b in enumerate([-math.inf] + _BETA_EDGES)]
|
|
118
|
+
for i, p in enumerate([-math.inf] + _PSI_EDGES)
|
|
119
|
+
]
|
|
120
|
+
|
|
121
|
+
|
|
122
|
+
_TABLE = _build_table()
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
def _geometric_classify(psi_deg: float, beta_deg: float) -> Optional[EncounterType]:
|
|
126
|
+
"""Return EncounterType from vessel_a's perspective, or None."""
|
|
127
|
+
return _TABLE[bisect_right(_PSI_EDGES, psi_deg)][bisect_right(_BETA_EDGES, beta_deg)]
|
|
128
|
+
|
|
129
|
+
|
|
130
|
+
# ---------------------------------------------------------------------------
|
|
131
|
+
# Geometry helpers (NED, radians CW from North)
|
|
132
|
+
# ---------------------------------------------------------------------------
|
|
133
|
+
|
|
134
|
+
_TWO_PI = 2 * math.pi
|
|
135
|
+
_RAD2DEG = 180.0 / math.pi
|
|
136
|
+
|
|
137
|
+
|
|
138
|
+
def _wrap_deg(angle: float) -> float:
|
|
139
|
+
"""Wrap angle [rad] to [-pi, pi) and convert to degrees."""
|
|
140
|
+
return ((angle + math.pi) % _TWO_PI - math.pi) * _RAD2DEG
|
|
141
|
+
|
|
142
|
+
|
|
143
|
+
def _compute_dcpa_tcpa(a: VesselState, b: VesselState) -> tuple[float, float]:
|
|
144
|
+
"""
|
|
145
|
+
Closest Point of Approach distance [m] and time [s].
|
|
146
|
+
|
|
147
|
+
tcpa < 0 means vessels are already diverging.
|
|
148
|
+
"""
|
|
149
|
+
dn = b.north - a.north
|
|
150
|
+
de = b.east - a.east
|
|
151
|
+
dvn = float(b.vn - a.vn)
|
|
152
|
+
dve = float(b.ve - a.ve)
|
|
153
|
+
|
|
154
|
+
speed_sq = dvn * dvn + dve * dve
|
|
155
|
+
if speed_sq < 1e-6:
|
|
156
|
+
return math.hypot(dn, de), math.inf
|
|
157
|
+
|
|
158
|
+
tcpa = -(dn * dvn + de * dve) / speed_sq
|
|
159
|
+
dcpa = math.hypot(dn + tcpa * dvn, de + tcpa * dve)
|
|
160
|
+
return dcpa, tcpa
|
|
161
|
+
|
|
162
|
+
|
|
163
|
+
# ---------------------------------------------------------------------------
|
|
164
|
+
# Per-pair hysteresis state
|
|
165
|
+
# ---------------------------------------------------------------------------
|
|
166
|
+
|
|
167
|
+
@dataclass
|
|
168
|
+
class _PairState:
|
|
169
|
+
active_scenario: Optional[EncounterType] = None
|
|
170
|
+
hysteresis_score: float = 0.0 # in [0, 1]; scenario switches when it hits 0
|
|
171
|
+
clear_count: int = 0 # consecutive diverging steps; clears when >= n_clear
|
|
172
|
+
|
|
173
|
+
|
|
174
|
+
# ---------------------------------------------------------------------------
|
|
175
|
+
# Classifier
|
|
176
|
+
# ---------------------------------------------------------------------------
|
|
177
|
+
|
|
178
|
+
@dataclass
|
|
179
|
+
class ColregsClassifier:
|
|
180
|
+
"""
|
|
181
|
+
Stateful COLREGs classifier. Call update() once per timestep.
|
|
182
|
+
|
|
183
|
+
Parameters
|
|
184
|
+
----------
|
|
185
|
+
dcpa_max : float
|
|
186
|
+
DCPA gate threshold [m]. Pairs with DCPA > dcpa_max get no edge.
|
|
187
|
+
d_a : float
|
|
188
|
+
Maximum range at which a new scenario can be detected [m].
|
|
189
|
+
d_u : float
|
|
190
|
+
Range below which an active scenario is discarded once vessels
|
|
191
|
+
are separating [m].
|
|
192
|
+
n_t : int
|
|
193
|
+
Number of time steps for temporal projection (Gleeson §2.1.2).
|
|
194
|
+
w_reinforce : float
|
|
195
|
+
Hysteresis weight added per timestep when scenario is stable.
|
|
196
|
+
w_forget : float
|
|
197
|
+
Hysteresis weight subtracted per timestep when scenario changes.
|
|
198
|
+
Default 0.0 gives latching behaviour (scenario never forgotten
|
|
199
|
+
until distance check clears it).
|
|
200
|
+
n_clear : int
|
|
201
|
+
Number of consecutive diverging+beyond-d_u steps required before
|
|
202
|
+
an active scenario is cleared. Prevents noisy velocity from
|
|
203
|
+
flickering the tcpa sign and resetting the scenario prematurely.
|
|
204
|
+
|
|
205
|
+
A confirmed overtaking encounter is held until the overtaking vessel is
|
|
206
|
+
past the overtaken vessel's beam (COLREGs Rule 13(d): a later change of
|
|
207
|
+
bearing does not end the duty to keep clear, or turn the encounter into a
|
|
208
|
+
crossing, until she is finally past and clear). Until then it is neither
|
|
209
|
+
cleared nor reclassified, unless the vessels are more than d_a apart.
|
|
210
|
+
Without this, an overtaking vessel that turns away hard makes the TCPA
|
|
211
|
+
go negative and loses the encounter mid-overtake.
|
|
212
|
+
"""
|
|
213
|
+
|
|
214
|
+
dcpa_max: float = 1500.0
|
|
215
|
+
d_a: float = 10000.0
|
|
216
|
+
d_u: float = 150.0
|
|
217
|
+
n_t: int = 11
|
|
218
|
+
w_reinforce: float = 1.0
|
|
219
|
+
w_forget: float = 0.01
|
|
220
|
+
n_clear: int = 5
|
|
221
|
+
|
|
222
|
+
_pair_states: dict[tuple[int, int], _PairState] = field(
|
|
223
|
+
default_factory=dict, init=False, repr=False
|
|
224
|
+
)
|
|
225
|
+
|
|
226
|
+
# ------------------------------------------------------------------
|
|
227
|
+
# Public API
|
|
228
|
+
# ------------------------------------------------------------------
|
|
229
|
+
|
|
230
|
+
def update(self, states: dict[int, VesselState]) -> ColregsGraph:
|
|
231
|
+
"""
|
|
232
|
+
Classify all vessel pairs and return a ColregsGraph.
|
|
233
|
+
|
|
234
|
+
Parameters
|
|
235
|
+
----------
|
|
236
|
+
states : dict[int, VesselState]
|
|
237
|
+
Current observed vessel states keyed by vessel index.
|
|
238
|
+
"""
|
|
239
|
+
graph = ColregsGraph()
|
|
240
|
+
ids = sorted(states.keys())
|
|
241
|
+
|
|
242
|
+
for i, id_a in enumerate(ids):
|
|
243
|
+
for id_b in ids[i + 1:]:
|
|
244
|
+
key = (id_a, id_b) # always id_a < id_b (sorted above)
|
|
245
|
+
if key not in self._pair_states:
|
|
246
|
+
self._pair_states[key] = _PairState()
|
|
247
|
+
|
|
248
|
+
edge = self._classify_pair(
|
|
249
|
+
id_a, states[id_a],
|
|
250
|
+
id_b, states[id_b],
|
|
251
|
+
self._pair_states[key],
|
|
252
|
+
)
|
|
253
|
+
if edge is not None:
|
|
254
|
+
graph.add_edge(edge)
|
|
255
|
+
|
|
256
|
+
return graph
|
|
257
|
+
|
|
258
|
+
def reset(self) -> None:
|
|
259
|
+
"""Clear all hysteresis state (e.g. between episodes)."""
|
|
260
|
+
self._pair_states.clear()
|
|
261
|
+
|
|
262
|
+
# ------------------------------------------------------------------
|
|
263
|
+
# Internal: single pair
|
|
264
|
+
# ------------------------------------------------------------------
|
|
265
|
+
|
|
266
|
+
def _classify_pair(
|
|
267
|
+
self,
|
|
268
|
+
id_a: int, a: VesselState,
|
|
269
|
+
id_b: int, b: VesselState,
|
|
270
|
+
ps: _PairState,
|
|
271
|
+
) -> Optional[ColregsEdge]:
|
|
272
|
+
|
|
273
|
+
# Relative position / velocity of b w.r.t. a, and CPA (tcpa < 0 means
|
|
274
|
+
# the vessels are already diverging).
|
|
275
|
+
dn = b.north - a.north
|
|
276
|
+
de = b.east - a.east
|
|
277
|
+
dvn = b.speed * math.cos(b.psi) - a.speed * math.cos(a.psi)
|
|
278
|
+
dve = b.speed * math.sin(b.psi) - a.speed * math.sin(a.psi)
|
|
279
|
+
dist = math.hypot(dn, de)
|
|
280
|
+
speed_sq = dvn * dvn + dve * dve
|
|
281
|
+
if speed_sq < 1e-6:
|
|
282
|
+
dcpa, tcpa = dist, math.inf
|
|
283
|
+
else:
|
|
284
|
+
tcpa = -(dn * dvn + de * dve) / speed_sq
|
|
285
|
+
dcpa = math.hypot(dn + tcpa * dvn, de + tcpa * dve)
|
|
286
|
+
approaching = tcpa > 0
|
|
287
|
+
overtaking = self._overtaking_in_progress(a, b, dn, de, dist, ps)
|
|
288
|
+
|
|
289
|
+
# --- Hysteresis: clear active scenario if vessels are separating,
|
|
290
|
+
# beyond the discard distance, AND the backward-projected CPA also
|
|
291
|
+
# indicates they have truly parted (dcpa > d_u). The dcpa check
|
|
292
|
+
# prevents premature clearing when ego's manoeuvre makes the
|
|
293
|
+
# current-velocity TCPA go negative mid-encounter while the vessels
|
|
294
|
+
# haven't yet physically separated beyond d_u.
|
|
295
|
+
if ps.active_scenario is not None:
|
|
296
|
+
if dist > self.d_u and not approaching and dcpa > self.d_u and not overtaking:
|
|
297
|
+
ps.clear_count += 1
|
|
298
|
+
if ps.clear_count >= self.n_clear:
|
|
299
|
+
ps.active_scenario = None
|
|
300
|
+
ps.hysteresis_score = 0.0
|
|
301
|
+
ps.clear_count = 0
|
|
302
|
+
else:
|
|
303
|
+
ps.clear_count = 0
|
|
304
|
+
|
|
305
|
+
# --- Overtaking: hold the encounter as it is until the overtaking
|
|
306
|
+
# vessel is past (Rule 13(d)); bearing changes do not reclassify it.
|
|
307
|
+
if overtaking:
|
|
308
|
+
return self._active_edge(id_a, id_b, ps)
|
|
309
|
+
|
|
310
|
+
# --- DCPA gate: suppress new encounter detection when projected CPA is
|
|
311
|
+
# very large, but always preserve the already-active edge so the
|
|
312
|
+
# starboard constraint is not transiently dropped.
|
|
313
|
+
if dcpa > self.dcpa_max:
|
|
314
|
+
return self._active_edge(id_a, id_b, ps)
|
|
315
|
+
|
|
316
|
+
# --- Range / approach gate: only classify pairs within d_a and closing
|
|
317
|
+
if dist > self.d_a or not approaching:
|
|
318
|
+
return self._active_edge(id_a, id_b, ps)
|
|
319
|
+
|
|
320
|
+
# --- Temporally aware geometric classification
|
|
321
|
+
scenario = self._temporal_classify(a.psi, b.psi, dn, de, dvn, dve, tcpa)
|
|
322
|
+
if scenario is None:
|
|
323
|
+
return self._active_edge(id_a, id_b, ps)
|
|
324
|
+
|
|
325
|
+
# --- Hysteresis update
|
|
326
|
+
self._update_hysteresis(ps, scenario)
|
|
327
|
+
return self._active_edge(id_a, id_b, ps)
|
|
328
|
+
|
|
329
|
+
def _overtaking_in_progress(
|
|
330
|
+
self, a: VesselState, b: VesselState, dn: float, de: float, dist: float,
|
|
331
|
+
ps: _PairState,
|
|
332
|
+
) -> bool:
|
|
333
|
+
"""True while a confirmed overtaking encounter is not yet past: the
|
|
334
|
+
overtaking vessel is still abaft the overtaken vessel's beam, and the
|
|
335
|
+
vessels are within d_a. (dn, de) is b's position relative to a."""
|
|
336
|
+
if dist > self.d_a or ps.hysteresis_score < self.w_reinforce:
|
|
337
|
+
return False
|
|
338
|
+
if ps.active_scenario == EncounterType.OVERTAKING_AHEAD: # a overtakes b
|
|
339
|
+
along = -(dn * math.cos(b.psi) + de * math.sin(b.psi))
|
|
340
|
+
elif ps.active_scenario == EncounterType.OVERTAKING_ASTERN: # b overtakes a
|
|
341
|
+
along = dn * math.cos(a.psi) + de * math.sin(a.psi)
|
|
342
|
+
else:
|
|
343
|
+
return False
|
|
344
|
+
return along < 0.0
|
|
345
|
+
|
|
346
|
+
def _active_edge(
|
|
347
|
+
self, id_a: int, id_b: int, ps: _PairState
|
|
348
|
+
) -> Optional[ColregsEdge]:
|
|
349
|
+
if ps.active_scenario is None:
|
|
350
|
+
return None
|
|
351
|
+
role_a, role_b = ENCOUNTER_ROLES[ps.active_scenario]
|
|
352
|
+
return ColregsEdge(
|
|
353
|
+
vessel_a=id_a, vessel_b=id_b,
|
|
354
|
+
role_a=role_a, role_b=role_b,
|
|
355
|
+
encounter_type=ps.active_scenario,
|
|
356
|
+
)
|
|
357
|
+
|
|
358
|
+
# ------------------------------------------------------------------
|
|
359
|
+
# Temporal classification (Gleeson §2.1.2)
|
|
360
|
+
# ------------------------------------------------------------------
|
|
361
|
+
|
|
362
|
+
def _temporal_classify(
|
|
363
|
+
self,
|
|
364
|
+
psi_a: float, psi_b: float,
|
|
365
|
+
dn: float, de: float,
|
|
366
|
+
dvn: float, dve: float,
|
|
367
|
+
tcpa: float,
|
|
368
|
+
) -> Optional[EncounterType]:
|
|
369
|
+
"""
|
|
370
|
+
Linearly project both vessels to CPA, classify at n_t equally spaced
|
|
371
|
+
steps, and return the priority-weighted mode scenario label.
|
|
372
|
+
|
|
373
|
+
(dn, de) and (dvn, dve) are b's position and velocity relative to a.
|
|
374
|
+
Under constant-velocity projection the relative course psi is fixed,
|
|
375
|
+
so only the relative bearing beta(t) of dn + t*dvn, de + t*dve varies.
|
|
376
|
+
The DCPA is also invariant along the projection (same relative line),
|
|
377
|
+
so the caller's DCPA gate covers every projected step.
|
|
378
|
+
"""
|
|
379
|
+
row = _TABLE[bisect_right(_PSI_EDGES, _wrap_deg(psi_b - psi_a))]
|
|
380
|
+
beta0 = _wrap_deg(math.atan2(de, dn) - psi_a)
|
|
381
|
+
|
|
382
|
+
if not math.isfinite(tcpa) or tcpa <= 0:
|
|
383
|
+
return row[bisect_right(_BETA_EDGES, beta0)]
|
|
384
|
+
|
|
385
|
+
n = self.n_t - 1
|
|
386
|
+
votes: dict[EncounterType, int] = {}
|
|
387
|
+
|
|
388
|
+
for k in range(self.n_t):
|
|
389
|
+
t = tcpa * k / n
|
|
390
|
+
beta = _wrap_deg(math.atan2(de + t * dve, dn + t * dvn) - psi_a)
|
|
391
|
+
label = row[bisect_right(_BETA_EDGES, beta)]
|
|
392
|
+
if label is not None:
|
|
393
|
+
votes[label] = votes.get(label, 0) + 1
|
|
394
|
+
|
|
395
|
+
if not votes:
|
|
396
|
+
return row[bisect_right(_BETA_EDGES, beta0)]
|
|
397
|
+
|
|
398
|
+
max_votes = max(votes.values())
|
|
399
|
+
candidates = [s for s, v in votes.items() if v == max_votes]
|
|
400
|
+
return min(candidates, key=lambda s: _PRIORITY[s])
|
|
401
|
+
|
|
402
|
+
# ------------------------------------------------------------------
|
|
403
|
+
# Hysteresis (Gleeson §2.1.4)
|
|
404
|
+
# ------------------------------------------------------------------
|
|
405
|
+
|
|
406
|
+
def _update_hysteresis(self, ps: _PairState, new_scenario: EncounterType) -> None:
|
|
407
|
+
if ps.active_scenario is None:
|
|
408
|
+
ps.active_scenario = new_scenario
|
|
409
|
+
ps.hysteresis_score = 0.0
|
|
410
|
+
return
|
|
411
|
+
|
|
412
|
+
if new_scenario == ps.active_scenario:
|
|
413
|
+
ps.hysteresis_score = min(1.0, ps.hysteresis_score + self.w_reinforce)
|
|
414
|
+
else:
|
|
415
|
+
ps.hysteresis_score = max(0.0, ps.hysteresis_score - self.w_forget)
|
|
416
|
+
if ps.hysteresis_score <= 0.0:
|
|
417
|
+
ps.active_scenario = new_scenario
|
|
418
|
+
ps.hysteresis_score = 0.0
|