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,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
|
+
)
|