118 lines
4.8 KiB
Python
118 lines
4.8 KiB
Python
"""Published, algorithm-scoped GOOSE VLS-128 ground profile."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from dataclasses import dataclass
|
|
from typing import Final
|
|
|
|
GOOSE_PATCHWORK_PROFILE_SCHEMA: Final = "missioncore.goose-patchwork-profile/v1"
|
|
GOOSE_PAPER_URL: Final = "https://arxiv.org/pdf/2310.16788"
|
|
GOOSE_PAPER_SHA256: Final = "cb17f38c3ae498917966a63bc0e034cc5dd3fa4ad35873b121592f99026cc77e"
|
|
GOOSE_TF_DOT_URL: Final = "https://goose-dataset.de/docs/resources/mucar3_tf/mucar3_tf.dot"
|
|
GOOSE_TF_DOT_SHA256: Final = "1b10e905bcff17b90305ff02734b5e6bb7f5753d9b8e74cd7c43f3d0f32ef469"
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class GoosePatchworkProfile:
|
|
"""Narrow admission profile for one native GOOSE scan.
|
|
|
|
Figure 3 of the official paper dimensions ``base_link``/INS at 0.64 m
|
|
above ground and the VLS-128 optical center 1.60 m above ``base_link``.
|
|
Their sum is the physical height Patchwork++ requires. This does not claim
|
|
a complete vehicle mounting transform and cannot produce a normalized scan.
|
|
"""
|
|
|
|
profile_id: str = "goose-mucar3-vls128-patchwork/v1"
|
|
base_link_height_above_ground_m: float = 0.64
|
|
lidar_height_above_base_link_m: float = 1.60
|
|
patchwork_sensor_height_proxy_m: float = 2.24
|
|
patchwork_minimum_range_m: float = 2.7
|
|
patchwork_maximum_range_m: float = 80.0
|
|
patchwork_height_evidence: str = "published-dimensioned-schematic"
|
|
|
|
def __post_init__(self) -> None:
|
|
values = (
|
|
self.base_link_height_above_ground_m,
|
|
self.lidar_height_above_base_link_m,
|
|
self.patchwork_sensor_height_proxy_m,
|
|
self.patchwork_minimum_range_m,
|
|
self.patchwork_maximum_range_m,
|
|
)
|
|
if (
|
|
not self.profile_id
|
|
or not all(math.isfinite(value) for value in values)
|
|
or self.base_link_height_above_ground_m <= 0
|
|
or self.lidar_height_above_base_link_m <= 0
|
|
or not math.isclose(
|
|
self.base_link_height_above_ground_m + self.lidar_height_above_base_link_m,
|
|
self.patchwork_sensor_height_proxy_m,
|
|
abs_tol=1e-9,
|
|
)
|
|
or not 0 < self.patchwork_minimum_range_m < self.patchwork_maximum_range_m
|
|
or self.patchwork_height_evidence != "published-dimensioned-schematic"
|
|
):
|
|
raise ValueError("GOOSE Patchwork++ profile is invalid")
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"schema_version": GOOSE_PATCHWORK_PROFILE_SCHEMA,
|
|
"profile_id": self.profile_id,
|
|
"source_id": "goose-3d/v2025-08-22",
|
|
"representation": "native-scan",
|
|
"sensor_frame": {
|
|
"frame_id": "sensor/lidar/vls128_roof",
|
|
"handedness": "right",
|
|
"x": "forward",
|
|
"y": "left",
|
|
"z": "up",
|
|
"one_revolution": True,
|
|
},
|
|
"height": {
|
|
"base_link_above_ground_m": self.base_link_height_above_ground_m,
|
|
"lidar_above_base_link_m": self.lidar_height_above_base_link_m,
|
|
"sensor_above_ground_m": self.patchwork_sensor_height_proxy_m,
|
|
"evidence": self.patchwork_height_evidence,
|
|
},
|
|
"patchworkpp": {
|
|
"provider_id": "patchworkpp/v1.4.1",
|
|
"minimum_range_m": self.patchwork_minimum_range_m,
|
|
"maximum_range_m": self.patchwork_maximum_range_m,
|
|
"enable_rnr": True,
|
|
"enable_rvpf": True,
|
|
"enable_tgr": True,
|
|
},
|
|
"evidence": {
|
|
"dimensioned_schematic": {
|
|
"url": GOOSE_PAPER_URL,
|
|
"sha256": GOOSE_PAPER_SHA256,
|
|
"paper_version": "arxiv-2310.16788v2",
|
|
"figure": 3,
|
|
},
|
|
"tf_topology": {
|
|
"url": GOOSE_TF_DOT_URL,
|
|
"sha256": GOOSE_TF_DOT_SHA256,
|
|
"numeric_transform_present": False,
|
|
},
|
|
"first_frame_cross_check": {
|
|
"method": "independent-ground-near-field-plane",
|
|
"expected_ground_z_m": -self.patchwork_sensor_height_proxy_m,
|
|
"observed_intercept_range_m": [-2.18, -2.14],
|
|
"used_for_calibration": False,
|
|
},
|
|
},
|
|
"scope": {
|
|
"patchworkpp_eligible": True,
|
|
"normalized_scan_produced": False,
|
|
"complete_vehicle_transform_known": False,
|
|
"deskew_claimed": False,
|
|
},
|
|
"authority": {
|
|
"qualification_only": True,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
DEFAULT_GOOSE_PATCHWORK_PROFILE: Final = GoosePatchworkProfile()
|