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.
Files changed (89) hide show
  1. opensmi/ros2/__init__.py +89 -0
  2. opensmi/ros2/__main__.py +11 -0
  3. opensmi/ros2/cli.py +143 -0
  4. opensmi/ros2/py.typed +0 -0
  5. opensmi/ros2/runtime/__init__.py +67 -0
  6. opensmi/ros2/runtime/_decode.py +101 -0
  7. opensmi/ros2/runtime/_encode.py +102 -0
  8. opensmi/ros2/runtime/_parser.py +147 -0
  9. opensmi/ros2/runtime/errors.py +141 -0
  10. opensmi/ros2/runtime/goal.py +232 -0
  11. opensmi/ros2/runtime/graph.py +288 -0
  12. opensmi/ros2/runtime/guard.py +213 -0
  13. opensmi/ros2/runtime/interface.py +140 -0
  14. opensmi/ros2/runtime/messages/__init__.py +503 -0
  15. opensmi/ros2/runtime/messages/industrial_msgs.py +62 -0
  16. opensmi/ros2/runtime/messages/industrial_msgs.pyi +159 -0
  17. opensmi/ros2/runtime/messages/motoros2_interfaces.py +79 -0
  18. opensmi/ros2/runtime/messages/motoros2_interfaces.pyi +307 -0
  19. opensmi/ros2/runtime/messages/neo_msgs2.py +55 -0
  20. opensmi/ros2/runtime/messages/neo_msgs2.pyi +138 -0
  21. opensmi/ros2/runtime/messages/neo_srvs2.py +72 -0
  22. opensmi/ros2/runtime/messages/neo_srvs2.pyi +224 -0
  23. opensmi/ros2/runtime/outcomes.py +103 -0
  24. opensmi/ros2/runtime/parameters.py +723 -0
  25. opensmi/ros2/runtime/publisher.py +125 -0
  26. opensmi/ros2/runtime/qos.py +394 -0
  27. opensmi/ros2/runtime/runtime.py +1171 -0
  28. opensmi/ros2/runtime/selection.py +150 -0
  29. opensmi/ros2/runtime/stamp.py +100 -0
  30. opensmi/ros2/runtime/subscription.py +354 -0
  31. opensmi/ros2/runtime/tf.py +714 -0
  32. opensmi/ros2/server/__init__.py +148 -0
  33. opensmi/ros2/server/access.py +109 -0
  34. opensmi/ros2/server/bindings/__init__.py +92 -0
  35. opensmi/ros2/server/bindings/_base.py +154 -0
  36. opensmi/ros2/server/bindings/action.py +133 -0
  37. opensmi/ros2/server/bindings/keyed.py +127 -0
  38. opensmi/ros2/server/bindings/lifecycle.py +131 -0
  39. opensmi/ros2/server/bindings/method.py +130 -0
  40. opensmi/ros2/server/bindings/monitoring.py +109 -0
  41. opensmi/ros2/server/bindings/parameter.py +133 -0
  42. opensmi/ros2/server/bindings/publisher.py +192 -0
  43. opensmi/ros2/server/bindings/registry.py +95 -0
  44. opensmi/ros2/server/bindings/tf.py +256 -0
  45. opensmi/ros2/server/browse_names.py +94 -0
  46. opensmi/ros2/server/component.py +334 -0
  47. opensmi/ros2/server/config.py +493 -0
  48. opensmi/ros2/server/containers.py +191 -0
  49. opensmi/ros2/server/drivers.py +421 -0
  50. opensmi/ros2/server/errors.py +159 -0
  51. opensmi/ros2/server/events.py +406 -0
  52. opensmi/ros2/server/introspection.py +850 -0
  53. opensmi/ros2/server/keyed.py +749 -0
  54. opensmi/ros2/server/lifecycle.py +238 -0
  55. opensmi/ros2/server/limits.py +66 -0
  56. opensmi/ros2/server/loader.py +977 -0
  57. opensmi/ros2/server/machine.py +225 -0
  58. opensmi/ros2/server/method.py +307 -0
  59. opensmi/ros2/server/mirror.py +582 -0
  60. opensmi/ros2/server/mutation.py +992 -0
  61. opensmi/ros2/server/nodesets/Opc.Ua.SmartFactory.ROS2.NodeSet2.xml +85 -0
  62. opensmi/ros2/server/nodesets/__init__.py +18 -0
  63. opensmi/ros2/server/nodesets/smart_factory_ros2.py +26 -0
  64. opensmi/ros2/server/parameters.py +704 -0
  65. opensmi/ros2/server/projection.py +1176 -0
  66. opensmi/ros2/server/projections/__init__.py +41 -0
  67. opensmi/ros2/server/projections/scope.py +222 -0
  68. opensmi/ros2/server/provenance.py +121 -0
  69. opensmi/ros2/server/publisher.py +781 -0
  70. opensmi/ros2/server/recipes.py +514 -0
  71. opensmi/ros2/server/required.py +633 -0
  72. opensmi/ros2/server/skill.py +363 -0
  73. opensmi/ros2/server/spatial.py +283 -0
  74. opensmi/ros2/server/startup.py +626 -0
  75. opensmi/ros2/server/status.py +107 -0
  76. opensmi/ros2/server/structures.py +135 -0
  77. opensmi/ros2/server/transforms/__init__.py +39 -0
  78. opensmi/ros2/server/transforms/conversions.py +58 -0
  79. opensmi/ros2/server/transforms/filing.py +340 -0
  80. opensmi/ros2/server/transforms/mirror.py +462 -0
  81. opensmi/ros2/server/transforms/outward.py +180 -0
  82. opensmi/ros2/server/transforms/records.py +195 -0
  83. opensmi/ros2/server/transforms/spatial.py +154 -0
  84. opensmi_ros2-0.1.0.dist-info/METADATA +183 -0
  85. opensmi_ros2-0.1.0.dist-info/RECORD +89 -0
  86. opensmi_ros2-0.1.0.dist-info/WHEEL +5 -0
  87. opensmi_ros2-0.1.0.dist-info/entry_points.txt +2 -0
  88. opensmi_ros2-0.1.0.dist-info/licenses/LICENSES/MIT.txt +18 -0
  89. opensmi_ros2-0.1.0.dist-info/top_level.txt +1 -0
