spatialdds-ros 0.3.0__py3-none-any.whl

spatialdds_ros/announce.py

@@ -0,0 +1,231 @@
+"""
+The robot's presence on the discovery lane.
+
+A robot that publishes its pose and accepts goals is discoverable in exactly
+the way a VPS or a catalogue is: something that wants to drive it should be
+able to find it without being told its topic names. The prototype's robot does
+*not* announce — discovery there belongs to the VPS and catalogue servers — so
+this capability is new rather than extracted, and the catalogue server's
+announce is the shape it follows.
+
+**On `kind`.** 1.8's `ServiceKind` is VPS, MAPPING, RELOCAL, SEMANTICS,
+STORAGE, CONTENT, ANCHOR_REGISTRY, SENSING, INFRASTRUCTURE, FUSION, OTHER.
+None of them is an agent that reports where it is and accepts commands about
+where to go. `SENSING` is the nearest and it overstates — it reads as a service
+offering sensor feeds, which this is not. So the default is `OTHER`, which is
+accurate, and `kind` is a parameter so an operator whose robot genuinely is one
+of the others can say so.
+
+That gap is filed rather than papered over: see `docs/spec-queue.md`. Choosing
+`SENSING` because it is closest would make every robot on the bus advertise a
+capability it does not have, and the next reader would have no way to know the
+enum had been stretched.
+"""
+from __future__ import annotations
+
+from typing import Optional
+
+# **Resolved at their definitions, not through `spatial.disco`.** That module
+# re-exports most of what an announce needs, and six of the names it re-exports
+# are annotated typedef aliases that report zero members and raise
+# `TypeError: 'ForwardRef' object is not callable` when constructed — Aabb3,
+# FrameRef, Time, PoseSE3, SpatialUri and MetaKV among them. The convenient
+# import is the wrong one. `test/test_types_resolve_at_definitions.py` is the
+# gate; this trap has cost four separate debugging rounds across this
+# programme, so it is enforced rather than remembered.
+from spatialdds_idl18.builtin import Time
+from spatialdds_idl18.spatial.common import FrameRef
+from spatialdds_idl18.spatial.core import Aabb3, PoseSE3
+from spatialdds_idl18.spatial.disco import (
+    Announce, Capabilities, CoverageElement, ProfileSupport, ServiceKind,
+    TopicMeta, Transform, ValidityWindow)
+
+from spatialdds_ros.config import Config
+from spatialdds_ros.frames import (
+    earth_fixed_frame, enu_to_ecef_pose, now_time, robot_frame)
+from spatialdds_ros.qos import MODEL_COMMAND, MODEL_FAST, MODEL_LATCHED
+from spatialdds_ros.topics import Topics
+
+#: An absent bbox still needs four numbers, because `BBox2D` is a fixed
+#: `double[4]`. Paired with `has_bbox=False`, so nothing reads them.
+_NO_BBOX = [0.0, 0.0, 0.0, 0.0]
+
+#: How long an announce stands before a consumer should stop believing it.
+#: Mapped onto DDS Lifespan by the writer, so the middleware expires it rather
+#: than every consumer re-deriving staleness.
+DEFAULT_TTL_SEC = 300
+
+#: What this library speaks. Stated as a range because that is what
+#: `ProfileSupport` is for, and narrow because claiming 1.7 would invite a 1.7
+#: client to assume the four members 1.8 appended are absent.
+PROFILES = (("spatial.core", 1, 8, 8), ("spatial.owm", 0, 1, 1))
+
+
+def _topics(config: Config) -> list[TopicMeta]:
+    """
+    The lanes this robot reads and writes, with their QoS profiles named.
+
+    `qos_profile` being advertised is what lets a consumer build a matching
+    endpoint without guessing — and the names here are the same ones
+    `qos.PROFILES` resolves, so the advertisement is true on the wire rather
+    than decorative. `target_rate_hz` is 0.0 where there is no steady rate:
+    0 means unspecified, which is a different statement from a slow rate.
+    """
+    names = Topics(config.scene)
+    return [
+        TopicMeta(name=names.entity, type="spatial.owm.Entity", version="v1",
+                  qos_profile=MODEL_LATCHED.name, target_rate_hz=0.0,
+                  max_chunk_bytes=0, durability="TRANSIENT_LOCAL"),
+        TopicMeta(name=names.pose, type="spatial.owm.ModelPose", version="v1",
+                  qos_profile=MODEL_FAST.name, target_rate_hz=0.0,
+                  max_chunk_bytes=0, durability="VOLATILE"),
+        TopicMeta(name=names.command, type="spatial.owm.ModelCommand",
+                  version="v1", qos_profile=MODEL_COMMAND.name,
+                  target_rate_hz=0.0, max_chunk_bytes=0,
+                  durability="VOLATILE"),
+    ]
+
+
+def _coverage(config: Config) -> list[CoverageElement]:
+    """
+    Where this robot is useful.
+
+    **With no anchor there is no geographic coverage to claim**, and a
+    `_global` element would be a loud lie — a robot in one building
+    advertising the planet. So the coverage is expressed in the robot's own
+    frame instead: a frame-referenced element with no bbox, which says "within
+    this frame" and nothing about the earth. That is the same honesty the
+    missing `FrameTransform` observes, carried onto the discovery lane.
+
+    With an anchor, a degenerate bbox at the anchor point: the robot moves, so
+    any extent would be a guess about how far, and a point is the one claim
+    that is true.
+    """
+    if config.anchor is None:
+        return [CoverageElement(
+            has_crs=False, crs="", has_bbox=False,
+            bbox=_NO_BBOX,
+            has_aabb=False,
+            aabb=Aabb3(min_xyz=[0.0, 0.0, 0.0], max_xyz=[0.0, 0.0, 0.0]),
+            _global=False,
+            has_frame_ref=True, frame_ref=robot_frame(config.frame_id),
+            has_coverage_window=False,
+            coverage_window_start=Time(sec=0, nanosec=0),
+            coverage_window_end=Time(sec=0, nanosec=0),
+            has_circle=False, circle_center=[0.0, 0.0, 0.0],
+            circle_radius_m=0.0)]
+    a = config.anchor
+    return [CoverageElement(
+        has_crs=True, crs="EPSG:4326", has_bbox=True,
+        # **[west, south, east, north]** — `BBox2D` is `double[4]`, not a
+        # struct with named members, and the order is stated only in a comment
+        # in the IDL (`discovery.idl:96`). Longitude first. Getting it wrong
+        # produces a well-formed box somewhere else entirely, which is why the
+        # order is written out here rather than inferred at the call site.
+        bbox=[a.lon_deg, a.lat_deg, a.lon_deg, a.lat_deg],
+        has_aabb=False,
+        aabb=Aabb3(min_xyz=[0.0, 0.0, 0.0], max_xyz=[0.0, 0.0, 0.0]),
+        _global=False,
+        has_frame_ref=True, frame_ref=robot_frame(config.frame_id),
+        has_coverage_window=False,
+        coverage_window_start=Time(sec=0, nanosec=0),
+        coverage_window_end=Time(sec=0, nanosec=0),
+        has_circle=False, circle_center=[0.0, 0.0, 0.0],
+        circle_radius_m=0.0)]
+
+
+def _transforms(config: Config) -> list[Transform]:
+    """
+    The announce's own copy of the georeference, when there is one.
+
+    Empty without an anchor, for the reason the frames lane is silent: there is
+    nothing true to say. Note the spelling — `_from` is the *local* frame and
+    `to` is earth-fixed, which is the opposite of `FrameTransform`'s
+    parent/child roles while carrying identical numbers. Mapping by name
+    rather than by role inverts it, which is why both ends are written out
+    here rather than passed through a helper that could be read either way.
+    """
+    if config.anchor is None:
+        return []
+    t, q = enu_to_ecef_pose(config.anchor.lat_deg, config.anchor.lon_deg,
+                            config.anchor.height_m)
+    return [Transform(
+        _from=robot_frame(config.frame_id),      # the local frame
+        to=earth_fixed_frame(),                  # the global one
+        pose=PoseSE3(t=t, q=q),
+        stamp=now_time(),
+        has_validity=False,
+        # `_from` and `seconds`, not start/end — a window with a duration
+        # rather than two instants. Unread while `has_validity` is false; a
+        # georeference that expires would say the frame moved, which is not
+        # what an anchor from a launch file means.
+        validity=ValidityWindow(_from=Time(sec=0, nanosec=0), seconds=0))]
+
+
+def announce_for(config: Config, kind: str = "OTHER",
+                 ttl_sec: int = DEFAULT_TTL_SEC) -> Announce:
+    """This robot, as something another participant can discover."""
+    return Announce(
+        service_id=config.effective_source_id,
+        name=config.effective_label,
+        kind=getattr(ServiceKind, kind),
+        version="1.8",
+        org="",
+        hints=[],
+        caps=Capabilities(
+            supported_profiles=[
+                ProfileSupport(name=n, major=major, min_minor=lo,
+                               max_minor=hi)
+                for n, major, lo, hi in PROFILES],
+            preferred_profiles=["spatial.owm/0.1"],
+            features=[]),
+        topics=_topics(config),
+        coverage=_coverage(config),
+        coverage_frame_ref=(earth_fixed_frame() if config.anchor
+                            else robot_frame(config.frame_id)),
+        has_coverage_eval_time=False,
+        coverage_eval_time=Time(sec=0, nanosec=0),
+        transforms=_transforms(config),
+        manifest_uri="",
+        auth_hint="",
+        stamp=now_time(),
+        ttl_sec=ttl_sec,
+        coverage_source_ids=[],
+    )
+
+
+#: The discovery lane is not scene-scoped — it is the one place a participant
+#: looks before it knows what scenes exist.
+ANNOUNCE_TOPIC = "spatialdds/discovery/announce/v1"
+
+
+class Announcer:
+    """
+    Publishes the announce, and re-stamps it on a cadence.
+
+    Re-stamped rather than written once: `ttl_sec` is mapped onto DDS Lifespan,
+    so a sample that is never refreshed expires and the robot silently stops
+    being discoverable while it is still running. The prototype learned the
+    same thing — "Re-stamp the announce on refresh, not just the Lifespan".
+    """
+
+    def __init__(self, participant, config: Config, kind: str = "OTHER",
+                 ttl_sec: int = DEFAULT_TTL_SEC) -> None:
+        from spatialdds_ros import transport as tt
+        self.config = config
+        self.kind = kind
+        self.ttl_sec = ttl_sec
+        self._writer = tt.make_writer(
+            participant, ANNOUNCE_TOPIC, Announce, MODEL_LATCHED.name,
+            lifespan_sec=float(ttl_sec))
+
+    def publish(self) -> str:
+        announce = announce_for(self.config, self.kind, self.ttl_sec)
+        self._writer.write(announce)
+        return (f"announce: {announce.service_id} as {announce.kind.name}, "
+                f"{len(announce.topics)} topics, ttl {announce.ttl_sec}s")
+
+    @property
+    def refresh_interval_s(self) -> float:
+        """Comfortably inside the TTL, so one missed refresh is not an outage."""
+        return max(1.0, self.ttl_sec / 3.0)

