spatialdds-ros 0.3.0__tar.gz → 0.4.0__tar.gz

{spatialdds_ros-0.3.0/spatialdds_ros.egg-info → spatialdds_ros-0.4.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: spatialdds-ros
-Version: 0.3.0
+Version: 0.4.0
 Summary: A ROS 2 robot as a SpatialDDS participant
 Home-page: https://github.com/OpenArCloud/spatialdds-ros
 Author: Open AR Cloud
@@ -48,8 +48,11 @@ Dynamic: summary
 
 Make a ROS 2 robot a SpatialDDS participant: its pose published as a
 `spatial.owm` entity on scene-scoped topics, `goto` commands consumed from
-`ModelCommand` and dispatched to nav2, and its presence announced — all on the
-same DDS bus ROS already runs on.
+`ModelCommand` and dispatched to nav2, its presence announced, and — when
+asked — the scene's other moving entities put into its own costmap. All on the
+same DDS bus ROS already runs on: one pip install makes a robot seen,
+steerable and aware, including of robots in other ROS domains it shares no
+middleware with.
 
 **A type adapter on a shared bus, not a protocol gateway.** Nothing is
 relayed. ROS and SpatialDDS meet on the wire because both are DDS, so this is
@@ -76,7 +79,7 @@ The split runs through the whole package:
 
 Only `spatialdds_ros.robot_node` requires a ROS 2 environment. It imports
 `rclpy` inside its entry point rather than at module scope, which is what
-lets the other twelve modules — and the entire test suite — run on a machine
+lets the other thirteen modules — and the entire test suite — run on a machine
 that has never had ROS installed.
 
 **That claim is a gate, not a promise.**
@@ -143,6 +146,117 @@ static and identity, which is a simulator and not a localizing robot.
 deferred** — there is no released binary package; install from source or with
 pip.
 
+## A robot that sees the scene
+
+```sh
+ros2 run spatialdds_ros robot_node --ros-args \
+    -p scene:=bare-scene \
+    -p entity_id:=ent:robot:b \
+    -p frame_id:=map/bare-scene \
+    -p awareness:=true \
+    -p awareness_range_m:=8.0
+```
+
+The node then reads the scene's other moving entities off the bus and
+publishes them where nav2 can plan around them: `~/peers/cloud` marks them,
+`~/peers/clearing` is a ring at `awareness_range_m` that clears whatever has
+gone, and `~/peers/markers` shows an operator what is being avoided. nav2
+needs one obstacle layer reading both clouds:
+
+```yaml
+peers_layer:
+  plugin: "nav2_costmap_2d::ObstacleLayer"
+  enabled: true
+  observation_sources: peers clearing
+  peers:
+    topic: /robot_b/peers/cloud
+    data_type: PointCloud2
+    marking: true
+    clearing: false
+    obstacle_max_range: 8.0
+    raytrace_max_range: 8.5
+    min_obstacle_height: 0.0
+    max_obstacle_height: 2.0
+  clearing:
+    topic: /robot_b/peers/clearing
+    data_type: PointCloud2
+    marking: false
+    clearing: true
+    obstacle_max_range: 8.0
+    raytrace_max_range: 8.5
+    min_obstacle_height: 0.0
+    max_obstacle_height: 2.0
+```
+
+Add `peers_layer` to the costmap's `plugins`, before the inflation layer, in
+every costmap that should avoid peers — the global one, for the planner to
+route around them. The topics carry the node's name (`robot_b` here).
+
+**Set `awareness_range_m` to your costmap's obstacle range or less** — both
+`obstacle_max_range` and `raytrace_max_range` above. The node refuses to start
+with awareness on and no range, because the right value lives in your nav2
+YAML, which it cannot read, and a guess beyond your raytrace range draws marks
+nothing will ever clear.
+
+**Why two clouds.** A marking cloud that empties raytraces nothing, so the
+last place a peer was seen stays lethal for good. Measured, with the ring
+beside it and without, in
+[`docs/awareness-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/awareness-census.md)
+§1.
+
+### What is drawn, and when it stops
+
+- **Moving things only:** `FAST`, `ACTIVE`, with a pose, within range, and of
+  an `awareness_types` URI if you set any. A declared region is not a peer;
+  the keep-out filter is the mechanism for those.
+- **Never this robot.** Its own record does reach its own reader, so this is
+  a filter that does work, on its own `entity_id`.
+- **Its own size, or `peer_radius_m`** (0.3 m) for a peer that publishes no
+  extent — which includes every robot this library publishes.
+- **Dropped when it goes quiet, and remembered.** A peer is drawn while its
+  poses keep arriving, within a window learned from its own cadence — at
+  least one second, three times its interval, measured on this robot's clock.
+  The record is kept, so the next pose brings it back with no rediscovery. An
+  `UNOBSERVED`, retired or disposed record, or a writer that has gone, drops
+  it at once.
+- **Never at a guessed position.** A peer in another frame is placed through
+  both frames' georeferences; with either one missing it is skipped, and the
+  log says which.
+
+### Two robots, two ROS domains, one bus
+
+The walkthrough is the acceptance test. Robot A runs in `ROS_DOMAIN_ID` 10,
+localized by an AMCL that converges at (3, 0) and then says nothing; robot B
+runs in 20 at the origin with nav2's planner; the SpatialDDS bus is DDS domain
+30. Nothing else connects them.
+
+```sh
+docker build -t spatialdds-ros-nav2 -f test/ros/Dockerfile .
+python3 -m pytest test/test_acceptance_two_domains.py -q -s
+```
+
+```
+  A absent: path 6.00 m, max |y| 0.000; cloud empty
+  A present: cloud 109 points near (3, 0), cost 100; path 6.30 m, max |y| 0.84
+  A still for 20 s: drawn in all 31 samples
+  quiet but latched: not drawing ent:robot:a: UNOBSERVED: no pose from svc:robot:a for 1.6s
+  killed: cost at A back to 0 within 2.8s (probe overhead included)
+  gateway served and validated 2 entities: ['ent:robot:a', 'ent:robot:b']
+```
+
+B's plan to (6, 0) bends around A, and straightens again once A is gone. Both
+ROS graphs are listed from one container, and neither holds a node or topic of
+the other: what separates them is the domain id, not the network.
+
+**What it does not do.** A peer beyond the range is not drawn. A mark left
+behind when this robot drives away from it stays until the robot comes back
+within range, which is how nav2 treats anything a sensor saw. A peer whose
+publisher sends a pose only when it moves will drop out when it stops — every
+`spatialdds_ros` robot sends at least `pose_floor_hz` for exactly this reason,
+but the wire gives a consumer no way to know what a foreign one promises
+([`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md)
+§3).
+
 ## Configuration
 
 Three parameters have no default and the node refuses to start without them,