@@ -0,0 +1,363 @@
1
+ # SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
2
+ #
3
+ # SPDX-License-Identifier: MIT
4
+
5
+ """An action is a finite Skill: a client starts it, watches it, and reads what it produced.
6
+
7
+ ====================== ========================================================================
8
+ ``Start`` Read the Parameters, build the goal, send it. ``Running`` is reached only
9
+ once the server has accepted the goal.
10
+ ``Running`` Feedback is written to Monitoring as it arrives; the goal's ending is
11
+ awaited.
12
+ ``Completed`` The goal succeeded. The result fields are in FinalResultData.
13
+ ``Halt`` Cancel the goal. An aborted or externally cancelled goal also ends here.
14
+ ``Reset`` Clear Monitoring and FinalResultData. The Parameters are left alone, so a
15
+ goal a client wrote before ``Reset`` is not discarded silently.
16
+ ``Suspend`` Not available: ROS2 actions cannot be paused, so the Skill is never
17
+ suspendable and opensmi removes the method.
18
+ ====================== ========================================================================
19
+
20
+ The outcome drives the state machine: ``Completed`` is success and ``Halted`` is every other
21
+ ending. FinalResultData holds only the fields of the result message. Three outcome booleans
22
+ (``succeeded``, ``aborted``, ``canceled``) can be added as a debugging aid with
23
+ :attr:`~opensmi.ros2.server.config.ROS2Config.debug_goal_outcome_booleans`.
24
+
25
+ opensmi runs the state machine and the ``_handle_*`` methods below are its hooks. ``Start``,
26
+ ``Halt`` and ``Reset`` need the Skill access level (``skill_access_level``, default
27
+ :data:`~opensmi.ros2.server.access.SKILL_ACCESS_LEVEL`) and the Machine's Lock; ``Start`` also needs
28
+ a completed startup. An ``OpenSmiRuntimeError`` from a handler halts the Skill and appears as a
29
+ runtime error in the Notification of the Component, with its code as ``ErrorCode``. Any other
30
+ exception and a cancelled handler halt it too.
31
+
32
+ :func:`default_goal_from` and :func:`default_result_to` are pure functions of a message and its
33
+ Projection context. A declaration may name its own pair; a replacement that needs the instance
34
+ overrides :meth:`ROS2ActionSkill.build_goal` or :meth:`ROS2ActionSkill.result_values` on a custom
35
+ class. The containers themselves are built in :mod:`opensmi.ros2.server.containers`.
36
+ """
37
+
38
+ from __future__ import annotations
39
+
40
+ import asyncio
41
+ from typing import TYPE_CHECKING, ClassVar, Final
42
+
43
+ from asyncua import ua
44
+ from opensmi.core.errors import OpenSmiRuntimeError
45
+ from opensmi.server import BaseMachineryItem, BaseSkill, FiniteSkillLogicMixin
46
+ from opensmi.server.mixins import FinalResultDataMixin, MonitoringMixin, ParameterSetMixin, ParentMixin
47
+
48
+ from opensmi.ros2.runtime.guard import require_sendable
49
+ from opensmi.ros2.server.access import SKILL_ACCESS_LEVEL
50
+ from opensmi.ros2.server.containers import ROS2ContainerMixin, VariableContainer
51
+ from opensmi.ros2.server.mutation import add_plan_variable, require_unused_name
52
+ from opensmi.ros2.server.projection import PlanVariable, narrowed, require_invertible, shaped
53
+
54
+ if TYPE_CHECKING:
55
+ from collections.abc import Mapping
56
+
57
+ from opensmi.ros2.runtime.goal import Goal
58
+ from opensmi.ros2.runtime.interface import RuntimeInterface
59
+ from opensmi.ros2.runtime.messages import ActionTypes, Message
60
+ from opensmi.ros2.runtime.outcomes import GoalOutcome
61
+ from opensmi.ros2.server.bindings import ActionBinding
62
+ from opensmi.ros2.server.config import ROS2Config
63
+ from opensmi.ros2.server.projection import ProjectionContext
64
+
65
+ __all__ = [
66
+ "GOAL_UNBUILDABLE",
67
+ "OUTCOME_BOOLEANS",
68
+ "ROS2ActionSkill",
69
+ "default_goal_from",
70
+ "default_result_to",
71
+ ]
72
+
73
+
74
+ OUTCOME_BOOLEANS: Final = ("succeeded", "aborted", "canceled")
75
+ """The three result booleans the opt-in debugging flag adds to FinalResultData.
76
+
77
+ They are derived from one outcome, so they cannot disagree with each other.
78
+ """
79
+
80
+
81
+ GOAL_UNBUILDABLE: Final = "GoalUnbuildable"
82
+ """The error code of a ``Start`` whose Parameters could not be built into a goal.
83
+
84
+ The Skill halts and the goal never reaches ROS2. The error arrives in the Component's
85
+ Notification with this code and a message naming the field and the reason.
86
+ """
87
+
88
+
89
+ def default_goal_from(
90
+ goal_type: type[Message],
91
+ values: Mapping[str, object],
92
+ context: ProjectionContext,
93
+ ) -> Message:
94
+ """Build a goal message out of what a client wrote into a Skill's Parameters.
95
+
96
+ The Projection covering the goal type, run backwards. A pure function: no server, ``rclpy``
97
+ or event loop.
98
+
99
+ :raises NotInvertibleError: When a field of the goal has no value, because the selection
100
+ removed it or no Parameter carries it. Such a field is refused instead of filled with a
101
+ default.
102
+ """
103
+ return context.registry.find(goal_type.ros2.type_name).inverse_of(goal_type, values, context)
104
+
105
+
106
+ def default_result_to(result: Message, context: ProjectionContext) -> Mapping[str, object]:
107
+ """Read a result message into the FinalResultData variables its type was shaped into."""
108
+ return context.registry.find(type(result).ros2.type_name).decode(result, context)
109
+
110
+
111
+ class ROS2ActionSkill(
112
+ ROS2ContainerMixin,
113
+ ParentMixin[BaseMachineryItem],
114
+ MonitoringMixin,
115
+ FinalResultDataMixin,
116
+ ParameterSetMixin,
117
+ FiniteSkillLogicMixin,
118
+ BaseSkill,
119
+ ):
120
+ """The finite Skill one :class:`~opensmi.ros2.server.bindings.ActionBinding` becomes.
121
+
122
+ Constructed by the declaring Component. A declaration may name a subclass of this class to add
123
+ behaviour; the subclass is checked when the declaration is read.
124
+ """
125
+
126
+ callable_kind: ClassVar[str] = "Skill"
127
+
128
+ def __init__(
129
+ self,
130
+ *,
131
+ binding: ActionBinding,
132
+ runtime: RuntimeInterface,
133
+ context: ProjectionContext,
134
+ config: ROS2Config,
135
+ minimum_access_level: int = SKILL_ACCESS_LEVEL,
136
+ ) -> None:
137
+ """Resolve everything about the declaration that can be wrong, before any node exists.
138
+
139
+ A missing interface package, a pattern that matches nothing or a goal that cannot be built
140
+ backwards fails here instead of on the first ``Start``.
141
+
142
+ :param binding: What to build.
143
+ :param runtime: Where the goal is sent. The Machine's, reached through the Component.
144
+ :param context: The Projection context of the Component, with nothing selected yet. The
145
+ Skill narrows it once per surface (goal, feedback, result).
146
+ :param config: The deployment's ROS2 settings.
147
+ :param minimum_access_level: The level ``Start``, ``Halt`` and ``Reset`` need.
148
+ :raises MessageTypeError: When the action type cannot be resolved.
149
+ :raises FieldSelectionError: When a selection pattern matches nothing.
150
+ :raises NotInvertibleError: When the goal type reaches a Projection that cannot run
151
+ backwards.
152
+ """
153
+ super().__init__(
154
+ name=binding.name,
155
+ minimum_access_level=minimum_access_level,
156
+ # ROS2 has no pause; opensmi deletes the Suspend method from a Skill that says so.
157
+ suspendable=False,
158
+ )
159
+ self._binding = binding
160
+ self._runtime = runtime
161
+ self._config = config
162
+ self._types: ActionTypes = runtime.describe_action(binding.action_type)
163
+ self._goal_context = narrowed(context, self._types.goal, binding.selection)
164
+ self._feedback_context = narrowed(context, self._types.feedback, binding.feedback_selection)
165
+ self._result_context = narrowed(context, self._types.result, binding.result_selection)
166
+ require_invertible(self._types.goal, self._goal_context)
167
+ self._goal_plan = shaped(self._types.goal, self._goal_context)
168
+ self._feedback_plan = shaped(self._types.feedback, self._feedback_context)
169
+ self._result_plan = shaped(self._types.result, self._result_context)
170
+ # opensmi's containers exist from first use; `_init` fills these records.
171
+ self._parameters = VariableContainer(name="ParameterSet", container=self.parameter_set)
172
+ self._monitoring = VariableContainer(name="Monitoring", container=self.monitoring)
173
+ self._results = VariableContainer(name="FinalResultData", container=self.final_result_data)
174
+ self._goal: Goal | None = None
175
+
176
+ @property
177
+ def binding(self) -> ActionBinding:
178
+ """Return the declaration this Skill was built from."""
179
+ return self._binding
180
+
181
+ @property
182
+ def action_types(self) -> ActionTypes:
183
+ """Return the goal, feedback and result types of the action this Skill is bound to."""
184
+ return self._types
185
+
186
+ @property
187
+ def ros2_goal(self) -> Goal | None:
188
+ """Return the goal currently in flight, or ``None`` when none is.
189
+
190
+ Held on the Skill because opensmi cancels the running handler's task before the halting
191
+ handler runs, so a handle kept only there would already be gone.
192
+ """
193
+ return self._goal
194
+
195
+ async def _init(self) -> None:
196
+ """Let opensmi build the Skill, create the variables, and prove the defaults can be sent.
197
+
198
+ Building a goal from the initial Parameters must work, so a value ROS2 would abort on
199
+ fails here, on the machine of whoever wrote the declaration.
200
+ """
201
+ await super()._init()
202
+ await self._create(self._goal_plan, self._parameters, message_type=self._types.goal, writable=True)
203
+ await self._create(self._feedback_plan, self._monitoring, message_type=self._types.feedback)
204
+ await self._create(self._result_plan, self._results, message_type=self._types.result)
205
+ await self._create_outcome_booleans()
206
+ require_sendable(await self.build_goal(await self.read_parameters()))
207
+
208
+ async def read_parameters(self) -> Mapping[str, object]:
209
+ """Return what the ParameterSet holds now, by variable name."""
210
+ return await self._read_values(self._parameters)
211
+
212
+ async def read_monitoring(self) -> Mapping[str, object]:
213
+ """Return what the Monitoring holds now, by variable name."""
214
+ return await self._read_values(self._monitoring)
215
+
216
+ async def read_results(self) -> Mapping[str, object]:
217
+ """Return what the FinalResultData holds now, by variable name."""
218
+ return await self._read_values(self._results)
219
+
220
+ async def build_goal(self, values: Mapping[str, object]) -> Message:
221
+ """Return the goal message to send, given what a client wrote into the Parameters.
222
+
223
+ Override on a custom class when building the goal needs the instance. The declaration's
224
+ ``goal_from`` is the pure-function form, and :func:`default_goal_from` is the default.
225
+ An override must also work with the declaration's defaults, because ``_init`` calls it
226
+ once with them.
227
+ """
228
+ build = self._binding.goal_from or default_goal_from
229
+ return build(self._types.goal, values, self._goal_context)
230
+
231
+ async def result_values(self, result: Message) -> Mapping[str, object]:
232
+ """Return the FinalResultData values one result message carries, by variable name.
233
+
234
+ Overridable like :meth:`build_goal`; the declaration's ``result_to`` is the pure-function
235
+ form and :func:`default_result_to` the default.
236
+ """
237
+ read = self._binding.result_to or default_result_to
238
+ return read(result, self._result_context)
239
+
240
+ async def _handle_starting(self) -> None:
241
+ """Read the Parameters, build the goal and send it; reach Running only if it is taken.
242
+
243
+ A goal the server refuses halts the Skill. Parameters that cannot be built into a goal halt
244
+ it with :data:`GOAL_UNBUILDABLE`; every refusal of a client's value is a ``ValueError``,
245
+ which opensmi would otherwise report without an error code.
246
+ """
247
+ try:
248
+ goal = await self.build_goal(await self.read_parameters())
249
+ except ValueError as refused:
250
+ raise OpenSmiRuntimeError(str(refused), error_code=GOAL_UNBUILDABLE) from refused
251
+ self._goal = await self._runtime.send_goal(
252
+ action=self._binding.path,
253
+ action_type=self._types,
254
+ goal=goal,
255
+ depth=self._binding.feedback_depth,
256
+ server_timeout=self._binding.server_timeout,
257
+ )
258
+ if self._goal.accepted:
259
+ return
260
+ outcome = self._goal.outcome
261
+ await self._let_go()
262
+ raise OpenSmiRuntimeError(_fault_text(outcome), error_code="GoalRejected")
263
+
264
+ async def _handle_running(self) -> None:
265
+ """Follow the feedback and wait for the ending, then let the ending decide the state.
266
+
267
+ A cancel this Skill asked for never arrives here, because opensmi cancels this handler
268
+ before running the halting one. Any non-success outcome that does arrive comes from
269
+ outside and halts the Skill.
270
+ """
271
+ goal = self._goal
272
+ if goal is None: # pragma: no cover -- starting either sets one or halts
273
+ gone = "the goal was gone before it was awaited"
274
+ raise OpenSmiRuntimeError(gone, error_code="GoalMissing")
275
+ following = asyncio.create_task(self._follow_feedback(goal), name=f"ros2-feedback-{self.name}")
276
+ try:
277
+ outcome = await goal.result()
278
+ # Awaited, not cancelled: the stream ends by itself after draining the buffer, so the
279
+ # last progress report is not lost.
280
+ await following
281
+ finally:
282
+ following.cancel()
283
+ await asyncio.gather(following, return_exceptions=True)
284
+ # ROS2 sends a (often partial) result for aborted and cancelled goals too.
285
+ await self._write_result(outcome)
286
+ if not outcome.succeeded:
287
+ raise OpenSmiRuntimeError(_fault_text(outcome), error_code=outcome.status.name)
288
+ await self._let_go()
289
+
290
+ async def _handle_halting(self) -> None:
291
+ """Ask the server to cancel whatever is in flight, then let the handle go."""
292
+ goal = self._goal
293
+ if goal is None:
294
+ return
295
+ try:
296
+ await goal.cancel()
297
+ finally:
298
+ await self._let_go()
299
+
300
+ async def _handle_resetting(self) -> None:
301
+ """Clear what the last run produced and keep the Parameters a client wrote.
302
+
303
+ Monitoring and FinalResultData go back to their declared initial values.
304
+ """
305
+ await self._let_go()
306
+ await self._reset_values(self._monitoring)
307
+ await self._reset_values(self._results)
308
+ await self._write_outcome_booleans(None)
309
+
310
+ async def _follow_feedback(self, goal: Goal) -> None:
311
+ """Write every feedback message this goal produces, until it ends."""
312
+ async for feedback in goal:
313
+ try:
314
+ await self._write_feedback(feedback)
315
+ except Exception:
316
+ # A runtime condition is never fatal: the next message is likely fine.
317
+ self.logger.exception(
318
+ "Could not write action feedback to its Monitoring variables",
319
+ binding=self._binding.name,
320
+ action=self._binding.path,
321
+ )
322
+
323
+ async def _write_feedback(self, feedback: Message) -> None:
324
+ """Write one feedback message into the Monitoring variables its type was shaped into."""
325
+ projection = self._feedback_context.registry.find(type(feedback).ros2.type_name)
326
+ await self._write_values(self._monitoring, projection.decode(feedback, self._feedback_context))
327
+
328
+ async def _write_result(self, outcome: GoalOutcome) -> None:
329
+ """Write what the goal produced, and the debugging booleans if they were asked for."""
330
+ if outcome.result is not None:
331
+ await self._write_values(self._results, await self.result_values(outcome.result))
332
+ await self._write_outcome_booleans(outcome)
333
+
334
+ async def _create_outcome_booleans(self) -> None:
335
+ """Create the three booleans if the debugging aid is on."""
336
+ if not self._config.debug_goal_outcome_booleans:
337
+ return
338
+ for label in OUTCOME_BOOLEANS:
339
+ require_unused_name(label, (self.final_result_data,), self._named(self._results))
340
+ entry = PlanVariable(name=label, source="", initial=False, variant_type=ua.VariantType.Boolean)
341
+ await add_plan_variable(self.final_result_data, label, entry, None)
342
+
343
+ async def _write_outcome_booleans(self, outcome: GoalOutcome | None) -> None:
344
+ """Write the three booleans from one outcome; ``None`` means a reset."""
345
+ if not self._config.debug_goal_outcome_booleans:
346
+ return
347
+ answers = (False, False, False) if outcome is None else (outcome.succeeded, outcome.aborted, outcome.canceled)
348
+ for label, value in zip(OUTCOME_BOOLEANS, answers, strict=True):
349
+ if label in self.final_result_data: # pragma: no branch -- created together with the flag
350
+ await self._write(self.final_result_data[label], value, ua.VariantType.Boolean)
351
+
352
+ async def _let_go(self) -> None:
353
+ """Release the goal handle, if there is one. It never commands anything."""
354
+ goal, self._goal = self._goal, None
355
+ if goal is not None:
356
+ await goal.close()
357
+
358
+
359
+ def _fault_text(outcome: GoalOutcome | None) -> str:
360
+ """Return what a halting Skill reports about a goal, out of that goal's own faults."""
361
+ if outcome is None: # pragma: no cover -- an unaccepted goal always carries one
362
+ return "the goal ended without an outcome"
363
+ return "; ".join(outcome.faults) or "the goal did not succeed"
@@ -0,0 +1,283 @@
1
+ # SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
2
+ #
3
+ # SPDX-License-Identifier: MIT
4
+
5
+ """Which frame belongs to which object, which folder it lands in, and what its Base points at.
6
+
7
+ The partition rules as pure functions of a frame tree (child frame to parent frame) and
8
+ a set of declared head frames. There is no server, no ``asyncua`` node and no ROS2 here, so the
9
+ rules can be tested without any of them.
10
+
11
+ Every other frame belongs to the nearest ancestor-or-self that is a declared head. A frame that is
12
+ the TF parent of another object's head is an attach point; every other frame is an internal frame. A
13
+ Location's ``Base`` is the node holding its TF parent, and a frame removed from the middle of a
14
+ chain re-bases its children onto the nearest surviving ancestor. For the outward direction,
15
+ :func:`rotation` inverts :func:`angles`, :func:`one_direction_per_frame` checks the inward/outward
16
+ rule across a Machine, and :func:`tree_roots` finds the roots of the rendered tree.
17
+ """
18
+
19
+ from __future__ import annotations
20
+
21
+ import math
22
+ from dataclasses import dataclass
23
+ from fnmatch import fnmatchcase
24
+ from typing import TYPE_CHECKING, Final, TypeVar
25
+
26
+ if TYPE_CHECKING:
27
+ from collections.abc import Collection, Iterable, Mapping
28
+
29
+ from opensmi.ros2.runtime.tf import FrameLink
30
+
31
+ __all__ = [
32
+ "ALTERNATIVE_FRAMES",
33
+ "ATTACH_POINTS",
34
+ "INTERNAL_FRAMES",
35
+ "FrameClash",
36
+ "angles",
37
+ "attach_points",
38
+ "base_frame_of",
39
+ "base_node_of",
40
+ "excluded",
41
+ "folder_of",
42
+ "nearest_ancestor",
43
+ "one_direction_per_frame",
44
+ "owner_of",
45
+ "owners",
46
+ "parents_of",
47
+ "rotation",
48
+ "tree_roots",
49
+ ]
50
+
51
+ _Node = TypeVar("_Node")
52
+ """Whatever a node is to the caller. This module never looks at one."""
53
+
54
+ ATTACH_POINTS: Final = "AttachPoints"
55
+ """The folder for a position another object can be attached at. Inferred, never declared."""
56
+
57
+ INTERNAL_FRAMES: Final = "InternalFrames"
58
+ """The folder for every other frame of an object.
59
+
60
+ Where nearly every TF frame lands. 6.1.1 says an internal frame may not be used as an attach point,
61
+ so filing a wheel link under :data:`ATTACH_POINTS` would assert something false.
62
+ """
63
+
64
+ ALTERNATIVE_FRAMES: Final = "AlternativeFrames"
65
+ """The folder for a pose composed against some base other than the frame's own TF parent."""
66
+
67
+ _DEGREES: Final = 180.0 / math.pi
68
+ """Radians to degrees. The RSL ``Orientation`` is in degrees."""
69
+
70
+
71
+ def parents_of(frames: Mapping[str, FrameLink]) -> Mapping[str, str]:
72
+ """Return the frame tree as the partition reads it: each child frame mapped to its parent.
73
+
74
+ This is the one place the module reads a :class:`~opensmi.ros2.runtime.tf.FrameLink`. Everything
75
+ else takes a plain mapping of names, which tests can write by hand.
76
+ """
77
+ return {child: link.parent for child, link in frames.items()}
78
+
79
+
80
+ def nearest_ancestor(
81
+ frame: str,
82
+ *,
83
+ parents: Mapping[str, str],
84
+ admitted: Collection[str],
85
+ include_self: bool = True,
86
+ ) -> str | None:
87
+ """Return the closest frame at or above ``frame`` that is admitted, or ``None``.
88
+
89
+ Both the partition and the re-basing walk up until something qualifies. A cycle (which tf2
90
+ forbids and a misbehaving publisher can still produce) ends the walk and answers ``None``, so a
91
+ poll never hangs.
92
+
93
+ :param frame: Where to start.
94
+ :param parents: Child frame to parent frame.
95
+ :param admitted: The frames that qualify.
96
+ :param include_self: Whether ``frame`` itself may be the answer.
97
+ """
98
+ seen: set[str] = set()
99
+ current: str | None = frame if include_self else parents.get(frame)
100
+ while current is not None and current not in seen:
101
+ if current in admitted:
102
+ return current
103
+ seen.add(current)
104
+ current = parents.get(current)
105
+ return None
106
+
107
+
108
+ def owner_of(frame: str, *, parents: Mapping[str, str], heads: Collection[str]) -> str | None:
109
+ """Return the head frame of the object one frame belongs to, or ``None`` if no object does.
110
+
111
+ A head frame owns itself, which makes it the object's ``PositionFrame``. Frames above every
112
+ declared head (a ``map``, say) belong to no object and are rendered by nobody.
113
+ """
114
+ return nearest_ancestor(frame, parents=parents, admitted=heads, include_self=True)
115
+
116
+
117
+ def owners(*, parents: Mapping[str, str], heads: Collection[str]) -> Mapping[str, str]:
118
+ """Return every frame that some declared object owns, mapped to that object's head frame.
119
+
120
+ TF has one parent per frame, so no frame can be under two heads.
121
+ """
122
+ found = {frame: owner_of(frame, parents=parents, heads=heads) for frame in parents}
123
+ return {frame: head for frame, head in found.items() if head is not None}
124
+
125
+
126
+ def attach_points(*, parents: Mapping[str, str], heads: Collection[str]) -> frozenset[str]:
127
+ """Return the frames that are attach points because another object's head hangs off them.
128
+
129
+ A head whose parent is itself a head attaches to that object's ``PositionFrame``, so nothing is
130
+ filed for it.
131
+ """
132
+ return frozenset(
133
+ parent
134
+ for head in heads
135
+ if (parent := parents.get(head)) is not None and parent not in heads and parent in parents
136
+ )
137
+
138
+
139
+ def folder_of(frame: str, *, head: str, attach: Collection[str]) -> str | None:
140
+ """Return which folder of its object's SpatialObject one frame lands in.
141
+
142
+ ``None`` for the object's own head frame, which is the ``PositionFrame`` and in no folder.
143
+ """
144
+ if frame == head:
145
+ return None
146
+ return ATTACH_POINTS if frame in attach else INTERNAL_FRAMES
147
+
148
+
149
+ def base_frame_of(frame: str, *, parents: Mapping[str, str], kept: Collection[str]) -> str | None:
150
+ """Return the frame whose node one Location's ``Base`` points at, or ``None`` for the world.
151
+
152
+ The search starts strictly above ``frame``. ``None`` means the walk left the tree without
153
+ meeting a frame that has a node, which is the root of the TF tree, i.e. the list's World Frame.
154
+ A frame excluded from the middle of a chain is not in ``kept``, so its children walk past it.
155
+ """
156
+ return nearest_ancestor(frame, parents=parents, admitted=kept, include_self=False)
157
+
158
+
159
+ def base_node_of(
160
+ frame: str,
161
+ *,
162
+ parents: Mapping[str, str],
163
+ index: Mapping[str, _Node],
164
+ world: _Node,
165
+ ) -> _Node:
166
+ """Return the node one Location's ``Base`` points at, resolved through the frame-to-node index.
167
+
168
+ Walks up until a frame has a node and answers that node, or the World Frame when the walk leaves
169
+ the index. Generic in what a node is: the caller's index holds NodeIds.
170
+
171
+ :param frame: The frame whose Base is wanted.
172
+ :param parents: Child frame to parent frame.
173
+ :param index: Every frame that has a node, and the node it has.
174
+ :param world: What to answer when the walk leaves the index.
175
+ """
176
+ found = base_frame_of(frame, parents=parents, kept=index)
177
+ if found is None:
178
+ return world
179
+ return index.get(found, world)
180
+
181
+
182
+ @dataclass(frozen=True, slots=True, kw_only=True)
183
+ class FrameClash:
184
+ """One frame that two declarations on a Machine both claim, and how each claims it.
185
+
186
+ :ivar frame: The frame in dispute.
187
+ :ivar outward: The name of the Binding that publishes it.
188
+ :ivar against: The name of the Binding that claims it too.
189
+ :ivar inward: Whether ``against`` claims it as its head frame (read from the tree) instead of
190
+ publishing it as well.
191
+ """
192
+
193
+ frame: str
194
+ outward: str
195
+ against: str
196
+ inward: bool
197
+
198
+
199
+ def tree_roots(parents: Mapping[str, str]) -> tuple[str, ...]:
200
+ """Return every frame that is somebody's parent and nobody's child, in sorted order.
201
+
202
+ Like :func:`~opensmi.ros2.runtime.tf.roots_of` but over the rendered tree, not the facet's raw
203
+ record: what a Machine broadcasts comes back through its own subscription, so the raw record
204
+ would report its own outward declarations as a second coordinate system. A well-formed tree has
205
+ one root, which OPC 10000-210 calls the World Frame.
206
+ """
207
+ return tuple(sorted(set(parents.values()) - set(parents)))
208
+
209
+
210
+ def one_direction_per_frame(
211
+ *,
212
+ heads: Mapping[str, str],
213
+ published: Mapping[str, Collection[str]],
214
+ ) -> FrameClash | None:
215
+ """Return the first frame two declarations on one Machine both claim, or ``None``.
216
+
217
+ This is the cross-object half of the rule that a frame is inward or outward and never both. One
218
+ declaration contradicting itself is caught by :class:`~opensmi.ros2.server.bindings.TFBinding`.
219
+ Two cases are caught here: a frame one object publishes and another declares as its head (the
220
+ two would fight over one Location's pose), and a frame two objects publish (tf2 would keep
221
+ whichever transform arrived last). The result is deterministic: the caller's order over the
222
+ objects, then alphabetical over the frames.
223
+
224
+ :param heads: Binding name to the head frame that Binding declares.
225
+ :param published: Binding name to the frames that Binding publishes outward.
226
+ """
227
+ for name, frames in published.items():
228
+ for frame in sorted(frames):
229
+ for other, head in heads.items():
230
+ if head == frame:
231
+ return FrameClash(frame=frame, outward=name, against=other, inward=True)
232
+ for other, others in published.items():
233
+ if other != name and frame in others:
234
+ return FrameClash(frame=frame, outward=name, against=other, inward=False)
235
+ return None
236
+
237
+
238
+ def excluded(frame: str, patterns: Iterable[str]) -> bool:
239
+ """Return whether one frame is removed by a declaration's exclusions.
240
+
241
+ The patterns are ``fnmatch`` globs over the whole frame id (see
242
+ :mod:`opensmi.ros2.runtime.selection`): ``*_wheel_link`` removes four frames on a
243
+ differential-drive base, and a bare frame id removes one.
244
+ """
245
+ return any(fnmatchcase(frame, pattern) for pattern in patterns)
246
+
247
+
248
+ def angles(rotation: tuple[float, float, float, float]) -> tuple[float, float, float]:
249
+ """Return a ROS2 quaternion as the ``A``, ``B`` and ``C`` angles RSL wants, in degrees.
250
+
251
+ The convention is the nodeset's: fixed-axis roll-pitch-yaw (ISO 9787:2013), the same as tf2's
252
+ ``getRPY``. The unit is degrees because opensmi's ``Orientation`` carries ``°``. A pitch at
253
+ either pole is clamped, because floating-point drift can push the ``asin`` argument slightly
254
+ past one.
255
+
256
+ :param rotation: The quaternion as ROS2 orders it: x, y, z, w.
257
+ """
258
+ x, y, z, w = rotation
259
+ roll = math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
260
+ pitch = math.asin(max(-1.0, min(1.0, 2.0 * (w * y - z * x))))
261
+ yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
262
+ return (roll * _DEGREES, pitch * _DEGREES, yaw * _DEGREES)
263
+
264
+
265
+ def rotation(degrees: tuple[float, float, float]) -> tuple[float, float, float, float]:
266
+ """Return the ``A``, ``B`` and ``C`` angles RSL holds as the quaternion ROS2 wants.
267
+
268
+ The exact inverse of :func:`angles`, used when a pose a client writes goes out on ``/tf``. Three
269
+ angles always name one rotation, so no clamping is needed.
270
+
271
+ :param degrees: ``A``, ``B`` and ``C`` in degrees: roll about X, pitch about Y, yaw about Z.
272
+ :return: The quaternion as ROS2 orders it: x, y, z, w.
273
+ """
274
+ half_roll, half_pitch, half_yaw = (angle / _DEGREES / 2.0 for angle in degrees)
275
+ sin_roll, cos_roll = math.sin(half_roll), math.cos(half_roll)
276
+ sin_pitch, cos_pitch = math.sin(half_pitch), math.cos(half_pitch)
277
+ sin_yaw, cos_yaw = math.sin(half_yaw), math.cos(half_yaw)
278
+ return (
279
+ sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw,
280
+ cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw,
281
+ cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw,
282
+ cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw,
283
+ )