spatialdds_ros/config.py

@@ -0,0 +1,293 @@
+"""
+The parameter surface, and what each silence means.
+
+Ruled 2026-10-19; `docs/extraction-census.md` §12 is the reasoning and this is
+the implementation of it. The rule the surface is built on is the programme's:
+**conventions fill plumbing and never claims.** So a parameter exists where an
+operator could be wrong *about the world*, and a default exists only where
+being wrong is impossible.
+
+Three parameters have no honest default and therefore refuse to start:
+
+* `scene` — which world this robot is in. A made-up default puts it somewhere
+  nobody is watching, and the failure is an empty bus rather than an error.
+* `entity_id` — which robot it is. A default collides the moment two run, and
+  the one-writer-per-key guard would then stop the second with a reason that
+  points at the wrong thing.
+* `frame_id` — which frame its pose is expressed in. A pose without its frame
+  is not a weak claim, it is a meaningless one.
+
+One silence is a legitimate operating mode rather than a misconfiguration:
+
+* `anchor` absent — **no `FrameTransform` is published**, said once plainly.
+  The adapter is still fully functional on the bus; the scene simply has no
+  georeference from it. The alternative, defaulting the anchor to the frame
+  origin, would have every unconfigured robot claiming to be in the Gulf of
+  Guinea with a pose that looks entirely plausible. A default is not neutral
+  when the thing being defaulted is a claim.
+"""
+from __future__ import annotations
+
+import re
+from dataclasses import dataclass
+from typing import Optional, Sequence, Tuple
+
+#: "mobile robot" — an automatic machine capable of movement in any given
+#: environment, `subclass of` (P279) Q11012 (robot). Carried from the
+#: prototype, where it was verified against the entity data rather than taken
+#: from a search result: several US patents are also labelled "Mobile robot".
+#: A default rather than a fixture — a drone or an AMR may want its own.
+DEFAULT_TYPE_URI = "http://www.wikidata.org/entity/Q4810574"
+
+#: nav2's own action name. A parameter because a robot may remap it; this is
+#: what nav2 calls it unremapped.
+DEFAULT_NAV2_ACTION = "navigate_to_pose"
+
+#: How a map-frame pose is obtained. `tf` is the default for a reason the
+#: census documents at length: the prototype read `nav_msgs/Odometry` on
+#: `/odom`, which is map-frame *only* in the simulator, where `map -> odom` is
+#: static and identity. On a real robot odom drifts against map, so publishing
+#: it as a georeferenced OBSERVED pose emits a slowly-drifting claim about
+#: where a robot sits on the earth with every number looking plausible.
+POSE_SOURCES = ("tf", "amcl", "odom")
+
+#: The IDL's bounds on the two reference sequences
+#: (`sequence[str, 8]` and `sequence[KV, 8]` on `spatial.owm.Entity`).
+#: Named here so a refusal can cite the limit rather than just report a
+#: serialize failure.
+MAX_CONTENT_REFS = 8
+MAX_EXTERNAL_REFS = 8
+
+#: One path segment of a SpatialDDS topic name (§3.3.1). Deliberately strict:
+#: a scene name carrying a slash would silently produce a six-segment topic,
+#: which is a name no other participant looks for.
+_SEGMENT = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._-]*$")
+
+
+class ConfigError(ValueError):
+    """
+    Raised for a configuration that cannot be started.
+
+    Carries *every* problem rather than the first, because a robot operator
+    editing a launch file should learn about three missing parameters in one
+    pass instead of three.
+    """
+
+
+@dataclass(frozen=True)
+class Anchor:
+    """
+    Where a local frame's origin sits on the earth.
+
+    Validated rather than taken on trust: these three numbers are the library's
+    only statement about the planet, and a transposed pair is not detectable
+    downstream — an anchor at (-97.7, 30.3) instead of (30.3, -97.7) is a
+    perfectly well-formed point in the South Atlantic.
+    """
+
+    lat_deg: float
+    lon_deg: float
+    height_m: float
+
+    def __post_init__(self) -> None:
+        problems = []
+        if not -90.0 <= self.lat_deg <= 90.0:
+            problems.append(
+                f"lat_deg {self.lat_deg} is outside [-90, 90]; a latitude "
+                f"beyond the poles usually means lat and lon were swapped")
+        if not -180.0 <= self.lon_deg <= 180.0:
+            problems.append(
+                f"lon_deg {self.lon_deg} is outside [-180, 180]")
+        if problems:
+            raise ConfigError("; ".join(problems))
+
+
+@dataclass(frozen=True)
+class Config:
+    """
+    Everything the adapter needs, validated at construction.
+
+    Frozen: the parameter surface is read at startup and a value that changed
+    underneath a running adapter would mean the entity it has been publishing
+    all along was a different claim than the one it is making now.
+    """
+
+    scene: str
+    entity_id: str
+    frame_id: str
+
+    anchor: Optional[Anchor] = None
+    source_id: Optional[str] = None
+    domain: int = 0
+    ground_z: float = 0.0
+    pose_source: str = "tf"
+    nav2_action: str = DEFAULT_NAV2_ACTION
+    type_uri: str = DEFAULT_TYPE_URI
+    label: Optional[str] = None
+
+    #: What this robot *is*, for a consumer that wants to draw it.
+    #:
+    #: Added 2026-10-19 because their absence blocked the first real adoption.
+    #: SpatialDDS-demo's robot carries `catalog:dcef7c31-…`, and a consumer
+    #: reading that reference is what makes it render a robot rather than a
+    #: pin: no reference, no asset, no model. An adapter that cannot say what
+    #: a robot looks like cannot be a consumer's only publisher of that robot,
+    #: and there was no way for a consumer to supply it from outside — the
+    #: record simply had nowhere to put it.
+    #:
+    #: **Empty by default, and empty means empty.** The adapter states what the
+    #: operator configured and nothing else; it invents no reference and
+    #: guesses no asset. An empty sequence is a published fact — "this robot
+    #: names no content" — not an accident of omission, and the gates assert
+    #: both directions.
+    content_refs: Tuple[str, ...] = ()
+
+    #: What this robot is *also known as*: wikidata, an asset-management id, a
+    #: fleet number. Same rule — only what was configured.
+    external_refs: Tuple[Tuple[str, str], ...] = ()
+
+    def __post_init__(self) -> None:
+        # A launch file or a dict will hand these over as lists. The dataclass
+        # is frozen, so they are normalised to tuples here rather than stored
+        # mutable — a shared mutable default on a frozen "value" is a bug
+        # waiting for a second robot.
+        object.__setattr__(self, "content_refs",
+                           tuple(self.content_refs or ()))
+        object.__setattr__(self, "external_refs",
+                           tuple(tuple(pair) for pair in
+                                 (self.external_refs or ())))
+        problems: list[str] = []
+
+        # The three with no honest default. Reported together, and each with
+        # what the silence would have meant rather than just "required".
+        if not self.scene:
+            problems.append(
+                "scene is required: it names the world this robot joins, and "
+                "there is no default that is not a guess about where it is. "
+                "A wrong scene is an empty bus, not an error")
+        elif not _SEGMENT.match(self.scene):
+            problems.append(
+                f"scene {self.scene!r} is not one topic segment (§3.3.1): "
+                f"expected letters, digits, dot, dash or underscore. A scene "
+                f"containing a slash silently produces a topic name no other "
+                f"participant looks for")
+
+        if not self.entity_id:
+            problems.append(
+                "entity_id is required: it names which robot this is. A "
+                "default collides as soon as a second robot runs, and the "
+                "one-writer-per-key guard would then refuse the second for a "
+                "reason that points at the wrong thing")
+
+        if not self.frame_id:
+            problems.append(
+                "frame_id is required: a pose without the frame it is "
+                "expressed in is not a weak claim, it is a meaningless one")
+
+        if self.pose_source not in POSE_SOURCES:
+            problems.append(
+                f"pose_source {self.pose_source!r} is not one of "
+                f"{', '.join(POSE_SOURCES)}")
+
+        if self.domain < 0 or self.domain > 232:
+            problems.append(
+                f"domain {self.domain} is outside the DDS range [0, 232]")
+
+        # Bounded sequences in the IDL: `content_refs` and `external_refs` are
+        # both `sequence[..., 8]`. Checked here rather than left to serialize,
+        # which fails late and says nothing about which field or why.
+        if len(self.content_refs) > MAX_CONTENT_REFS:
+            problems.append(
+                f"content_refs has {len(self.content_refs)} entries; the IDL "
+                f"bounds it at {MAX_CONTENT_REFS}")
+        for ref in self.content_refs:
+            if not ref or not str(ref).strip():
+                problems.append(
+                    "a content_ref is empty; an empty reference resolves to "
+                    "nothing and is not the same as publishing none")
+        if len(self.external_refs) > MAX_EXTERNAL_REFS:
+            problems.append(
+                f"external_refs has {len(self.external_refs)} entries; the IDL "
+                f"bounds it at {MAX_EXTERNAL_REFS}")
+        for pair in self.external_refs:
+            if len(tuple(pair)) != 2 or not all(str(v).strip() for v in pair):
+                problems.append(
+                    f"external_refs entries are (key, value) pairs and both "
+                    f"halves must be non-empty; got {pair!r}")
+
+        if problems:
+            raise ConfigError(
+                "this configuration cannot be started:\n  - "
+                + "\n  - ".join(problems))
+
+    # --- derived, never asked for -----------------------------------------
+
+    @property
+    def effective_source_id(self) -> str:
+        """
+        Who is publishing, which is not a claim about the world.
+
+        Derived from `entity_id` when unset: the publisher's identity follows
+        the thing it publishes, and asking an operator for both invites them
+        to disagree.
+        """
+        if self.source_id:
+            return self.source_id
+        tail = self.entity_id.split(":")[-1] or self.entity_id
+        return f"svc:robot:{tail}"
+
+    @property
+    def effective_label(self) -> str:
+        """A human-facing name. Cosmetic, so the id's tail will do."""
+        return self.label or (self.entity_id.split(":")[-1] or self.entity_id)
+
+    @property
+    def publishes_georeference(self) -> bool:
+        return self.anchor is not None
+
+    def startup_notes(self) -> list[str]:
+        """
+        What to say once, at startup, about the modes this configuration is in.
+
+        Returned rather than logged so the caller owns the logging and the
+        tests can read it. Each line states a *consequence*, not a setting:
+        "no anchor configured" leaves a reader to work out what follows, and
+        the whole point of the degraded mode being legitimate is that it is
+        legible.
+        """
+        notes = [
+            f"scene {self.scene}, entity {self.entity_id} in frame "
+            f"{self.frame_id}, DDS domain {self.domain}",
+        ]
+        if self.anchor is None:
+            # **Reworded 2026-10-19.** This used to say "nothing
+            # earth-references them", which assumed this adapter is the only
+            # thing that could — and in the first real deployment it was not:
+            # the venue publishes the scene's own FrameTransform, and running
+            # the robot anchorless is correct *because* somebody else owns
+            # that claim, which is the opposite of the absence the old wording
+            # described. So the note now says what it actually knows: this
+            # adapter is not publishing one. Whether anyone else is, it cannot
+            # see from here.
+            notes.append(
+                "no anchor configured, so this adapter publishes no "
+                "FrameTransform: its entity and poses are on the bus and "
+                "complete, and it makes no claim about where that frame sits "
+                "on the earth. Another participant may own that claim — a "
+                "venue publishing its own georeference is the ordinary case, "
+                "and then this is the correct configuration. If nobody "
+                "publishes one, a consumer cannot place this robot on a globe "
+                "and should say so rather than assume a position")
+        else:
+            notes.append(
+                f"anchor {self.anchor.lat_deg:.8f}, {self.anchor.lon_deg:.8f}, "
+                f"{self.anchor.height_m:.3f} m — publishing {self.frame_id} -> "
+                f"earth-fixed")
+        if self.pose_source == "odom":
+            notes.append(
+                "pose_source is odom: odom drifts against map, so this "
+                "publishes a pose that will slowly diverge from where the "
+                "robot actually is. Correct only where map -> odom is static "
+                "and identity, which is true of a simulator and false of a "
+                "localizing robot. Prefer pose_source=tf")
+        return notes