@@ -156,7 +270,9 @@ because every possible default would be a false statement about the world:
 
 Everything else has one: `domain` 0, `ground_z` 0.0, `pose_source` tf,
 `nav2_action` navigate_to_pose, `source_id` derived from `entity_id`,
-`content_refs` and `external_refs` empty.
+`content_refs` and `external_refs` empty, `pose_floor_hz` 2.0, `awareness`
+off, `peer_radius_m` 0.3, `awareness_types` empty. One more has none, and is
+needed only with awareness on: `awareness_range_m`, for the reason above.
 
 ### What the robot *is*, and what it is *also known as*
 
@@ -244,8 +360,20 @@ not neutral when the thing being defaulted is a claim.
 The adapter publishes `basis = OBSERVED` and offers no parameter to change it:
 a self-localizing robot is reporting where it has worked out that it is, which
 is a sighting rather than an assertion. It claims no extent — a robot locates
-itself, it does not legislate a region, and an extent would be read as
-keep-out by anything applying the `DECLARED` rule.
+itself, and its footprint is not a region anybody should plan by. (Until 0.4.0
+this said an extent would be read as keep-out by the `DECLARED` rule; the
+robot is `OBSERVED`, so that rule excludes it before it looks at extent. The
+conclusion stands; the reason was wrong. Publishing a true footprint is a
+follow-up.)
+
+**Its pose goes out at least `pose_floor_hz` (2 Hz), moving or not.** A
+consumer stops drawing a peer it has not heard from within that peer's
+cadence, and some localizers fall silent when the robot stops — AMCL updates
+on motion — so without a floor a stationary robot would vanish from its
+peers' costmaps while standing in their way. In `pose_source=amcl` a live
+AMCL publisher counts as holding the estimate; a robot whose localizer has
+actually gone still goes `UNOBSERVED` on time. Anything at or below 1 Hz is
+refused, as slower than a consumer's shortest window.
 
 Three things it refuses to do quietly:
 
