OpenSMI-ROS2 0.1.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.
- opensmi/ros2/__init__.py +89 -0
- opensmi/ros2/__main__.py +11 -0
- opensmi/ros2/cli.py +143 -0
- opensmi/ros2/py.typed +0 -0
- opensmi/ros2/runtime/__init__.py +67 -0
- opensmi/ros2/runtime/_decode.py +101 -0
- opensmi/ros2/runtime/_encode.py +102 -0
- opensmi/ros2/runtime/_parser.py +147 -0
- opensmi/ros2/runtime/errors.py +141 -0
- opensmi/ros2/runtime/goal.py +232 -0
- opensmi/ros2/runtime/graph.py +288 -0
- opensmi/ros2/runtime/guard.py +213 -0
- opensmi/ros2/runtime/interface.py +140 -0
- opensmi/ros2/runtime/messages/__init__.py +503 -0
- opensmi/ros2/runtime/messages/industrial_msgs.py +62 -0
- opensmi/ros2/runtime/messages/industrial_msgs.pyi +159 -0
- opensmi/ros2/runtime/messages/motoros2_interfaces.py +79 -0
- opensmi/ros2/runtime/messages/motoros2_interfaces.pyi +307 -0
- opensmi/ros2/runtime/messages/neo_msgs2.py +55 -0
- opensmi/ros2/runtime/messages/neo_msgs2.pyi +138 -0
- opensmi/ros2/runtime/messages/neo_srvs2.py +72 -0
- opensmi/ros2/runtime/messages/neo_srvs2.pyi +224 -0
- opensmi/ros2/runtime/outcomes.py +103 -0
- opensmi/ros2/runtime/parameters.py +723 -0
- opensmi/ros2/runtime/publisher.py +125 -0
- opensmi/ros2/runtime/qos.py +394 -0
- opensmi/ros2/runtime/runtime.py +1171 -0
- opensmi/ros2/runtime/selection.py +150 -0
- opensmi/ros2/runtime/stamp.py +100 -0
- opensmi/ros2/runtime/subscription.py +354 -0
- opensmi/ros2/runtime/tf.py +714 -0
- opensmi/ros2/server/__init__.py +148 -0
- opensmi/ros2/server/access.py +109 -0
- opensmi/ros2/server/bindings/__init__.py +92 -0
- opensmi/ros2/server/bindings/_base.py +154 -0
- opensmi/ros2/server/bindings/action.py +133 -0
- opensmi/ros2/server/bindings/keyed.py +127 -0
- opensmi/ros2/server/bindings/lifecycle.py +131 -0
- opensmi/ros2/server/bindings/method.py +130 -0
- opensmi/ros2/server/bindings/monitoring.py +109 -0
- opensmi/ros2/server/bindings/parameter.py +133 -0
- opensmi/ros2/server/bindings/publisher.py +192 -0
- opensmi/ros2/server/bindings/registry.py +95 -0
- opensmi/ros2/server/bindings/tf.py +256 -0
- opensmi/ros2/server/browse_names.py +94 -0
- opensmi/ros2/server/component.py +334 -0
- opensmi/ros2/server/config.py +493 -0
- opensmi/ros2/server/containers.py +191 -0
- opensmi/ros2/server/drivers.py +421 -0
- opensmi/ros2/server/errors.py +159 -0
- opensmi/ros2/server/events.py +406 -0
- opensmi/ros2/server/introspection.py +850 -0
- opensmi/ros2/server/keyed.py +749 -0
- opensmi/ros2/server/lifecycle.py +238 -0
- opensmi/ros2/server/limits.py +66 -0
- opensmi/ros2/server/loader.py +977 -0
- opensmi/ros2/server/machine.py +225 -0
- opensmi/ros2/server/method.py +307 -0
- opensmi/ros2/server/mirror.py +582 -0
- opensmi/ros2/server/mutation.py +992 -0
- opensmi/ros2/server/nodesets/Opc.Ua.SmartFactory.ROS2.NodeSet2.xml +85 -0
- opensmi/ros2/server/nodesets/__init__.py +18 -0
- opensmi/ros2/server/nodesets/smart_factory_ros2.py +26 -0
- opensmi/ros2/server/parameters.py +704 -0
- opensmi/ros2/server/projection.py +1176 -0
- opensmi/ros2/server/projections/__init__.py +41 -0
- opensmi/ros2/server/projections/scope.py +222 -0
- opensmi/ros2/server/provenance.py +121 -0
- opensmi/ros2/server/publisher.py +781 -0
- opensmi/ros2/server/recipes.py +514 -0
- opensmi/ros2/server/required.py +633 -0
- opensmi/ros2/server/skill.py +363 -0
- opensmi/ros2/server/spatial.py +283 -0
- opensmi/ros2/server/startup.py +626 -0
- opensmi/ros2/server/status.py +107 -0
- opensmi/ros2/server/structures.py +135 -0
- opensmi/ros2/server/transforms/__init__.py +39 -0
- opensmi/ros2/server/transforms/conversions.py +58 -0
- opensmi/ros2/server/transforms/filing.py +340 -0
- opensmi/ros2/server/transforms/mirror.py +462 -0
- opensmi/ros2/server/transforms/outward.py +180 -0
- opensmi/ros2/server/transforms/records.py +195 -0
- opensmi/ros2/server/transforms/spatial.py +154 -0
- opensmi_ros2-0.1.0.dist-info/METADATA +183 -0
- opensmi_ros2-0.1.0.dist-info/RECORD +89 -0
- opensmi_ros2-0.1.0.dist-info/WHEEL +5 -0
- opensmi_ros2-0.1.0.dist-info/entry_points.txt +2 -0
- opensmi_ros2-0.1.0.dist-info/licenses/LICENSES/MIT.txt +18 -0
- opensmi_ros2-0.1.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,462 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Inward mirroring: the poll that renders the frame network as Locations."""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
import asyncio
|
|
10
|
+
from collections.abc import AsyncGenerator
|
|
11
|
+
from typing import TYPE_CHECKING, Any, Final
|
|
12
|
+
|
|
13
|
+
from asyncua import ua
|
|
14
|
+
from opensmi.core.lifecycle_mixin import lifecycle
|
|
15
|
+
from opensmi.server.base_machinery_item import BaseMachineryItem
|
|
16
|
+
from opensmi.server.spatial_object import Orientation, Pose, Position
|
|
17
|
+
|
|
18
|
+
from opensmi.ros2.runtime.tf import FrameLink
|
|
19
|
+
from opensmi.ros2.server.errors import (
|
|
20
|
+
FrameDirectionError,
|
|
21
|
+
)
|
|
22
|
+
from opensmi.ros2.server.events import Severity
|
|
23
|
+
from opensmi.ros2.server.mutation import ended
|
|
24
|
+
from opensmi.ros2.server.spatial import (
|
|
25
|
+
angles,
|
|
26
|
+
attach_points,
|
|
27
|
+
excluded,
|
|
28
|
+
one_direction_per_frame,
|
|
29
|
+
owners,
|
|
30
|
+
parents_of,
|
|
31
|
+
tree_roots,
|
|
32
|
+
)
|
|
33
|
+
from opensmi.ros2.server.status import GOOD, status_value
|
|
34
|
+
from opensmi.ros2.server.transforms.conversions import _LEVEL, _ZERO, DEGREES, METRES
|
|
35
|
+
from opensmi.ros2.server.transforms.outward import _OutwardPublishing
|
|
36
|
+
from opensmi.ros2.server.transforms.records import (
|
|
37
|
+
_Bearers,
|
|
38
|
+
_Frame,
|
|
39
|
+
_Owner,
|
|
40
|
+
_Quiet,
|
|
41
|
+
)
|
|
42
|
+
|
|
43
|
+
if TYPE_CHECKING:
|
|
44
|
+
from collections.abc import Iterator, Mapping, Sequence
|
|
45
|
+
|
|
46
|
+
from asyncua.common.node import Node
|
|
47
|
+
|
|
48
|
+
|
|
49
|
+
SEPARATE_FRAME_TREES: Final = "SEPARATE_FRAME_TREES"
|
|
50
|
+
"""The BindingEvent code more than one root in the rendered frame network carries.
|
|
51
|
+
|
|
52
|
+
A well-formed TF tree has one root, which OPC 10000-210 calls the World Frame. With several,
|
|
53
|
+
unrelated coordinate systems are presented as one.
|
|
54
|
+
"""
|
|
55
|
+
|
|
56
|
+
|
|
57
|
+
class ROS2TransformMirrorMixin(_OutwardPublishing):
|
|
58
|
+
"""Keeps every SpatialObject on one Machine in step with the TF tree, on a timer.
|
|
59
|
+
|
|
60
|
+
Machine only: a frame belongs to the nearest declared head above it, and the heads are spread
|
|
61
|
+
over the Machine and its Components. The period is declared on the Machine for the same reason
|
|
62
|
+
(:attr:`~opensmi.ros2.server.config.TransformConfig.interval`). How many Locations one TF
|
|
63
|
+
Binding may gain is :attr:`~opensmi.ros2.server.mutation.ROS2MutationMixin.mutation_node_cap` on
|
|
64
|
+
the declaring item; a robot with more frames says so once.
|
|
65
|
+
"""
|
|
66
|
+
|
|
67
|
+
def __init__(self, *args: Any, **kwargs: Any) -> None:
|
|
68
|
+
"""Start out with nothing rendered, and pass every argument to opensmi."""
|
|
69
|
+
super().__init__(*args, **kwargs)
|
|
70
|
+
self._transform_interval = 0.0
|
|
71
|
+
self._transform_task: asyncio.Task[None] | None = None
|
|
72
|
+
self._transform_polls = 0
|
|
73
|
+
self._transform_outward: frozenset[str] = frozenset()
|
|
74
|
+
self._transform_roots: tuple[str, ...] = ()
|
|
75
|
+
self._transform_quiet_after = 0.0
|
|
76
|
+
|
|
77
|
+
@property
|
|
78
|
+
def ros2_transform_polls(self) -> int:
|
|
79
|
+
"""Return how often the frame network has been read, which a Machine with no TF leaves at 0."""
|
|
80
|
+
return self._transform_polls
|
|
81
|
+
|
|
82
|
+
@property
|
|
83
|
+
def ros2_frame_nodes(self) -> Mapping[str, str]:
|
|
84
|
+
"""Return the frame-to-node index: every rendered frame and the node holding it, as NodeId text."""
|
|
85
|
+
return {frame: node.to_string() for frame, node in self._transform_index.items()}
|
|
86
|
+
|
|
87
|
+
@lifecycle(after=BaseMachineryItem.lifecycle_machinery_item)
|
|
88
|
+
async def lifecycle_ros2_transform_mirror(self) -> AsyncGenerator[None]:
|
|
89
|
+
"""Start the poll once the whole Machine is built, and stop it before teardown.
|
|
90
|
+
|
|
91
|
+
It starts after the Machine's ``_init`` because the Components' objects must exist first,
|
|
92
|
+
and stops before ``_shutdown`` so no tick writes into a Component that is going away.
|
|
93
|
+
"""
|
|
94
|
+
await self._init_transform_mirror()
|
|
95
|
+
yield
|
|
96
|
+
await self._stop_transform_mirror()
|
|
97
|
+
|
|
98
|
+
async def _init_transform_mirror(self) -> None:
|
|
99
|
+
"""Collect every SpatialObject on this Machine and start the poll, if there are any.
|
|
100
|
+
|
|
101
|
+
Nothing is created for frames the robot publishes and nothing is asked of it: the first tick
|
|
102
|
+
is one interval away. Declared outward frames are created here, so a client may write them
|
|
103
|
+
before the robot is on. The cross-object direction check runs first.
|
|
104
|
+
|
|
105
|
+
:raises FrameDirectionError: When two declarations on this Machine claim one frame.
|
|
106
|
+
"""
|
|
107
|
+
objects = self._declared_spatial_objects()
|
|
108
|
+
if not objects:
|
|
109
|
+
return
|
|
110
|
+
listed: Any = getattr(self, "spatial_object_list", None)
|
|
111
|
+
self._transform_world = listed.world_frame.ua_node.nodeid
|
|
112
|
+
self._transform_objects = objects
|
|
113
|
+
self._require_one_direction_per_frame(objects)
|
|
114
|
+
self._transform_outward = frozenset(frame for owner in objects for frame in owner.binding.published_frames)
|
|
115
|
+
for owner in objects:
|
|
116
|
+
head = owner.binding.head
|
|
117
|
+
self._transform_index[head] = owner.spatial.position_frame.ua_node.nodeid
|
|
118
|
+
# The head's Location is the mandatory PositionFrame, which opensmi built.
|
|
119
|
+
owner.frames[head] = _Frame(
|
|
120
|
+
frame=head,
|
|
121
|
+
browse=head,
|
|
122
|
+
location=owner.spatial.position_frame,
|
|
123
|
+
folder=None,
|
|
124
|
+
)
|
|
125
|
+
for owner in objects:
|
|
126
|
+
for declared in owner.binding.publishes:
|
|
127
|
+
await self._open_published(owner, declared)
|
|
128
|
+
# One poll, one period: the Machine's.
|
|
129
|
+
self._transform_interval = self.ros2_transform_settings.interval
|
|
130
|
+
self._transform_quiet_after = self.ros2_transform_settings.quiet_after
|
|
131
|
+
self._transform_task = asyncio.create_task(self._poll_transforms(), name="ros2-tf-poll")
|
|
132
|
+
|
|
133
|
+
def _require_one_direction_per_frame(self, objects: Sequence[_Owner]) -> None:
|
|
134
|
+
"""Refuse a Machine on which two declarations claim one frame.
|
|
135
|
+
|
|
136
|
+
This is the cross-object half of the inward/outward rule, which no single declaration can
|
|
137
|
+
see. It runs before anything is created.
|
|
138
|
+
|
|
139
|
+
:raises FrameDirectionError: When one frame is claimed twice.
|
|
140
|
+
"""
|
|
141
|
+
clash = one_direction_per_frame(
|
|
142
|
+
heads={owner.binding.name: owner.binding.head for owner in objects},
|
|
143
|
+
published={owner.binding.name: owner.binding.published_frames for owner in objects},
|
|
144
|
+
)
|
|
145
|
+
if clash is None:
|
|
146
|
+
return
|
|
147
|
+
if clash.inward:
|
|
148
|
+
against = (
|
|
149
|
+
f"the TF Binding {clash.against!r} declares it as its head frame, which is read "
|
|
150
|
+
f"*inward* from the tree -- so the pose a client wrote would be written over by "
|
|
151
|
+
f"the robot's answer on the next tick, for ever"
|
|
152
|
+
)
|
|
153
|
+
else:
|
|
154
|
+
against = (
|
|
155
|
+
f"the TF Binding {clash.against!r} publishes it too -- and a frame has one parent "
|
|
156
|
+
f"and one publisher, so two declarations asserting it would put two transforms on "
|
|
157
|
+
f"the topic every tick and leave tf2 keeping whichever arrived last"
|
|
158
|
+
)
|
|
159
|
+
message = (
|
|
160
|
+
f"the TF Binding {clash.outward!r} publishes the frame {clash.frame!r} and {against}. "
|
|
161
|
+
f"A frame is inward or outward and never both, which is what makes an echo loop "
|
|
162
|
+
f"impossible rather than something to suppress"
|
|
163
|
+
)
|
|
164
|
+
raise FrameDirectionError(message)
|
|
165
|
+
|
|
166
|
+
async def _stop_transform_mirror(self) -> None:
|
|
167
|
+
"""Stop the poll. Idempotent, and it leaves the watches to the items that opened them."""
|
|
168
|
+
await ended(self._transform_task)
|
|
169
|
+
self._transform_task = None
|
|
170
|
+
self._transform_objects = ()
|
|
171
|
+
|
|
172
|
+
def _declared_spatial_objects(self) -> tuple[_Owner, ...]:
|
|
173
|
+
"""Return this Machine's SpatialObject and every Component's, in browse order."""
|
|
174
|
+
found: list[_Owner] = []
|
|
175
|
+
for item in _machinery_items(self):
|
|
176
|
+
binding = getattr(item, "ros2_tf_binding", None)
|
|
177
|
+
spatial = getattr(item, "ros2_spatial_object", None)
|
|
178
|
+
if binding is None or spatial is None:
|
|
179
|
+
continue
|
|
180
|
+
found.append(_Owner(item=item, binding=binding, spatial=spatial))
|
|
181
|
+
return tuple(found)
|
|
182
|
+
|
|
183
|
+
async def _poll_transforms(self) -> None:
|
|
184
|
+
"""Read the frame network on the interval and render it, until cancelled."""
|
|
185
|
+
while True:
|
|
186
|
+
await asyncio.sleep(self._transform_interval)
|
|
187
|
+
try:
|
|
188
|
+
await self.refresh_transforms()
|
|
189
|
+
except Exception:
|
|
190
|
+
# A runtime condition is never fatal; a tree that failed once is likely fine on the
|
|
191
|
+
# next tick.
|
|
192
|
+
self.logger.exception("Could not render the TF tree; the poll continues")
|
|
193
|
+
|
|
194
|
+
async def refresh_transforms(self) -> None:
|
|
195
|
+
"""Read the frame network once and bring every SpatialObject into step with it.
|
|
196
|
+
|
|
197
|
+
Public so that tests can drive the inward path without waiting for the timer.
|
|
198
|
+
"""
|
|
199
|
+
self._transform_polls += 1
|
|
200
|
+
# A snapshot, because the facet's record is live. Outward frames are dropped from it: our own
|
|
201
|
+
# broadcast comes back through our subscription and cannot be told from the robot's, so an outward
|
|
202
|
+
# pose is only ever read from its Location.
|
|
203
|
+
frames = {
|
|
204
|
+
frame: link
|
|
205
|
+
for frame, link in self._ros2_runtime_for_tf().tf.frames.items()
|
|
206
|
+
if frame not in self._transform_outward
|
|
207
|
+
}
|
|
208
|
+
# Declared outward links join the tree, so the partition, attach points, folders and Bases treat them
|
|
209
|
+
# like published ones.
|
|
210
|
+
parents = dict(parents_of(frames))
|
|
211
|
+
for owner in self._transform_objects:
|
|
212
|
+
for declared in owner.binding.publishes:
|
|
213
|
+
parents[declared.frame] = declared.base
|
|
214
|
+
roots = tree_roots(parents)
|
|
215
|
+
await self._report_separate_trees(roots)
|
|
216
|
+
facet = self._ros2_runtime_for_tf().tf
|
|
217
|
+
# One reading of the clock and of the presence per tick, so no two Locations disagree.
|
|
218
|
+
quiet = _Quiet(
|
|
219
|
+
present=facet.satisfied,
|
|
220
|
+
seen=facet.seen,
|
|
221
|
+
now=facet.now,
|
|
222
|
+
within=self._transform_quiet_after,
|
|
223
|
+
roots=frozenset(roots),
|
|
224
|
+
)
|
|
225
|
+
heads = {owner.binding.head for owner in self._transform_objects}
|
|
226
|
+
held = owners(parents=parents, heads=heads)
|
|
227
|
+
attach = attach_points(parents=parents, heads=heads)
|
|
228
|
+
for owner in self._transform_objects:
|
|
229
|
+
owner.wanted = {
|
|
230
|
+
frame
|
|
231
|
+
for frame, head in held.items()
|
|
232
|
+
if head == owner.binding.head and not excluded(frame, owner.binding.exclude)
|
|
233
|
+
}
|
|
234
|
+
# Always: the head is the mandatory PositionFrame.
|
|
235
|
+
owner.wanted.add(owner.binding.head)
|
|
236
|
+
await self._create_new_locations(owner, parents=parents, attach=attach)
|
|
237
|
+
for owner in self._transform_objects:
|
|
238
|
+
await self._write_locations(owner, frames=frames, parents=parents, quiet=quiet)
|
|
239
|
+
await self._write_alternatives(owner, frames=frames, quiet=quiet)
|
|
240
|
+
await self._rebase_published(owner, parents=parents)
|
|
241
|
+
await self._broadcast_published()
|
|
242
|
+
|
|
243
|
+
async def _write_locations(
|
|
244
|
+
self,
|
|
245
|
+
owner: _Owner,
|
|
246
|
+
*,
|
|
247
|
+
frames: Mapping[str, FrameLink],
|
|
248
|
+
parents: Mapping[str, str],
|
|
249
|
+
quiet: _Quiet,
|
|
250
|
+
) -> None:
|
|
251
|
+
"""Write every Location of one object: its pose, and the Base its TF parent implies.
|
|
252
|
+
|
|
253
|
+
This runs after all new Locations exist, so a Base never points at the World Frame for one
|
|
254
|
+
tick and at a Location the next.
|
|
255
|
+
"""
|
|
256
|
+
for frame, held in sorted(owner.frames.items()):
|
|
257
|
+
link = frames.get(frame)
|
|
258
|
+
await self._write_one(
|
|
259
|
+
held,
|
|
260
|
+
link,
|
|
261
|
+
base=self._base_node(frame, parents=parents),
|
|
262
|
+
status=quiet.status_of(frame, link),
|
|
263
|
+
)
|
|
264
|
+
|
|
265
|
+
async def _write_one(
|
|
266
|
+
self,
|
|
267
|
+
held: _Frame,
|
|
268
|
+
link: FrameLink | None,
|
|
269
|
+
*,
|
|
270
|
+
base: ua.NodeId,
|
|
271
|
+
status: ua.StatusCode,
|
|
272
|
+
) -> None:
|
|
273
|
+
"""Write one Location from the transform its TF parent published, and say how it is.
|
|
274
|
+
|
|
275
|
+
A frame nothing has published yet still gets its Base and status.
|
|
276
|
+
"""
|
|
277
|
+
pose = None if link is None else (link.transform.translation, link.transform.rotation)
|
|
278
|
+
await self._write_pose(held, pose, base=base, status=status)
|
|
279
|
+
if link is not None and link.static and not held.constant:
|
|
280
|
+
# Staticness is a fact about the message, and a head frame's Location exists before any message.
|
|
281
|
+
await self._mark_constant(held.node)
|
|
282
|
+
held.constant = True
|
|
283
|
+
|
|
284
|
+
async def _write_pose(
|
|
285
|
+
self,
|
|
286
|
+
held: _Frame,
|
|
287
|
+
pose: tuple[tuple[float, float, float], tuple[float, float, float, float]] | None,
|
|
288
|
+
*,
|
|
289
|
+
base: ua.NodeId,
|
|
290
|
+
status: ua.StatusCode,
|
|
291
|
+
) -> None:
|
|
292
|
+
"""Write one Location's pose, status and Base, and only what changed.
|
|
293
|
+
|
|
294
|
+
Writing an unchanged value still wakes every MonitoredItem on the node. The status counts as
|
|
295
|
+
a change: a bad status nulls its value (OPC UA Part 4 7.7.1), so a frame that returns at the
|
|
296
|
+
pose it left must still be written. Used for both a frame's Location and an alternative's,
|
|
297
|
+
which differ only in where the pose came from.
|
|
298
|
+
"""
|
|
299
|
+
if pose is not None:
|
|
300
|
+
translation, turned = pose
|
|
301
|
+
position = Position(*translation, unit=METRES)
|
|
302
|
+
orientation = Orientation(*angles(turned), unit=DEGREES)
|
|
303
|
+
else:
|
|
304
|
+
# No pose yet. The status still needs a value to go with (see status.py), so keep what the node
|
|
305
|
+
# reads.
|
|
306
|
+
position = held.position or _ZERO
|
|
307
|
+
orientation = held.orientation or _LEVEL
|
|
308
|
+
if position != held.position or orientation != held.orientation or status != held.status:
|
|
309
|
+
await self._write_reading(held, position, orientation, status)
|
|
310
|
+
if base != held.base:
|
|
311
|
+
await held.location.write_base(base)
|
|
312
|
+
held.base = base
|
|
313
|
+
|
|
314
|
+
async def _write_reading(
|
|
315
|
+
self,
|
|
316
|
+
held: _Frame,
|
|
317
|
+
position: Position,
|
|
318
|
+
orientation: Orientation,
|
|
319
|
+
status: ua.StatusCode,
|
|
320
|
+
) -> None:
|
|
321
|
+
"""Put one pose on a Location's nine value-bearing nodes, with its status.
|
|
322
|
+
|
|
323
|
+
A Good pose goes through opensmi's ``write_pose``. opensmi cannot write a status code, so a
|
|
324
|
+
bad status is written by hand, always with the value, because a status alone leaves the
|
|
325
|
+
variant null (see :mod:`~opensmi.ros2.server.status`).
|
|
326
|
+
"""
|
|
327
|
+
if status == GOOD:
|
|
328
|
+
await held.location.write_pose(Pose(position, orientation))
|
|
329
|
+
else:
|
|
330
|
+
await self._write_faulted(held, position, orientation, status)
|
|
331
|
+
held.position = position
|
|
332
|
+
held.orientation = orientation
|
|
333
|
+
held.status = status
|
|
334
|
+
|
|
335
|
+
async def _write_faulted(
|
|
336
|
+
self,
|
|
337
|
+
held: _Frame,
|
|
338
|
+
position: Position,
|
|
339
|
+
orientation: Orientation,
|
|
340
|
+
status: ua.StatusCode,
|
|
341
|
+
) -> None:
|
|
342
|
+
"""Write one bad reading onto the Location, its pose and its six scalars.
|
|
343
|
+
|
|
344
|
+
The scalars too, because clients subscribe to ``Position/X``. Not the ``Base``, which stays
|
|
345
|
+
true while the frame is unreachable. The Location is written first; opensmi's child sync
|
|
346
|
+
ignores a null value, so the children keep what is written to them next.
|
|
347
|
+
"""
|
|
348
|
+
if held.bearers is None:
|
|
349
|
+
held.bearers = await self._pose_nodes(held.node)
|
|
350
|
+
bearers = held.bearers
|
|
351
|
+
cartesian = ua.ThreeDCartesianCoordinates(
|
|
352
|
+
X=ua.Double(position.x), Y=ua.Double(position.y), Z=ua.Double(position.z)
|
|
353
|
+
)
|
|
354
|
+
turned = ua.ThreeDOrientation(
|
|
355
|
+
A=ua.Double(orientation.a), B=ua.Double(orientation.b), C=ua.Double(orientation.c)
|
|
356
|
+
)
|
|
357
|
+
extension = ua.VariantType.ExtensionObject
|
|
358
|
+
await bearers.frame.write_value(
|
|
359
|
+
status_value(ua.ThreeDFrame(CartesianCoordinates=cartesian, Orientation=turned), extension, status)
|
|
360
|
+
)
|
|
361
|
+
await asyncio.gather(
|
|
362
|
+
bearers.position.write_value(status_value(cartesian, extension, status)),
|
|
363
|
+
bearers.orientation.write_value(status_value(turned, extension, status)),
|
|
364
|
+
*[
|
|
365
|
+
node.write_value(status_value(ua.Double(value), ua.VariantType.Double, status))
|
|
366
|
+
for node, value in (
|
|
367
|
+
(bearers.x, position.x),
|
|
368
|
+
(bearers.y, position.y),
|
|
369
|
+
(bearers.z, position.z),
|
|
370
|
+
(bearers.a, orientation.a),
|
|
371
|
+
(bearers.b, orientation.b),
|
|
372
|
+
(bearers.c, orientation.c),
|
|
373
|
+
)
|
|
374
|
+
],
|
|
375
|
+
)
|
|
376
|
+
|
|
377
|
+
async def _pose_nodes(self, node: Node) -> _Bearers:
|
|
378
|
+
"""Browse the nine value-bearing nodes of one Location, once, on its first bad write.
|
|
379
|
+
|
|
380
|
+
The result is safe to keep because nodes are never moved.
|
|
381
|
+
"""
|
|
382
|
+
ns = self._rsl_namespace
|
|
383
|
+
position = await node.get_child(f"{ns}:Position")
|
|
384
|
+
orientation = await node.get_child(f"{ns}:Orientation")
|
|
385
|
+
# Named, so that a wrong arity is caught by the type checker.
|
|
386
|
+
return _Bearers(
|
|
387
|
+
frame=node,
|
|
388
|
+
position=position,
|
|
389
|
+
x=await position.get_child("X"),
|
|
390
|
+
y=await position.get_child("Y"),
|
|
391
|
+
z=await position.get_child("Z"),
|
|
392
|
+
orientation=orientation,
|
|
393
|
+
a=await orientation.get_child(f"{ns}:A"),
|
|
394
|
+
b=await orientation.get_child(f"{ns}:B"),
|
|
395
|
+
c=await orientation.get_child(f"{ns}:C"),
|
|
396
|
+
)
|
|
397
|
+
|
|
398
|
+
async def _write_alternatives(self, owner: _Owner, *, frames: Mapping[str, FrameLink], quiet: _Quiet) -> None:
|
|
399
|
+
"""Compose and write every alternative this object declared, creating new ones.
|
|
400
|
+
|
|
401
|
+
This is the only place the transform buffer is asked anything. The lookup is synchronous,
|
|
402
|
+
and a chain it cannot complete is skipped. An alternative carries the status of the frame it
|
|
403
|
+
views, so two nodes never disagree about a quiet frame.
|
|
404
|
+
"""
|
|
405
|
+
facet = self._ros2_runtime_for_tf().tf
|
|
406
|
+
for base, frame in owner.binding.alternatives:
|
|
407
|
+
composed = facet.lookup(target=base, source=frame)
|
|
408
|
+
if composed is None:
|
|
409
|
+
continue
|
|
410
|
+
node = self._alternative_base(base, frames)
|
|
411
|
+
if node is None:
|
|
412
|
+
await self._report_absent_base(owner, base, frame)
|
|
413
|
+
continue
|
|
414
|
+
held = owner.alternatives.get((base, frame))
|
|
415
|
+
if held is None:
|
|
416
|
+
held = await self._open_alternative(owner, base, frame, node)
|
|
417
|
+
if held is None:
|
|
418
|
+
continue
|
|
419
|
+
await self._write_pose(
|
|
420
|
+
held,
|
|
421
|
+
(composed.translation, composed.rotation),
|
|
422
|
+
base=node,
|
|
423
|
+
status=quiet.status_of(frame, frames.get(frame)),
|
|
424
|
+
)
|
|
425
|
+
|
|
426
|
+
async def _report_separate_trees(self, roots: tuple[str, ...]) -> None:
|
|
427
|
+
"""Say, as a BindingEvent, that more than one coordinate system is being presented as one.
|
|
428
|
+
|
|
429
|
+
Once per distinct set of roots, and there is no clearing event. Nothing is said while no
|
|
430
|
+
robot is publishing: before that, a Machine that publishes two frames under different bases
|
|
431
|
+
has two roots by construction. Reported on the Machine's own TF Binding.
|
|
432
|
+
"""
|
|
433
|
+
if not self._ros2_runtime_for_tf().tf.satisfied:
|
|
434
|
+
# Re-evaluated, so the condition is reported the first time it is seen against a robot that is on.
|
|
435
|
+
self._transform_roots = ()
|
|
436
|
+
return
|
|
437
|
+
if roots == self._transform_roots:
|
|
438
|
+
return
|
|
439
|
+
self._transform_roots = roots
|
|
440
|
+
binding = self.ros2_tf_binding
|
|
441
|
+
if binding is None or len(roots) < 2:
|
|
442
|
+
return
|
|
443
|
+
listed = ", ".join(repr(root) for root in roots)
|
|
444
|
+
self.logger.warning("The frame network has more than one root", roots=roots)
|
|
445
|
+
await self.emit_binding_event(
|
|
446
|
+
binding=binding.name,
|
|
447
|
+
path=self.ros2_transform_settings.path,
|
|
448
|
+
code=SEPARATE_FRAME_TREES,
|
|
449
|
+
severity=Severity.WARNING,
|
|
450
|
+
text=(
|
|
451
|
+
f"the frame network has {len(roots)} roots ({listed}); a well-formed TF tree has "
|
|
452
|
+
f"one, and every Location whose Base walk leaves the rendered tree lands on the "
|
|
453
|
+
f"same WorldFrame -- so these unrelated coordinate systems are presented as one"
|
|
454
|
+
),
|
|
455
|
+
)
|
|
456
|
+
|
|
457
|
+
|
|
458
|
+
def _machinery_items(machine: Any) -> Iterator[Any]:
|
|
459
|
+
"""Yield a Machine and every Component under it, however deeply nested."""
|
|
460
|
+
yield machine
|
|
461
|
+
for component in getattr(machine, "components", ()):
|
|
462
|
+
yield from _machinery_items(component)
|
|
@@ -0,0 +1,180 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Outward publishing: declared frames a client may write, and their broadcast."""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
import asyncio
|
|
10
|
+
from typing import TYPE_CHECKING, Any
|
|
11
|
+
|
|
12
|
+
from opensmi.server.spatial_object import Orientation, Pose, Position
|
|
13
|
+
|
|
14
|
+
from opensmi.ros2.runtime.errors import PublishFailedError
|
|
15
|
+
from opensmi.ros2.runtime.tf import FrameLink, Transform
|
|
16
|
+
from opensmi.ros2.server.bindings import PublishedFrame
|
|
17
|
+
from opensmi.ros2.server.config import ROS2Config
|
|
18
|
+
from opensmi.ros2.server.events import Severity
|
|
19
|
+
from opensmi.ros2.server.keyed import member_browse_name
|
|
20
|
+
from opensmi.ros2.server.mutation import _write_display_name
|
|
21
|
+
from opensmi.ros2.server.publisher import PUBLISH_FAILED
|
|
22
|
+
from opensmi.ros2.server.spatial import (
|
|
23
|
+
ATTACH_POINTS,
|
|
24
|
+
)
|
|
25
|
+
from opensmi.ros2.server.transforms.conversions import DEGREES, METRES, _as_transform
|
|
26
|
+
from opensmi.ros2.server.transforms.filing import _FrameFiling
|
|
27
|
+
from opensmi.ros2.server.transforms.records import (
|
|
28
|
+
_Observed,
|
|
29
|
+
_Owner,
|
|
30
|
+
_Published,
|
|
31
|
+
)
|
|
32
|
+
|
|
33
|
+
if TYPE_CHECKING:
|
|
34
|
+
from collections.abc import Awaitable, Callable, Mapping, Sequence
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
class _OutwardPublishing(_FrameFiling):
|
|
38
|
+
"""Puts the poses a client writes into declared Locations on the TF topics."""
|
|
39
|
+
|
|
40
|
+
def __init__(self, *args: Any, **kwargs: Any) -> None:
|
|
41
|
+
"""Start out with nothing refused, and pass every argument to opensmi."""
|
|
42
|
+
super().__init__(*args, **kwargs)
|
|
43
|
+
self._broadcast_refused = False
|
|
44
|
+
self._broadcast_lock = asyncio.Lock()
|
|
45
|
+
|
|
46
|
+
async def _open_published(self, owner: _Owner, declared: PublishedFrame) -> None:
|
|
47
|
+
"""Create one declared outward frame's Location, writable, in ``AttachPoints``.
|
|
48
|
+
|
|
49
|
+
It is created at init and outside the chokepoint because it is declared. Access level and
|
|
50
|
+
write check are set here, since a node cannot be re-shaped later. A client needs the
|
|
51
|
+
Machine's Skill level (:attr:`~opensmi.ros2.server.config.ROS2Config.skill_access_level`),
|
|
52
|
+
as for a publishing Skill, and the Lock. ``Base`` stays read-only. It starts at the World
|
|
53
|
+
Frame and is re-resolved every tick, because the declared base frame may not be published
|
|
54
|
+
yet.
|
|
55
|
+
"""
|
|
56
|
+
browse = member_browse_name((declared.frame,))
|
|
57
|
+
world = self._require_world()
|
|
58
|
+
location = _Observed(
|
|
59
|
+
name=browse,
|
|
60
|
+
initial_pose=Pose(
|
|
61
|
+
Position(*declared.position, unit=METRES),
|
|
62
|
+
Orientation(*declared.orientation, unit=DEGREES),
|
|
63
|
+
),
|
|
64
|
+
initial_base=world,
|
|
65
|
+
# Opt-in writable pose; writes reach this module once stored.
|
|
66
|
+
minimum_access_level=(getattr(self, "ros2_config", None) or ROS2Config()).skill_access_level,
|
|
67
|
+
written=self._pose_written(owner, declared.frame),
|
|
68
|
+
)
|
|
69
|
+
await self._file(owner, ATTACH_POINTS, location)
|
|
70
|
+
owner.folders.add(ATTACH_POINTS)
|
|
71
|
+
await _write_display_name(location.ua_node, declared.frame)
|
|
72
|
+
owner.claimed[browse] = declared.frame
|
|
73
|
+
owner.published[declared.frame] = _Published(declared=declared, location=location, base=world)
|
|
74
|
+
self._transform_index[declared.frame] = location.ua_node.nodeid
|
|
75
|
+
|
|
76
|
+
def _pose_written(self, owner: _Owner, frame: str) -> Callable[[Pose], Awaitable[None]]:
|
|
77
|
+
"""Return the callback one outward Location is created with. opensmi awaits it with the stored pose."""
|
|
78
|
+
|
|
79
|
+
async def written(pose: Pose) -> None:
|
|
80
|
+
await self._broadcast_written(owner, frame, pose)
|
|
81
|
+
|
|
82
|
+
return written
|
|
83
|
+
|
|
84
|
+
async def _broadcast_written(self, owner: _Owner, frame: str, pose: Pose) -> None:
|
|
85
|
+
"""Put the pose a client wrote on the wire.
|
|
86
|
+
|
|
87
|
+
The write is never refused for the broadcast: it is already stored, and a failed broadcast
|
|
88
|
+
is a ``PUBLISH_FAILED`` BindingEvent that the next tick retries.
|
|
89
|
+
"""
|
|
90
|
+
held = owner.published.get(frame)
|
|
91
|
+
if held is None:
|
|
92
|
+
return
|
|
93
|
+
await self._broadcast([(held, _as_transform(pose.position, pose.orientation))])
|
|
94
|
+
|
|
95
|
+
async def _rebase_published(self, owner: _Owner, *, parents: Mapping[str, str]) -> None:
|
|
96
|
+
"""Re-resolve every outward Location's ``Base``, the only part of it this module writes."""
|
|
97
|
+
for held in owner.published.values():
|
|
98
|
+
base = self._base_node(held.declared.frame, parents=parents)
|
|
99
|
+
if base == held.base:
|
|
100
|
+
continue
|
|
101
|
+
await held.location.write_base(base)
|
|
102
|
+
held.base = base
|
|
103
|
+
|
|
104
|
+
async def _broadcast_published(self) -> None:
|
|
105
|
+
"""Re-assert what every outward Location holds, in one message per topic.
|
|
106
|
+
|
|
107
|
+
A moving frame goes out every tick, because a pose published once ages out of listeners'
|
|
108
|
+
buffers. A static frame goes out only when it differs from what last went out, which also
|
|
109
|
+
retries a pose the middleware refused. That is why
|
|
110
|
+
:meth:`~opensmi.ros2.runtime.tf.TFFacet.broadcast` owns its publisher instead of using
|
|
111
|
+
``tf2_ros.StaticTransformBroadcaster``. The pose is read back off the Location because it is
|
|
112
|
+
the client's.
|
|
113
|
+
"""
|
|
114
|
+
staged: list[tuple[_Published, Transform]] = []
|
|
115
|
+
for owner in self._transform_objects:
|
|
116
|
+
for held in owner.published.values():
|
|
117
|
+
pose = await held.location.read_pose()
|
|
118
|
+
transform = _as_transform(pose.position, pose.orientation)
|
|
119
|
+
if held.declared.static and transform == held.sent:
|
|
120
|
+
continue
|
|
121
|
+
staged.append((held, transform))
|
|
122
|
+
await self._broadcast(staged)
|
|
123
|
+
|
|
124
|
+
async def _broadcast(self, staged: Sequence[tuple[_Published, Transform]]) -> None:
|
|
125
|
+
"""Put outward poses on the wire, and record what went out only once it has.
|
|
126
|
+
|
|
127
|
+
A refusal is reported as an event and never raised. An outward pose carries no status code,
|
|
128
|
+
because the quiet conditions of :mod:`~opensmi.ros2.server.status` are facts about a robot
|
|
129
|
+
publishing to us. Serialised, because a client's write and the tick may both broadcast.
|
|
130
|
+
"""
|
|
131
|
+
if not staged:
|
|
132
|
+
return
|
|
133
|
+
sending = [
|
|
134
|
+
FrameLink(
|
|
135
|
+
child=held.declared.frame,
|
|
136
|
+
parent=held.declared.base,
|
|
137
|
+
transform=transform,
|
|
138
|
+
static=held.declared.static,
|
|
139
|
+
)
|
|
140
|
+
for held, transform in staged
|
|
141
|
+
]
|
|
142
|
+
async with self._broadcast_lock:
|
|
143
|
+
try:
|
|
144
|
+
await self._ros2_runtime_for_tf().tf.broadcast(sending)
|
|
145
|
+
except PublishFailedError as error:
|
|
146
|
+
await self._report_broadcast_refused(sending, error)
|
|
147
|
+
return
|
|
148
|
+
# Recorded after the wire. Recorded before, a refused static frame would compare equal on every
|
|
149
|
+
# later tick and never be retried.
|
|
150
|
+
for held, transform in staged:
|
|
151
|
+
held.sent = transform
|
|
152
|
+
self._broadcast_refused = False
|
|
153
|
+
|
|
154
|
+
async def _report_broadcast_refused(self, sending: Sequence[FrameLink], error: Exception) -> None:
|
|
155
|
+
"""Say, as a BindingEvent, that the poses a client wrote did not reach ROS2.
|
|
156
|
+
|
|
157
|
+
Once per spell of failure; the streak clears on the first broadcast that goes out. It is
|
|
158
|
+
reported on the Machine's own TF Binding, because one broadcast carries every object's
|
|
159
|
+
frames. It uses :data:`~opensmi.ros2.server.publisher.PUBLISH_FAILED`, the code the
|
|
160
|
+
publishing Skill reports for the same fact.
|
|
161
|
+
"""
|
|
162
|
+
binding = self.ros2_tf_binding
|
|
163
|
+
if binding is None: # pragma: no cover -- nothing broadcasts without a declaration
|
|
164
|
+
return
|
|
165
|
+
if self._broadcast_refused:
|
|
166
|
+
return
|
|
167
|
+
# After both guards, so the latch is only set by a failure that was reported.
|
|
168
|
+
self._broadcast_refused = True
|
|
169
|
+
frames = sorted(link.child for link in sending)
|
|
170
|
+
self.logger.error("A transform broadcast did not happen", binding=binding.name, frames=frames)
|
|
171
|
+
await self.emit_binding_event(
|
|
172
|
+
binding=binding.name,
|
|
173
|
+
path=self.ros2_transform_settings.path,
|
|
174
|
+
code=PUBLISH_FAILED,
|
|
175
|
+
severity=Severity.ERROR,
|
|
176
|
+
text=(
|
|
177
|
+
f"the poses written into {', '.join(frames)} did not reach ROS2: {error}; the Locations hold "
|
|
178
|
+
f"what a client wrote and nothing on the robot has been told about it"
|
|
179
|
+
),
|
|
180
|
+
)
|