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.
- spatialdds_idl18/PROVENANCE.md +66 -0
- spatialdds_idl18/__init__.py +9 -0
- spatialdds_idl18/_field_aliases.py +26 -0
- spatialdds_idl18/builtin/__init__.py +11 -0
- spatialdds_idl18/builtin/_types.py +28 -0
- spatialdds_idl18/spatial/__init__.py +33 -0
- spatialdds_idl18/spatial/anchors/__init__.py +20 -0
- spatialdds_idl18/spatial/anchors/_anchors.py +102 -0
- spatialdds_idl18/spatial/argeo/__init__.py +22 -0
- spatialdds_idl18/spatial/argeo/_argeo.py +97 -0
- spatialdds_idl18/spatial/common/__init__.py +27 -0
- spatialdds_idl18/spatial/common/_types.py +104 -0
- spatialdds_idl18/spatial/core/GnssService/__init__.py +17 -0
- spatialdds_idl18/spatial/core/GnssService/_core.py +27 -0
- spatialdds_idl18/spatial/core/__init__.py +40 -0
- spatialdds_idl18/spatial/core/_core.py +338 -0
- spatialdds_idl18/spatial/disco/__init__.py +34 -0
- spatialdds_idl18/spatial/disco/_discovery.py +252 -0
- spatialdds_idl18/spatial/events/__init__.py +29 -0
- spatialdds_idl18/spatial/events/_events.py +164 -0
- spatialdds_idl18/spatial/events/enums/__init__.py +19 -0
- spatialdds_idl18/spatial/events/enums/crossing_direction_enum/__init__.py +11 -0
- spatialdds_idl18/spatial/events/enums/crossing_direction_enum/_events.py +25 -0
- spatialdds_idl18/spatial/events/enums/event_state_enum/__init__.py +11 -0
- spatialdds_idl18/spatial/events/enums/event_state_enum/_events.py +26 -0
- spatialdds_idl18/spatial/events/enums/event_type_enum/__init__.py +11 -0
- spatialdds_idl18/spatial/events/enums/event_type_enum/_events.py +36 -0
- spatialdds_idl18/spatial/events/enums/severity_enum/__init__.py +11 -0
- spatialdds_idl18/spatial/events/enums/severity_enum/_events.py +26 -0
- spatialdds_idl18/spatial/events/enums/zone_kind_enum/__init__.py +11 -0
- spatialdds_idl18/spatial/events/enums/zone_kind_enum/_events.py +31 -0
- spatialdds_idl18/spatial/mapping/__init__.py +30 -0
- spatialdds_idl18/spatial/mapping/_mapping.py +168 -0
- spatialdds_idl18/spatial/mapping/enums/__init__.py +19 -0
- spatialdds_idl18/spatial/mapping/enums/alignment_method/__init__.py +11 -0
- spatialdds_idl18/spatial/mapping/enums/alignment_method/_mapping.py +31 -0
- spatialdds_idl18/spatial/mapping/enums/edge_type/__init__.py +11 -0
- spatialdds_idl18/spatial/mapping/enums/edge_type/_mapping.py +34 -0
- spatialdds_idl18/spatial/mapping/enums/map_event_kind/__init__.py +11 -0
- spatialdds_idl18/spatial/mapping/enums/map_event_kind/_mapping.py +29 -0
- spatialdds_idl18/spatial/mapping/enums/map_kind/__init__.py +11 -0
- spatialdds_idl18/spatial/mapping/enums/map_kind/_mapping.py +32 -0
- spatialdds_idl18/spatial/mapping/enums/map_status/__init__.py +11 -0
- spatialdds_idl18/spatial/mapping/enums/map_status/_mapping.py +27 -0
- spatialdds_idl18/spatial/owm/__init__.py +22 -0
- spatialdds_idl18/spatial/owm/_owm.py +103 -0
- spatialdds_idl18/spatial/semantics/__init__.py +22 -0
- spatialdds_idl18/spatial/semantics/_semantics.py +174 -0
- spatialdds_idl18/spatial/sensing/__init__.py +21 -0
- spatialdds_idl18/spatial/sensing/common/__init__.py +34 -0
- spatialdds_idl18/spatial/sensing/common/_common.py +192 -0
- spatialdds_idl18/spatial/sensing/lidar/__init__.py +32 -0
- spatialdds_idl18/spatial/sensing/lidar/_lidar.py +135 -0
- spatialdds_idl18/spatial/sensing/rad/__init__.py +33 -0
- spatialdds_idl18/spatial/sensing/rad/_rad.py +191 -0
- spatialdds_idl18/spatial/sensing/radio/__init__.py +22 -0
- spatialdds_idl18/spatial/sensing/radio/_radio.py +141 -0
- spatialdds_idl18/spatial/sensing/rf_beam/__init__.py +25 -0
- spatialdds_idl18/spatial/sensing/rf_beam/_rf_beam.py +117 -0
- spatialdds_idl18/spatial/sensing/vision/__init__.py +36 -0
- spatialdds_idl18/spatial/sensing/vision/_vision.py +170 -0
- spatialdds_idl18/spatial/slam_frontend/__init__.py +22 -0
- spatialdds_idl18/spatial/slam_frontend/_slam_frontend.py +144 -0
- spatialdds_idl18/spatial/vio/__init__.py +20 -0
- spatialdds_idl18/spatial/vio/_vio.py +134 -0
- spatialdds_ros/__init__.py +6 -0
- spatialdds_ros/adapter.py +336 -0
- spatialdds_ros/announce.py +231 -0
- spatialdds_ros/config.py +293 -0
- spatialdds_ros/frames.py +219 -0
- spatialdds_ros/kinematic.py +140 -0
- spatialdds_ros/qos.py +101 -0
- spatialdds_ros/recording.py +167 -0
- spatialdds_ros/robot_node.py +287 -0
- spatialdds_ros/standalone.py +221 -0
- spatialdds_ros/tempo.py +114 -0
- spatialdds_ros/topics.py +64 -0
- spatialdds_ros/transport.py +170 -0
- spatialdds_ros-0.3.0.data/data/share/ament_index/resource_index/packages/spatialdds_ros +0 -0
- spatialdds_ros-0.3.0.data/data/share/spatialdds_ros/package.xml +29 -0
- spatialdds_ros-0.3.0.dist-info/METADATA +331 -0
- spatialdds_ros-0.3.0.dist-info/RECORD +87 -0
- spatialdds_ros-0.3.0.dist-info/WHEEL +5 -0
- spatialdds_ros-0.3.0.dist-info/entry_points.txt +3 -0
- spatialdds_ros-0.3.0.dist-info/licenses/LICENSE +21 -0
- spatialdds_ros-0.3.0.dist-info/top_level.txt +2 -0
- 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())
|
spatialdds_ros/tempo.py
ADDED
|
@@ -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)
|
spatialdds_ros/topics.py
ADDED
|
@@ -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
|
+
}
|