@@ -307,6 +435,13 @@ This adapter still does not *act* on disposal: it owns one key, never disposes
 it, and reports going quiet as `UNOBSERVED` — a weaker statement than "this
 robot is gone". Not acting on an event is not the same as not receiving one.
 
+**And nothing it writes is disposed for it.** CycloneDDS's default writer QoS
+disposes a writer's instances when the writer goes, so until 0.4.0 a robot
+that crashed reached every reader as `NOT_ALIVE_DISPOSED` — a deliberate
+removal, by §2.14's reading — ten seconds after the crash. Every writer here
+now turns that off, and the same events arrive as `NOT_ALIVE_NO_WRITERS`:
+liveliness lost, which is what happened.
+
 ## Tests
 
 ```sh
@@ -321,8 +456,9 @@ Everything runs with no ROS installed — that is a gate
 | document | what is in it |
 |---|---|
 | [`docs/extraction-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/extraction-census.md) | what became a parameter, what the library fills, and why |
+| [`docs/awareness-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/awareness-census.md) | what becomes an obstacle, when it stops, in which frame — measured, and ruled |
 | [`docs/failability.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/failability.md) | the two claims broken deliberately, with the failures |
-| [`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md) | two things for the specification rather than worked around |
+| [`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md) | things for the specification rather than worked around |
 | [`spatialdds_idl18/PROVENANCE.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/spatialdds_idl18/PROVENANCE.md) | where the vendored 1.8 bindings came from |
 | [`docs/RELEASING.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/RELEASING.md) | how a version reaches PyPI, and how the artifact is checked against the commit |
 

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/README.md

@@ -2,8 +2,11 @@
 
 Make a ROS 2 robot a SpatialDDS participant: its pose published as a
 `spatial.owm` entity on scene-scoped topics, `goto` commands consumed from
-`ModelCommand` and dispatched to nav2, and its presence announced — all on the
-same DDS bus ROS already runs on.
+`ModelCommand` and dispatched to nav2, its presence announced, and — when
+asked — the scene's other moving entities put into its own costmap. All on the
+same DDS bus ROS already runs on: one pip install makes a robot seen,
+steerable and aware, including of robots in other ROS domains it shares no
+middleware with.
 
 **A type adapter on a shared bus, not a protocol gateway.** Nothing is
 relayed. ROS and SpatialDDS meet on the wire because both are DDS, so this is
@@ -30,7 +33,7 @@ The split runs through the whole package:
 
 Only `spatialdds_ros.robot_node` requires a ROS 2 environment. It imports
 `rclpy` inside its entry point rather than at module scope, which is what
-lets the other twelve modules — and the entire test suite — run on a machine
+lets the other thirteen modules — and the entire test suite — run on a machine
 that has never had ROS installed.
 
 **That claim is a gate, not a promise.**
@@ -97,6 +100,117 @@ static and identity, which is a simulator and not a localizing robot.
 deferred** — there is no released binary package; install from source or with
 pip.
 
