spatialdds-ros 0.3.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.
Files changed (87) hide show
  1. spatialdds_idl18/PROVENANCE.md +66 -0
  2. spatialdds_idl18/__init__.py +9 -0
  3. spatialdds_idl18/_field_aliases.py +26 -0
  4. spatialdds_idl18/builtin/__init__.py +11 -0
  5. spatialdds_idl18/builtin/_types.py +28 -0
  6. spatialdds_idl18/spatial/__init__.py +33 -0
  7. spatialdds_idl18/spatial/anchors/__init__.py +20 -0
  8. spatialdds_idl18/spatial/anchors/_anchors.py +102 -0
  9. spatialdds_idl18/spatial/argeo/__init__.py +22 -0
  10. spatialdds_idl18/spatial/argeo/_argeo.py +97 -0
  11. spatialdds_idl18/spatial/common/__init__.py +27 -0
  12. spatialdds_idl18/spatial/common/_types.py +104 -0
  13. spatialdds_idl18/spatial/core/GnssService/__init__.py +17 -0
  14. spatialdds_idl18/spatial/core/GnssService/_core.py +27 -0
  15. spatialdds_idl18/spatial/core/__init__.py +40 -0
  16. spatialdds_idl18/spatial/core/_core.py +338 -0
  17. spatialdds_idl18/spatial/disco/__init__.py +34 -0
  18. spatialdds_idl18/spatial/disco/_discovery.py +252 -0
  19. spatialdds_idl18/spatial/events/__init__.py +29 -0
  20. spatialdds_idl18/spatial/events/_events.py +164 -0
  21. spatialdds_idl18/spatial/events/enums/__init__.py +19 -0
  22. spatialdds_idl18/spatial/events/enums/crossing_direction_enum/__init__.py +11 -0
  23. spatialdds_idl18/spatial/events/enums/crossing_direction_enum/_events.py +25 -0
  24. spatialdds_idl18/spatial/events/enums/event_state_enum/__init__.py +11 -0
  25. spatialdds_idl18/spatial/events/enums/event_state_enum/_events.py +26 -0
  26. spatialdds_idl18/spatial/events/enums/event_type_enum/__init__.py +11 -0
  27. spatialdds_idl18/spatial/events/enums/event_type_enum/_events.py +36 -0
  28. spatialdds_idl18/spatial/events/enums/severity_enum/__init__.py +11 -0
  29. spatialdds_idl18/spatial/events/enums/severity_enum/_events.py +26 -0
  30. spatialdds_idl18/spatial/events/enums/zone_kind_enum/__init__.py +11 -0
  31. spatialdds_idl18/spatial/events/enums/zone_kind_enum/_events.py +31 -0
  32. spatialdds_idl18/spatial/mapping/__init__.py +30 -0
  33. spatialdds_idl18/spatial/mapping/_mapping.py +168 -0
  34. spatialdds_idl18/spatial/mapping/enums/__init__.py +19 -0
  35. spatialdds_idl18/spatial/mapping/enums/alignment_method/__init__.py +11 -0
  36. spatialdds_idl18/spatial/mapping/enums/alignment_method/_mapping.py +31 -0
  37. spatialdds_idl18/spatial/mapping/enums/edge_type/__init__.py +11 -0
  38. spatialdds_idl18/spatial/mapping/enums/edge_type/_mapping.py +34 -0
  39. spatialdds_idl18/spatial/mapping/enums/map_event_kind/__init__.py +11 -0
  40. spatialdds_idl18/spatial/mapping/enums/map_event_kind/_mapping.py +29 -0
  41. spatialdds_idl18/spatial/mapping/enums/map_kind/__init__.py +11 -0
  42. spatialdds_idl18/spatial/mapping/enums/map_kind/_mapping.py +32 -0
  43. spatialdds_idl18/spatial/mapping/enums/map_status/__init__.py +11 -0
  44. spatialdds_idl18/spatial/mapping/enums/map_status/_mapping.py +27 -0
  45. spatialdds_idl18/spatial/owm/__init__.py +22 -0
  46. spatialdds_idl18/spatial/owm/_owm.py +103 -0
  47. spatialdds_idl18/spatial/semantics/__init__.py +22 -0
  48. spatialdds_idl18/spatial/semantics/_semantics.py +174 -0
  49. spatialdds_idl18/spatial/sensing/__init__.py +21 -0
  50. spatialdds_idl18/spatial/sensing/common/__init__.py +34 -0
  51. spatialdds_idl18/spatial/sensing/common/_common.py +192 -0
  52. spatialdds_idl18/spatial/sensing/lidar/__init__.py +32 -0
  53. spatialdds_idl18/spatial/sensing/lidar/_lidar.py +135 -0
  54. spatialdds_idl18/spatial/sensing/rad/__init__.py +33 -0
  55. spatialdds_idl18/spatial/sensing/rad/_rad.py +191 -0
  56. spatialdds_idl18/spatial/sensing/radio/__init__.py +22 -0
  57. spatialdds_idl18/spatial/sensing/radio/_radio.py +141 -0
  58. spatialdds_idl18/spatial/sensing/rf_beam/__init__.py +25 -0
  59. spatialdds_idl18/spatial/sensing/rf_beam/_rf_beam.py +117 -0
  60. spatialdds_idl18/spatial/sensing/vision/__init__.py +36 -0
  61. spatialdds_idl18/spatial/sensing/vision/_vision.py +170 -0
  62. spatialdds_idl18/spatial/slam_frontend/__init__.py +22 -0
  63. spatialdds_idl18/spatial/slam_frontend/_slam_frontend.py +144 -0
  64. spatialdds_idl18/spatial/vio/__init__.py +20 -0
  65. spatialdds_idl18/spatial/vio/_vio.py +134 -0
  66. spatialdds_ros/__init__.py +6 -0
  67. spatialdds_ros/adapter.py +336 -0
  68. spatialdds_ros/announce.py +231 -0
  69. spatialdds_ros/config.py +293 -0
  70. spatialdds_ros/frames.py +219 -0
  71. spatialdds_ros/kinematic.py +140 -0
  72. spatialdds_ros/qos.py +101 -0
  73. spatialdds_ros/recording.py +167 -0
  74. spatialdds_ros/robot_node.py +287 -0
  75. spatialdds_ros/standalone.py +221 -0
  76. spatialdds_ros/tempo.py +114 -0
  77. spatialdds_ros/topics.py +64 -0
  78. spatialdds_ros/transport.py +170 -0
  79. spatialdds_ros-0.3.0.data/data/share/ament_index/resource_index/packages/spatialdds_ros +0 -0
  80. spatialdds_ros-0.3.0.data/data/share/spatialdds_ros/package.xml +29 -0
  81. spatialdds_ros-0.3.0.dist-info/METADATA +331 -0
  82. spatialdds_ros-0.3.0.dist-info/RECORD +87 -0
  83. spatialdds_ros-0.3.0.dist-info/WHEEL +5 -0
  84. spatialdds_ros-0.3.0.dist-info/entry_points.txt +3 -0
  85. spatialdds_ros-0.3.0.dist-info/licenses/LICENSE +21 -0
  86. spatialdds_ros-0.3.0.dist-info/top_level.txt +2 -0
  87. spatialdds_ros-0.3.0.dist-info/zip-safe +1 -0