+## A robot that sees the scene
+
+```sh
+ros2 run spatialdds_ros robot_node --ros-args \
+    -p scene:=bare-scene \
+    -p entity_id:=ent:robot:b \
+    -p frame_id:=map/bare-scene \
+    -p awareness:=true \
+    -p awareness_range_m:=8.0
+```
+
+The node then reads the scene's other moving entities off the bus and
+publishes them where nav2 can plan around them: `~/peers/cloud` marks them,
+`~/peers/clearing` is a ring at `awareness_range_m` that clears whatever has
+gone, and `~/peers/markers` shows an operator what is being avoided. nav2
+needs one obstacle layer reading both clouds:
+
+```yaml
+peers_layer:
+  plugin: "nav2_costmap_2d::ObstacleLayer"
+  enabled: true
+  observation_sources: peers clearing
+  peers:
+    topic: /robot_b/peers/cloud
+    data_type: PointCloud2
+    marking: true
+    clearing: false
+    obstacle_max_range: 8.0
+    raytrace_max_range: 8.5
+    min_obstacle_height: 0.0
+    max_obstacle_height: 2.0
+  clearing:
+    topic: /robot_b/peers/clearing
+    data_type: PointCloud2
+    marking: false
+    clearing: true
+    obstacle_max_range: 8.0
+    raytrace_max_range: 8.5
+    min_obstacle_height: 0.0
+    max_obstacle_height: 2.0
+```
+
+Add `peers_layer` to the costmap's `plugins`, before the inflation layer, in
+every costmap that should avoid peers — the global one, for the planner to
+route around them. The topics carry the node's name (`robot_b` here).
+
+**Set `awareness_range_m` to your costmap's obstacle range or less** — both
+`obstacle_max_range` and `raytrace_max_range` above. The node refuses to start
+with awareness on and no range, because the right value lives in your nav2
+YAML, which it cannot read, and a guess beyond your raytrace range draws marks
+nothing will ever clear.
+
+**Why two clouds.** A marking cloud that empties raytraces nothing, so the
+last place a peer was seen stays lethal for good. Measured, with the ring
+beside it and without, in
+[`docs/awareness-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/awareness-census.md)
+§1.
+
+### What is drawn, and when it stops
+
+- **Moving things only:** `FAST`, `ACTIVE`, with a pose, within range, and of
+  an `awareness_types` URI if you set any. A declared region is not a peer;
+  the keep-out filter is the mechanism for those.
+- **Never this robot.** Its own record does reach its own reader, so this is
+  a filter that does work, on its own `entity_id`.
+- **Its own size, or `peer_radius_m`** (0.3 m) for a peer that publishes no
+  extent — which includes every robot this library publishes.
+- **Dropped when it goes quiet, and remembered.** A peer is drawn while its
+  poses keep arriving, within a window learned from its own cadence — at
+  least one second, three times its interval, measured on this robot's clock.
+  The record is kept, so the next pose brings it back with no rediscovery. An
+  `UNOBSERVED`, retired or disposed record, or a writer that has gone, drops
+  it at once.
+- **Never at a guessed position.** A peer in another frame is placed through
+  both frames' georeferences; with either one missing it is skipped, and the
+  log says which.
+
+### Two robots, two ROS domains, one bus
+
+The walkthrough is the acceptance test. Robot A runs in `ROS_DOMAIN_ID` 10,
+localized by an AMCL that converges at (3, 0) and then says nothing; robot B
+runs in 20 at the origin with nav2's planner; the SpatialDDS bus is DDS domain
+30. Nothing else connects them.
+
+```sh
+docker build -t spatialdds-ros-nav2 -f test/ros/Dockerfile .
+python3 -m pytest test/test_acceptance_two_domains.py -q -s
+```
+
+```
+  A absent: path 6.00 m, max |y| 0.000; cloud empty
+  A present: cloud 109 points near (3, 0), cost 100; path 6.30 m, max |y| 0.84
+  A still for 20 s: drawn in all 31 samples
+  quiet but latched: not drawing ent:robot:a: UNOBSERVED: no pose from svc:robot:a for 1.6s
+  killed: cost at A back to 0 within 2.8s (probe overhead included)
+  gateway served and validated 2 entities: ['ent:robot:a', 'ent:robot:b']
+```
+
+B's plan to (6, 0) bends around A, and straightens again once A is gone. Both
+ROS graphs are listed from one container, and neither holds a node or topic of
+the other: what separates them is the domain id, not the network.
+
+**What it does not do.** A peer beyond the range is not drawn. A mark left
+behind when this robot drives away from it stays until the robot comes back
+within range, which is how nav2 treats anything a sensor saw. A peer whose
+publisher sends a pose only when it moves will drop out when it stops — every
+`spatialdds_ros` robot sends at least `pose_floor_hz` for exactly this reason,
+but the wire gives a consumer no way to know what a foreign one promises
+([`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md)
+§3).
+
 ## Configuration
 
 Three parameters have no default and the node refuses to start without them,
@@ -110,7 +224,9 @@ because every possible default would be a false statement about the world:
 
 Everything else has one: `domain` 0, `ground_z` 0.0, `pose_source` tf,
 `nav2_action` navigate_to_pose, `source_id` derived from `entity_id`,
-`content_refs` and `external_refs` empty.
+`content_refs` and `external_refs` empty, `pose_floor_hz` 2.0, `awareness`
+off, `peer_radius_m` 0.3, `awareness_types` empty. One more has none, and is
+needed only with awareness on: `awareness_range_m`, for the reason above.
 
 ### What the robot *is*, and what it is *also known as*
 
@@ -198,8 +314,20 @@ not neutral when the thing being defaulted is a claim.
 The adapter publishes `basis = OBSERVED` and offers no parameter to change it:
 a self-localizing robot is reporting where it has worked out that it is, which
 is a sighting rather than an assertion. It claims no extent — a robot locates
-itself, it does not legislate a region, and an extent would be read as
-keep-out by anything applying the `DECLARED` rule.
+itself, and its footprint is not a region anybody should plan by. (Until 0.4.0
+this said an extent would be read as keep-out by the `DECLARED` rule; the
+robot is `OBSERVED`, so that rule excludes it before it looks at extent. The
+conclusion stands; the reason was wrong. Publishing a true footprint is a
+follow-up.)
+
+**Its pose goes out at least `pose_floor_hz` (2 Hz), moving or not.** A
+consumer stops drawing a peer it has not heard from within that peer's
+cadence, and some localizers fall silent when the robot stops — AMCL updates
+on motion — so without a floor a stationary robot would vanish from its
+peers' costmaps while standing in their way. In `pose_source=amcl` a live
+AMCL publisher counts as holding the estimate; a robot whose localizer has
+actually gone still goes `UNOBSERVED` on time. Anything at or below 1 Hz is
+refused, as slower than a consumer's shortest window.
 
 Three things it refuses to do quietly:
 
@@ -261,6 +389,13 @@ This adapter still does not *act* on disposal: it owns one key, never disposes
 it, and reports going quiet as `UNOBSERVED` — a weaker statement than "this
 robot is gone". Not acting on an event is not the same as not receiving one.
 
+**And nothing it writes is disposed for it.** CycloneDDS's default writer QoS
+disposes a writer's instances when the writer goes, so until 0.4.0 a robot
+that crashed reached every reader as `NOT_ALIVE_DISPOSED` — a deliberate
+removal, by §2.14's reading — ten seconds after the crash. Every writer here
+now turns that off, and the same events arrive as `NOT_ALIVE_NO_WRITERS`:
+liveliness lost, which is what happened.
+
 ## Tests
 
 ```sh
@@ -275,8 +410,9 @@ Everything runs with no ROS installed — that is a gate
 | document | what is in it |
 |---|---|
 | [`docs/extraction-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/extraction-census.md) | what became a parameter, what the library fills, and why |
+| [`docs/awareness-census.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/awareness-census.md) | what becomes an obstacle, when it stops, in which frame — measured, and ruled |
 | [`docs/failability.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/failability.md) | the two claims broken deliberately, with the failures |