@@ -0,0 +1,287 @@
1
+ """
2
+ The ROS 2 node: one process holding both stacks.
3
+
4
+ **rclpy is imported inside `main`, not at module scope.** That is not
5
+ tidiness. It keeps every other module in this package importable — and
6
+ therefore testable — on a machine with no ROS, which is where this library's
7
+ unit tests run and where its CI runs. The prototype does the same thing for
8
+ the same reason.
9
+
10
+ **Both stacks in one process is the proven pattern, not a gamble.** ROS
11
+ Humble's middleware is CycloneDDS 0.10.5 while the Python binding here is
12
+ 11.0.1, which sounds like it cannot work and does: the binding loads its own
13
+ `libddsc` and shares no symbols with `rmw_cyclonedds_cpp`'s. SpatialDDS and
14
+ ROS meet on the wire because both are DDS — here, in one process, with the
15
+ model read as model and ROS read as ROS. Nothing is relayed and nothing is
16
+ translated into an envelope type and back.
17
+ """
18
+ from __future__ import annotations
19
+
20
+ import math
21
+ import sys
22
+ from typing import Optional
23
+
24
+ from spatialdds_ros.config import Anchor, Config, ConfigError
25
+
26
+
27
+ def _declare(node) -> Config:
28
+ """
29
+ Read the parameter surface off the node, and let `Config` judge it.
30
+
31
+ Every parameter is declared with a *sentinel* default rather than a real
32
+ one for the three that must refuse to start, so "not set" is
33
+ distinguishable from "set to the empty string". `Config` is the single
34
+ place that decides what is acceptable; this function only collects.
35
+ """
36
+ node.declare_parameter("scene", "")
37
+ node.declare_parameter("entity_id", "")
38
+ node.declare_parameter("frame_id", "")
39
+ node.declare_parameter("source_id", "")
40
+ node.declare_parameter("label", "")
41
+ node.declare_parameter("domain", 0)
42
+ node.declare_parameter("ground_z", 0.0)
43
+ node.declare_parameter("pose_source", "tf")
44
+ node.declare_parameter("nav2_action", "navigate_to_pose")
45
+ node.declare_parameter("type_uri", "")
46
+ # String arrays, because that is what a launch file can express. Declared
47
+ # with empty defaults: an adapter states only what the operator
48
+ # configured, so "not set" has to be distinguishable from "set to
49
+ # nothing" — and both mean the same published empty sequence.
50
+ node.declare_parameter("content_refs", [""])
51
+ node.declare_parameter("external_refs", [""])
52
+ node.declare_parameter("base_frame", "base_link")
53
+ node.declare_parameter("map_frame", "map")
54
+ node.declare_parameter("odom_topic", "/odom")
55
+ node.declare_parameter("amcl_topic", "/amcl_pose")
56
+ node.declare_parameter("rate_hz", 10.0)
57
+ # Three separate parameters rather than one array, so a launch file that
58
+ # sets two of them fails on the third instead of silently shipping a
59
+ # two-element anchor.
60
+ node.declare_parameter("anchor_lat_deg", float("nan"))
61
+ node.declare_parameter("anchor_lon_deg", float("nan"))
62
+ node.declare_parameter("anchor_height_m", float("nan"))
63
+
64
+ def text(name: str) -> str:
65
+ return str(node.get_parameter(name).value or "")
66
+
67
+ def number(name: str) -> float:
68
+ return float(node.get_parameter(name).value)
69
+
70
+ lat, lon, height = (number("anchor_lat_deg"), number("anchor_lon_deg"),
71
+ number("anchor_height_m"))
72
+ given = [not math.isnan(v) for v in (lat, lon, height)]
73
+ if any(given) and not all(given):
74
+ raise ConfigError(
75
+ "an anchor needs all three of anchor_lat_deg, anchor_lon_deg and "
76
+ "anchor_height_m; a partial anchor is not a weaker claim about "
77
+ "where the frame is, it is an unusable one. Set all three, or none "
78
+ "— none is a supported mode and publishes no FrameTransform")
79
+ anchor = Anchor(lat, lon, height) if all(given) else None
80
+
81
+ kwargs = dict(
82
+ scene=text("scene"),
83
+ entity_id=text("entity_id"),
84
+ frame_id=text("frame_id"),
85
+ anchor=anchor,
86
+ domain=int(node.get_parameter("domain").value),
87
+ ground_z=number("ground_z"),
88
+ pose_source=text("pose_source") or "tf",
89
+ nav2_action=text("nav2_action") or "navigate_to_pose",
90
+ )
91
+ if text("source_id"):
92
+ kwargs["source_id"] = text("source_id")
93
+ if text("label"):
94
+ kwargs["label"] = text("label")
95
+ if text("type_uri"):
96
+ kwargs["type_uri"] = text("type_uri")
97
+
98
+ def strings(name: str) -> list:
99
+ raw = node.get_parameter(name).value or []
100
+ return [str(v).strip() for v in raw if str(v).strip()]
101
+
102
+ refs = strings("content_refs")
103
+ if refs:
104
+ kwargs["content_refs"] = refs
105
+ external = []
106
+ for item in strings("external_refs"):
107
+ if "=" not in item:
108
+ raise ConfigError(
109
+ f"external_refs entries are key=value; got {item!r}. A "
110
+ f"reference without a namespace cannot be resolved by "
111
+ f"anyone, which is worse than publishing none")
112
+ key, _, value = item.partition("=")
113
+ external.append((key.strip(), value.strip()))
114
+ if external:
115
+ kwargs["external_refs"] = external
116
+ return Config(**kwargs)
117
+
118
+
119
+ def main(argv=None) -> int: # pragma: no cover
120
+ import rclpy
121
+ from rclpy.node import Node
122
+
123
+ from cyclonedds.domain import DomainParticipant
124
+
125
+ from spatialdds_ros.adapter import RobotAdapter, already_published
126
+
127
+ rclpy.init(args=argv)
128
+
129
+ class SpatialDdsRobot(Node):
130
+ def __init__(self) -> None:
131
+ super().__init__("spatialdds_robot")
132
+ self.config = _declare(self)
133
+ for note in self.config.startup_notes():
134
+ self.get_logger().info(note)
135
+
136
+ self.participant = DomainParticipant(self.config.domain)
137
+ existing = already_published(self.participant, self.config)
138
+ if existing is not None:
139
+ raise SystemExit(
140
+ f"{self.config.entity_id} is already being published by "
141
+ f"{existing}. Refusing to start: one writer per key, and "
142
+ f"two of us would put one robot in two places with "
143
+ f"nothing reporting the conflict")
144
+
145
+ self.adapter = RobotAdapter(
146
+ self.participant, self.config,
147
+ navigator=_Nav2Navigator(self, self.config.nav2_action))
148
+ note = self.adapter.publish_georeference()
149
+ if note:
150
+ self.get_logger().info(note)
151
+
152
+ self._subscribe_to_the_pose()
153
+ period = 1.0 / float(self.get_parameter("rate_hz").value)
154
+ self.create_timer(period, self._tick)
155
+ self.get_logger().info(
156
+ f"owns {self.config.entity_id} on DDS domain "
157
+ f"{self.config.domain}; driving through "
158
+ f"{self.config.nav2_action}")
159
+
160
+ # --- the pose in ------------------------------------------------
161
+
162
+ def _subscribe_to_the_pose(self) -> None:
163
+ """
164
+ **A map-frame pose, and that is the whole of this method's care.**
165
+
166
+ The prototype read `nav_msgs/Odometry` on `/odom`, which is
167
+ map-frame *only* in its simulator, where `map -> odom` is static
168
+ and identity. On a localizing robot odom drifts against map — that
169
+ drift is the entire reason AMCL publishes a correction — so reading
170
+ it would publish a slowly-diverging claim about where the robot is
171
+ on the earth, with every number looking plausible. The census files
172
+ that against the demo; here the default simply is not `/odom`.
173
+
174
+ `tf` is the default because it is what nav2 itself consumes, works
175
+ whatever the localizer is, and assumes no topic name.
176
+ """
177
+ source = self.config.pose_source
178
+ if source == "tf":
179
+ from tf2_ros import Buffer, TransformListener
180
+ self._tf_buffer = Buffer()
181
+ self._tf_listener = TransformListener(self._tf_buffer, self)
182
+ self._read_pose = self._pose_from_tf
183
+ elif source == "amcl":
184
+ from geometry_msgs.msg import PoseWithCovarianceStamped
185
+ self.create_subscription(
186
+ PoseWithCovarianceStamped,
187
+ str(self.get_parameter("amcl_topic").value),
188
+ self._on_amcl, 10)
189
+ self._read_pose = lambda: None
190
+ else: # odom, warned about above
191
+ from nav_msgs.msg import Odometry
192
+ self.create_subscription(
193
+ Odometry, str(self.get_parameter("odom_topic").value),
194
+ self._on_odom, 20)
195
+ self._read_pose = lambda: None
196
+
197
+ def _pose_from_tf(self) -> None:
198
+ from rclpy.time import Time as RosTime
199
+ map_frame = str(self.get_parameter("map_frame").value)
200
+ base_frame = str(self.get_parameter("base_frame").value)
201
+ try:
202
+ tf = self._tf_buffer.lookup_transform(
203
+ map_frame, base_frame, RosTime())
204
+ except Exception: # noqa: BLE001
205
+ # Not an error worth logging every tick: a localizer that has
206
+ # not converged yet simply has no transform, and the adapter's
207
+ # own silence handling already describes that state correctly.
208
+ return
209
+ t = tf.transform.translation
210
+ self.adapter.observed(t.x, t.y, _yaw_of(tf.transform.rotation))
211
+
212
+ def _on_amcl(self, msg) -> None:
213
+ p = msg.pose.pose
214
+ self.adapter.observed(p.position.x, p.position.y,
215
+ _yaw_of(p.orientation))
216
+
217
+ def _on_odom(self, msg) -> None:
218
+ p = msg.pose.pose
219
+ self.adapter.observed(p.position.x, p.position.y,
220
+ _yaw_of(p.orientation))
221
+
222
+ # --- housekeeping ------------------------------------------------
223
+
224
+ def _tick(self) -> None:
225
+ self._read_pose()
226
+ for note in self.adapter.poll_commands():
227
+ self.get_logger().info(note)
228
+ note = self.adapter.tick()
229
+ if note:
230
+ self.get_logger().info(note)
231
+
232
+ class _Nav2Navigator:
233
+ """nav2, asked properly and reported honestly."""
234
+
235
+ def __init__(self, node, action_name: str) -> None:
236
+ from nav2_msgs.action import NavigateToPose
237
+ from rclpy.action import ActionClient
238
+ self._node = node
239
+ self._goal_type = NavigateToPose
240
+ self._client = ActionClient(node, NavigateToPose, action_name)
241
+ self._action_name = action_name
242
+
243
+ def available(self) -> Optional[str]:
244
+ if not self._client.server_is_ready():
245
+ # One reason, said plainly. "nav2 is down" and "nav2 is up but
246
+ # inactive" look identical from here and lead to the same
247
+ # place: nothing will drive.
248
+ return (f"nav2 is not accepting goals (no {self._action_name} "
249
+ f"server)")
250
+ return None
251
+
252
+ def send(self, x: float, y: float) -> str:
253
+ goal = self._goal_type.Goal()
254
+ goal.pose.header.frame_id = str(
255
+ self._node.get_parameter("map_frame").value)
256
+ goal.pose.header.stamp = self._node.get_clock().now().to_msg()
257
+ goal.pose.pose.position.x = x
258
+ goal.pose.pose.position.y = y
259
+ goal.pose.pose.position.z = self._node.config.ground_z
260
+ goal.pose.pose.orientation.w = 1.0
261
+ self._client.send_goal_async(goal)
262
+ return f"nav2 accepted a goal at ({x:.2f}, {y:.2f})"
263
+
264
+ try:
265
+ node = SpatialDdsRobot()
266
+ except ConfigError as error:
267
+ print(str(error), file=sys.stderr)
268
+ rclpy.shutdown()
269
+ return 2
270
+ try:
271
+ rclpy.spin(node)
272
+ except KeyboardInterrupt:
273
+ pass
274
+ finally:
275
+ node.destroy_node()
276
+ rclpy.shutdown()
277
+ return 0
278
+
279
+
280
+ def _yaw_of(q) -> float:
281
+ """Yaw from a ROS quaternion message."""
282
+ return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
283
+ 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
284
+
285
+
286
+ if __name__ == "__main__": # pragma: no cover
287
+ sys.exit(main())
@@ -0,0 +1,221 @@
1
+ """
2
+ The no-hardware mode, wired up and runnable.
3
+
4
+ python3 -m spatialdds_ros.standalone --scene bare-scene \
5
+ --entity-id ent:robot:r1 --frame-id map/bare-scene
6
+
7
+ A robot on the bus in one command, with no ROS, no nav2 and no hardware. It is
8
+ the try-it story, and it is also the harness the acceptance run drives — the
9
+ same code path a person follows from the README, rather than a test-only
10
+ scaffold that could pass while the documented path was broken.
11
+
12
+ Everything here is wiring. The behaviour lives in `RobotAdapter` (what is
13
+ published, what is declined, what is refused), `KinematicSource` (how it
14
+ moves) and `Announcer` (how it is found).
15
+ """
16
+ from __future__ import annotations
17
+
18
+ import argparse
19
+ import signal
20
+ import sys
21
+ import time
22
+ from typing import Optional
23
+
24
+ from cyclonedds.domain import DomainParticipant
25
+
26
+ from spatialdds_ros.adapter import RobotAdapter, already_published
27
+ from spatialdds_ros.announce import Announcer
28
+ from spatialdds_ros.config import Anchor, Config, ConfigError
29
+ from spatialdds_ros.kinematic import (
30
+ Bounds, KinematicNavigator, KinematicSource)
31
+
32
+ DEFAULT_RATE_HZ = 10.0
33
+
34
+
35
+ class Runner:
36
+ """
37
+ One robot, driven by the kinematic source, for as long as it is stepped.
38
+
39
+ Split from `main` so the acceptance test drives exactly what a person
40
+ running the command drives — `main` only parses arguments and loops.
41
+ """
42
+
43
+ def __init__(self, config: Config, *, rate_hz: float = DEFAULT_RATE_HZ,
44
+ start: tuple = (0.0, 0.0), speed: Optional[float] = None,
45
+ bounds: Optional[Bounds] = None,
46
+ announce_kind: str = "OTHER") -> None:
47
+ self.config = config
48
+ self.rate_hz = rate_hz
49
+ self.interval = 1.0 / rate_hz
50
+ self.participant = DomainParticipant(config.domain)
51
+
52
+ existing = already_published(self.participant, config)
53
+ if existing is not None:
54
+ raise SystemExit(
55
+ f"{config.entity_id} is already being published by "
56
+ f"{existing}. Refusing to start: one writer per key, and two "
57
+ f"of us would put one robot in two places with nothing "
58
+ f"reporting the conflict")
59
+
60
+ self.source = KinematicSource(
61
+ start=start, speed=speed or 0.6, bounds=bounds)
62
+ self.adapter = RobotAdapter(
63
+ self.participant, config,
64
+ navigator=KinematicNavigator(self.source))
65
+ self.announcer = Announcer(self.participant, config, announce_kind)
66
+ self._announced_at = 0.0
67
+ self.notes: list[str] = []
68
+
69
+ def start(self) -> list[str]:
70
+ """Say what mode this is, announce, and publish the georeference."""
71
+ notes = list(self.config.startup_notes())
72
+ notes.append(self.announcer.publish())
73
+ self._announced_at = time.monotonic()
74
+ georeference = self.adapter.publish_georeference()
75
+ if georeference:
76
+ notes.append(georeference)
77
+ self.notes.extend(notes)
78
+ return notes
79
+
80
+ def step(self, dt: Optional[float] = None) -> list[str]:
81
+ """One iteration. Returns whatever is worth logging."""
82
+ dt = self.interval if dt is None else dt
83
+ notes = list(self.adapter.poll_commands())
84
+
85
+ x, y, yaw = self.source.step(dt)
86
+ abandoned = self.source.abandoned
87
+ if abandoned:
88
+ self.source.abandoned = None
89
+ self.adapter.observed(x, y, yaw)
90
+ if abandoned:
91
+ # After `observed`, so the amended record carries the current pose
92
+ # rather than being overwritten by it on the next line.
93
+ self.adapter.refused(abandoned)
94
+ notes.append(abandoned)
95
+
96
+ note = self.adapter.tick()
97
+ if note:
98
+ notes.append(note)
99
+
100
+ # Re-announce well inside the TTL, so one missed refresh is not an
101
+ # outage.
102
+ if (time.monotonic() - self._announced_at
103
+ > self.announcer.refresh_interval_s):
104
+ notes.append(self.announcer.publish())
105
+ self._announced_at = time.monotonic()
106
+
107
+ self.notes.extend(notes)
108
+ return notes
109
+
110
+ def run(self) -> int: # pragma: no cover
111
+ stop = False
112
+
113
+ def _stop(_signum, _frame):
114
+ nonlocal stop
115
+ stop = True
116
+
117
+ signal.signal(signal.SIGINT, _stop)
118
+ signal.signal(signal.SIGTERM, _stop)
119
+
120
+ for note in self.start():
121
+ print(f"robot: {note}", flush=True)
122
+ reported = 0
123
+ while not stop:
124
+ for note in self.step():
125
+ print(f"robot: {note}", flush=True)
126
+ reported += 1
127
+ if reported % int(self.rate_hz * 20) == 0:
128
+ print(f"robot: {reported} poses; at "
129
+ f"({self.source.x:.2f}, {self.source.y:.2f})", flush=True)
130
+ time.sleep(self.interval)
131
+ print("robot: stopping.", flush=True)
132
+ return 0
133
+
134
+
135
+ def _parser() -> argparse.ArgumentParser:
136
+ p = argparse.ArgumentParser(
137
+ description="Put a kinematic robot on a SpatialDDS bus, with no ROS")
138
+ # The three with no default, per the ruling. argparse does not mark them
139
+ # required: `Config` is the single place that decides, and its refusal
140
+ # explains *why* each has no default, which `argparse` cannot.
141
+ p.add_argument("--scene", default="")
142
+ p.add_argument("--entity-id", default="")
143
+ p.add_argument("--frame-id", default="")
144
+ p.add_argument("--source-id", default="")
145
+ p.add_argument("--label", default="")
146
+ p.add_argument("--domain", type=int, default=0)
147
+ p.add_argument("--ground-z", type=float, default=0.0)
148
+ p.add_argument("--rate-hz", type=float, default=DEFAULT_RATE_HZ)
149
+ p.add_argument("--speed", type=float, default=0.6)
150
+ p.add_argument("--start", default="0,0", metavar="X,Y")
151
+ p.add_argument("--anchor", default="", metavar="LAT,LON,HEIGHT",
152
+ help="publish a FrameTransform for this frame. Omitted, "
153
+ "none is published and the robot is simply not "
154
+ "earth-referenced — a supported mode")
155
+ p.add_argument("--bounds", default="", metavar="MINX,MINY,MAXX,MAXY",
156
+ help="a box this source will not leave, so the late-refusal "
157
+ "path can be exercised without hardware")
158
+ p.add_argument("--announce-kind", default="OTHER")
159
+ p.add_argument("--content-ref", action="append", default=[],
160
+ metavar="catalog:ID",
161
+ help="what this robot is, for a consumer that wants to "
162
+ "draw it. Repeatable. Omitted, the record carries an "
163
+ "empty sequence and a consumer renders a marker")
164
+ p.add_argument("--external-ref", action="append", default=[],
165
+ metavar="KEY=VALUE",
166
+ help="what it is also known as, e.g. wikidata=Q4810574. "
167
+ "Repeatable")
168
+ return p
169
+
170
+
171
+ def _config_from(args) -> tuple:
172
+ anchor = None
173
+ if args.anchor:
174
+ parts = [float(v) for v in args.anchor.split(",")]
175
+ if len(parts) != 3:
176
+ raise ConfigError(
177
+ "--anchor needs LAT,LON,HEIGHT — all three. A partial anchor "
178
+ "is not a weaker claim about where the frame is, it is an "
179
+ "unusable one; omit it entirely for the no-georeference mode")
180
+ anchor = Anchor(*parts)
181
+ bounds = None
182
+ if args.bounds:
183
+ parts = [float(v) for v in args.bounds.split(",")]
184
+ if len(parts) != 4:
185
+ raise ConfigError("--bounds needs MINX,MINY,MAXX,MAXY")
186
+ bounds = Bounds(*parts)
187
+ start = tuple(float(v) for v in args.start.split(","))
188
+ external = []
189
+ for item in (args.external_ref or []):
190
+ if "=" not in item:
191
+ raise ConfigError(
192
+ f"--external-ref takes KEY=VALUE; got {item!r}. A reference "
193
+ f"without a namespace cannot be resolved by anyone, which is "
194
+ f"worse than publishing none")
195
+ key, _, value = item.partition("=")
196
+ external.append((key.strip(), value.strip()))
197
+ kwargs = dict(scene=args.scene, entity_id=args.entity_id,
198
+ frame_id=args.frame_id, anchor=anchor, domain=args.domain,
199
+ ground_z=args.ground_z,
200
+ content_refs=tuple(args.content_ref or ()),
201
+ external_refs=tuple(external))
202
+ if args.source_id:
203
+ kwargs["source_id"] = args.source_id
204
+ if args.label:
205
+ kwargs["label"] = args.label
206
+ return Config(**kwargs), start, bounds
207
+
208
+
209
+ def main(argv=None) -> int: # pragma: no cover
210
+ args = _parser().parse_args(argv)
211
+ try:
212
+ config, start, bounds = _config_from(args)
213
+ except ConfigError as error:
214
+ print(str(error), file=sys.stderr)
215
+ return 2
216
+ return Runner(config, rate_hz=args.rate_hz, start=start, speed=args.speed,
217
+ bounds=bounds, announce_kind=args.announce_kind).run()
218
+
219
+
220
+ if __name__ == "__main__": # pragma: no cover
221
+ sys.exit(main())
@@ -0,0 +1,114 @@
1
+ """
2
+ When to refresh a latched record, and when to call a feed silent.
3
+
4
+ **The derivation is the convention, not any of the numbers.** A fixed silence
5
+ threshold shorter than the update interval makes every gap between updates
6
+ look like a stop. Measured in the prototype: a one-second threshold against a
7
+ mover giving each duck a turn every 1.5 s produced 94 "it stopped moving"
8
+ flushes while nothing had stopped, and "the fast tier quietly became the slow
9
+ one" (`spatialdds_demo/tempo.py`).
10
+
11
+ That is why this is not a parameter. A robot's publish rate is whatever ROS
12
+ publishes at — 20 Hz from a simulator, 5 Hz from an AMCL, something else on
13
+ hardware under load — and a library cannot know it. So it is measured per key
14
+ and the threshold follows it.
15
+
16
+ No I/O and no DDS: this answers questions and the caller decides what to
17
+ write, which keeps the single-writer rule with whoever owns the instance and
18
+ keeps this testable with no bus.
19
+ """
20
+ from __future__ import annotations
21
+
22
+ import time
23
+ from typing import Dict, List, Optional
24
+
25
+ #: A FAST entity's every update goes out on the pose lane; its latched record
26
+ #: is refreshed every Nth, so a late joiner is at most N-1 updates stale and
27
+ #: converges on the next pose.
28
+ LATCH_EVERY_N = 5
29
+
30
+ #: The floor under the idle threshold, and the multiple of an entity's own
31
+ #: observed interval taken as evidence it has stopped. A thing updating every
32
+ #: 100 ms is idle after a beat; a thing updating every ten seconds is not idle
33
+ #: after eleven.
34
+ IDLE_FLOOR_S = 1.0
35
+ IDLE_FACTOR = 3.0
36
+
37
+ #: How much of the observed interval survives each new measurement. Low enough
38
+ #: to follow a cadence change within a few updates, high enough that one late
39
+ #: sample does not convince it everything stopped.
40
+ GAP_SMOOTHING = 0.7
41
+
42
+
43
+ class Tempo:
44
+ """Per-key bookkeeping for a fast lane."""
45
+
46
+ def __init__(self, latch_every: int = LATCH_EVERY_N,
47
+ idle_floor: float = IDLE_FLOOR_S,
48
+ idle_factor: float = IDLE_FACTOR,
49
+ smoothing: float = GAP_SMOOTHING) -> None:
50
+ self.latch_every = latch_every
51
+ self.idle_floor = idle_floor
52
+ self.idle_factor = idle_factor
53
+ self.smoothing = smoothing
54
+ self._pending: Dict[str, int] = {}
55
+ self._last: Dict[str, float] = {}
56
+ self._gap: Dict[str, float] = {}
57
+
58
+ def observed(self, key: str, now: Optional[float] = None) -> bool:
59
+ """
60
+ Record an update. Returns whether the latched record is now due.
61
+
62
+ The caller publishes the fast sample either way; this decides only
63
+ about the expensive one.
64
+ """
65
+ now = time.time() if now is None else now
66
+ previous = self._last.get(key)
67
+ if previous is not None:
68
+ gap = now - previous
69
+ known = self._gap.get(key)
70
+ self._gap[key] = (gap if known is None
71
+ else self.smoothing * known
72
+ + (1 - self.smoothing) * gap)
73
+ self._last[key] = now
74
+
75
+ pending = self._pending.get(key, 0) + 1
76
+ if pending >= self.latch_every:
77
+ self._pending[key] = 0
78
+ return True
79
+ self._pending[key] = pending
80
+ return False
81
+
82
+ def threshold(self, key: str) -> float:
83
+ """How long without an update means this key has stopped."""
84
+ gap = self._gap.get(key)
85
+ if gap is None:
86
+ # One update is not a cadence. Until there are two, the floor is
87
+ # all there is to go on — which costs a slow feed one uneconomical
88
+ # but correct flush while its cadence is learned.
89
+ return self.idle_floor
90
+ return max(self.idle_floor, self.idle_factor * gap)
91
+
92
+ def idle(self, now: Optional[float] = None) -> List[str]:
93
+ """
94
+ Keys with pending changes that have gone quiet, marked clean.
95
+
96
+ So calling it every tick republishes a still world exactly once rather
97
+ than every tick.
98
+ """
99
+ now = time.time() if now is None else now
100
+ due = []
101
+ for key, pending in list(self._pending.items()):
102
+ if pending == 0:
103
+ continue
104
+ if now - self._last.get(key, 0.0) < self.threshold(key):
105
+ continue
106
+ self._pending[key] = 0
107
+ due.append(key)
108
+ return due
109
+
110
+ def forget(self, key: str) -> None:
111
+ """Stop tracking a key — it retired, or was disposed."""
112
+ self._pending.pop(key, None)
113
+ self._last.pop(key, None)
114
+ self._gap.pop(key, None)
@@ -0,0 +1,64 @@
1
+ """
2
+ Topic names, derived from the scene and never configured.
3
+
4
+ §3.3.1's five-segment grammar: `spatialdds/{scene}/{group}/{type}/{version}`.
5
+ The scene is the only free variable; the names below are that grammar applied
6
+ to it.
7
+
8
+ **Why these are not parameters.** A topic name is the whole of interop — two
9
+ participants meet because they chose the same string and for no other reason.
10
+ Exposing the strings would let an operator produce a name nothing else looks
11
+ for, and the symptom is an empty bus: no error, no warning, no mismatch
12
+ reported anywhere, just a robot publishing into a topic with no readers and a
13
+ scene that never sees it. There is one right answer per scene, so the library
14
+ fills it.
15
+
16
+ The prototype had the same four names with the scene baked into module-level
17
+ constants (`spatialdds_demo/topics.py`), which is the same decision made for a
18
+ single scene.
19
+ """
20
+ from __future__ import annotations
21
+
22
+ from dataclasses import dataclass
23
+
24
+ #: The frame that `earth-fixed` names, shared by every participant that
25
+ #: georeferences anything. Not a parameter: it is the one frame name that must
26
+ #: agree across the whole bus, and the prototype, the catalogue and the spec
27
+ #: all spell it this way.
28
+ EARTH_FIXED_FQN = "earth-fixed"
29
+
30
+
31
+ @dataclass(frozen=True)
32
+ class Topics:
33
+ """The four lanes this library uses, for one scene."""
34
+
35
+ scene: str
36
+
37
+ @property
38
+ def entity(self) -> str:
39
+ """Latched records: what exists, and what it is."""
40
+ return f"spatialdds/{self.scene}/world/entity/v1"
41
+
42
+ @property
43
+ def pose(self) -> str:
44
+ """The fast lane: where it is now."""
45
+ return f"spatialdds/{self.scene}/world/model_pose/v1"
46
+
47
+ @property
48
+ def command(self) -> str:
49
+ """Requests addressed to something in the scene."""
50
+ return f"spatialdds/{self.scene}/world/model_command/v1"
51
+
52
+ @property
53
+ def frame_transform(self) -> str:
54
+ """Where the scene's frames sit relative to one another."""
55
+ return f"spatialdds/{self.scene}/frames/frame_transform/v1"
56
+
57
+ def all(self) -> dict[str, str]:
58
+ """Every name, for a startup banner and for the tests to check."""
59
+ return {
60
+ "entity": self.entity,
61
+ "pose": self.pose,
62
+ "command": self.command,
63
+ "frame_transform": self.frame_transform,
64
+ }