-| [`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md) | two things for the specification rather than worked around |
+| [`docs/spec-queue.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/spec-queue.md) | things for the specification rather than worked around |
 | [`spatialdds_idl18/PROVENANCE.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/spatialdds_idl18/PROVENANCE.md) | where the vendored 1.8 bindings came from |
 | [`docs/RELEASING.md`](https://github.com/OpenArCloud/spatialdds-ros/blob/main/docs/RELEASING.md) | how a version reaches PyPI, and how the artifact is checked against the commit |
 

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://relaxng.org/ns/structure/1.0"?>
 <package format="3">
   <name>spatialdds_ros</name>
-  <version>0.3.0</version>
+  <version>0.4.0</version>
   <description>
     Makes a ROS 2 robot a SpatialDDS participant: its pose published as a
     spatial.owm entity on scene-scoped topics, goto commands consumed from
@@ -20,6 +20,8 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
 
   <test_depend>python3-pytest</test_depend>
 

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/setup.py

@@ -42,7 +42,7 @@ LONG_DESCRIPTION = (HERE / "README.md").read_text(encoding="utf-8")
 
 setup(
     name="spatialdds-ros",
-    version="0.3.0",
+    version="0.4.0",
     # `spatialdds_idl18` ships alongside: the generated bindings annotate
     # nested members with their own fully-qualified names, so the package has
     # to be importable as `spatialdds_idl18` and cannot be nested inside this

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/spatialdds_ros/__init__.py

@@ -3,4 +3,4 @@
 See docs/extraction-census.md for what is configurable and why.
 """
 
-__version__ = "0.3.0"
+__version__ = "0.4.0"

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/spatialdds_ros/adapter.py

@@ -30,6 +30,7 @@ statement:
 """
 from __future__ import annotations
 
+import math
 import time
 from typing import List, Optional, Protocol, Tuple
 
@@ -90,6 +91,7 @@ class RobotAdapter:
         self._goal: Optional[Tuple[float, float]] = None
         self._silent = False
         self._last_seen = 0.0
+        self._last_pose_write = 0.0
         self._navigator = navigator
         # **The last thing that happened to a goal, held until something
         # changes it.** A refusal published once and then superseded by the
@@ -120,9 +122,12 @@ class RobotAdapter:
             frame_ref=robot_frame(self.config.frame_id),
             has_pose=True,
             pose=pose,
-            # A robot locates itself; it does not legislate a region. An
-            # extent here would be read as keep-out by anything applying the
-            # DECLARED rule, which is not what a robot's footprint means.
+            # A robot locates itself; its footprint is not a region anybody
+            # should plan by. (This once said an extent would be read as
+            # keep-out by the DECLARED rule; the robot is OBSERVED, and that
+            # rule excludes it on basis first — awareness census §2.) A true
+            # footprint, for consumers that draw peers by size, is a filed
+            # follow-up rather than something to guess here.
             has_extent=False,
             extent=Aabb3(min_xyz=[0.0, 0.0, 0.0], max_xyz=[0.0, 0.0, 0.0]),
             properties=properties,
@@ -173,10 +178,7 @@ class RobotAdapter:
         self._last_seen = now
         self._silent = False
 
-        self._poses.write(ModelPose(
-            entity_id=self.config.entity_id, pose=pose,
-            source_id=self.config.effective_source_id,
-            stamp=self._entity.stamp))
+        self._write_pose(pose, now, self._entity.stamp)
 
         # The expensive record goes on the cadence — and unconditionally when
         # the robot first appears or comes back, because those are changes of
@@ -265,6 +267,29 @@ class RobotAdapter:
                 notes.append(note)
         return notes
 
+    def _write_pose(self, pose: PoseSE3, now: float, stamp=None) -> None:
+        # A floor republish is stamped when it is sent: it says where the
+        # robot is as of now, which is true for as long as the robot is not
+        # silent — and a silent robot gets no floor.
+        self._poses.write(ModelPose(
+            entity_id=self.config.entity_id, pose=pose,
+            source_id=self.config.effective_source_id,
+            stamp=stamp if stamp is not None else now_time()))
+        self._last_pose_write = now
+
+    def holding(self, now: Optional[float] = None) -> None:
+        """
+        The pose source is alive and its estimate has not changed.
+
+        For a source that only speaks when the robot moves — AMCL publishes
+        on filter updates — silence is not evidence of anything going wrong,
+        so the caller says what it knows: the localizer is still running.
+        That keeps the robot ACTIVE without inventing a new observation; the
+        floor in `tick` then keeps it fresh on the bus.
+        """
+        if self._entity is not None and not self._silent:
+            self._last_seen = time.time() if now is None else now
+
     def tick(self, now: Optional[float] = None) -> Optional[str]:
         """
         Housekeeping between poses: flush the latch when the robot stops
@@ -286,6 +311,14 @@ class RobotAdapter:
             self._tempo.forget(self.config.entity_id)
             return f"unobserved after {quiet_for:.1f}s of silence"
 
+        # The floor: the last pose again, freshly stamped, when nothing newer
+        # has gone out for a floor interval. Republishing is not a new
+        # sighting, so it touches neither the silence clock nor the latch
+        # cadence — a robot gone quiet still goes UNOBSERVED on time.
+        if (not self._silent
+                and now - self._last_pose_write >= 1.0 / self.config.pose_floor_hz):
+            self._write_pose(self._entity.pose, now)
+
         if not self._silent and self._tempo.idle(now):
             self._entities.write(self._entity)
             return "latch flushed — it stopped moving"
@@ -301,6 +334,16 @@ class RobotAdapter:
     def clear_goal(self) -> None:
         self._goal = None
 
+    @property
+    def pose(self) -> Optional[Tuple[float, float, float]]:
+        """The last published (x, y, yaw) in this robot's frame, or `None`."""
+        if self._entity is None:
+            return None
+        q = self._entity.pose.q
+        yaw = math.atan2(2.0 * (q[3] * q[2] + q[0] * q[1]),
+                         1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]))
+        return self._entity.pose.t[0], self._entity.pose.t[1], yaw
+
     @property
     def state(self) -> str:
         return "UNOBSERVED" if self._silent else "ACTIVE"

{spatialdds_ros-0.3.0 → spatialdds_ros-0.4.0}/spatialdds_ros/announce.py

@@ -218,13 +218,32 @@ class Announcer:
         self._writer = tt.make_writer(
             participant, ANNOUNCE_TOPIC, Announce, MODEL_LATCHED.name,
             lifespan_sec=float(ttl_sec))
+        self._last: Optional[Announce] = None
 
     def publish(self) -> str:
         announce = announce_for(self.config, self.kind, self.ttl_sec)
         self._writer.write(announce)
+        self._last = announce
         return (f"announce: {announce.service_id} as {announce.kind.name}, "
                 f"{len(announce.topics)} topics, ttl {announce.ttl_sec}s")
 
+    def depart(self) -> Optional[str]:
+        """
+        Leave the discovery lane on purpose: dispose the announce.
+
+        §3.3's Announce lifecycle: "A node leaving the bus gracefully MUST
+        dispose its Announce instance". Until 0.4.0 this happened only by
+        accident — CycloneDDS disposed every instance of a deleted writer —
+        and that same default made a *crash* look like a departure. With it
+        off (`qos.py`), a graceful exit has to say so, and this is where. An
+        ungraceful one is left to the ttl, which is the spec's backstop.
+        `Depart` (a SHOULD, for non-DDS transports) is not published.
+        """
+        if self._last is None:
+            return None
+        self._writer.dispose(self._last)
+        return f"departed: disposed the announce for {self._last.service_id}"
+
     @property
     def refresh_interval_s(self) -> float:
         """Comfortably inside the TTL, so one missed refresh is not an outage."""