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,626 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The StartupSkill a lifecycle node drives, and that drives the lifecycle node back.
|
|
6
|
+
|
|
7
|
+
The Binding is the StartupSkill. It derives from ``BaseStartupSkill``, which carries opensmi's
|
|
8
|
+
status supervision. ``Reset`` configures every node that is not configured yet
|
|
9
|
+
(``Halted`` to ``Ready`` is ``unconfigured`` to ``inactive``) and is refused while any node is
|
|
10
|
+
Terminal. ``Start`` activates every node that is not active. ``Running`` means every node
|
|
11
|
+
is active, and nothing ticks there. ``Halt`` takes back what this Skill did: ``deactivate`` for a
|
|
12
|
+
node it activated, ``cleanup`` for one it configured, never ``shutdown`` (``finalized`` is terminal,
|
|
13
|
+
so the Skill could never be reset), and nodes that were already active are left as found. The Skill
|
|
14
|
+
is not suspendable, because opensmi's ``BaseStartupSkill`` is not.
|
|
15
|
+
|
|
16
|
+
Following is downward only. A transition somebody else caused moves the Skill toward what the nodes
|
|
17
|
+
say and stops where the client has taken it: an external ``cleanup`` or ``deactivate`` halts the
|
|
18
|
+
Skill, an external ``activate`` does not start it. Recovery is the operator's ``Reset``
|
|
19
|
+
and ``Start``. Handlers command whoever fired the trigger, so a Skill halted by one external
|
|
20
|
+
``cleanup`` also takes back the other nodes it brought up. This is safe because a Driver sends
|
|
21
|
+
nothing to a node that is already where the trigger wants it, which makes every handler idempotent.
|
|
22
|
+
Transitions that arrive while a handler is commanding are its own: they are recorded and move
|
|
23
|
+
nothing, and one settle runs when the handler is done.
|
|
24
|
+
|
|
25
|
+
The state correspondence is in :mod:`opensmi.ros2.server.lifecycle`, the services in
|
|
26
|
+
:mod:`opensmi.ros2.server.drivers`. A refused transition has no BindingEvent: opensmi's failure path
|
|
27
|
+
reports it with :data:`TRANSITION_REFUSED`.
|
|
28
|
+
"""
|
|
29
|
+
|
|
30
|
+
from __future__ import annotations
|
|
31
|
+
|
|
32
|
+
import asyncio
|
|
33
|
+
from typing import TYPE_CHECKING, Any, Final, cast
|
|
34
|
+
|
|
35
|
+
from asyncua import ua
|
|
36
|
+
from asyncua.ua.status_codes import StatusCodes
|
|
37
|
+
from opensmi.core import SkillState
|
|
38
|
+
from opensmi.core.errors import OpenSmiRuntimeError
|
|
39
|
+
from opensmi.core.ua_node_util import write_value
|
|
40
|
+
from opensmi.server.mixins import MonitoringMixin
|
|
41
|
+
from opensmi.server.user import INTERNAL_USER
|
|
42
|
+
|
|
43
|
+
from opensmi.ros2.server.browse_names import browse_name
|
|
44
|
+
from opensmi.ros2.server.errors import DeclarationError
|
|
45
|
+
from opensmi.ros2.server.events import Severity
|
|
46
|
+
from opensmi.ros2.server.lifecycle import (
|
|
47
|
+
PRIMARY_STATES,
|
|
48
|
+
RESTING,
|
|
49
|
+
LifecycleNodeState,
|
|
50
|
+
LifecycleNodeTransition,
|
|
51
|
+
aggregate,
|
|
52
|
+
resting_skill_state,
|
|
53
|
+
skill_state_for,
|
|
54
|
+
trigger_toward,
|
|
55
|
+
)
|
|
56
|
+
from opensmi.ros2.server.mutation import add_plan_variable, ensure_groups, require_unused_name
|
|
57
|
+
from opensmi.ros2.server.projection import NodeLevel, PlanVariable
|
|
58
|
+
from opensmi.ros2.server.publisher import binding_event_emitter
|
|
59
|
+
from opensmi.ros2.server.required import ROS2StartupSkill
|
|
60
|
+
|
|
61
|
+
if TYPE_CHECKING:
|
|
62
|
+
from collections.abc import Awaitable, Callable, Sequence
|
|
63
|
+
|
|
64
|
+
from asyncua.common.node import Node
|
|
65
|
+
from opensmi.server.mixins.skill_logic_mixins import TRIGGER
|
|
66
|
+
from opensmi.server.protocols import UserAuthorization
|
|
67
|
+
|
|
68
|
+
from opensmi.ros2.runtime.interface import RuntimeInterface
|
|
69
|
+
from opensmi.ros2.server.bindings import LifecycleBinding
|
|
70
|
+
from opensmi.ros2.server.drivers import LifecycleDriver
|
|
71
|
+
|
|
72
|
+
__all__ = [
|
|
73
|
+
"EXTERNALLY_MOVED",
|
|
74
|
+
"SETTLE_INTERVAL",
|
|
75
|
+
"STATE_VARIABLE",
|
|
76
|
+
"TERMINAL",
|
|
77
|
+
"TRANSITION_REFUSED",
|
|
78
|
+
"UNKNOWN_STATE",
|
|
79
|
+
"ROS2LifecycleStartupSkill",
|
|
80
|
+
]
|
|
81
|
+
|
|
82
|
+
TERMINAL: Final = "TERMINAL"
|
|
83
|
+
"""What a lifecycle Binding reports when one of its nodes reaches ``finalized``.
|
|
84
|
+
|
|
85
|
+
Refusing ``Reset`` leaves ``CurrentState`` looking like an ordinary, resettable ``Halted``, so
|
|
86
|
+
without this event a client learns only by trying. Fired once per node per latch; the latch lifts
|
|
87
|
+
when a non-terminal state is observed on that node's path.
|
|
88
|
+
"""
|
|
89
|
+
|
|
90
|
+
EXTERNALLY_MOVED: Final = "EXTERNALLY_MOVED"
|
|
91
|
+
"""What a lifecycle Binding reports when something other than us moved one of its nodes.
|
|
92
|
+
|
|
93
|
+
``CurrentState`` says where the Skill ended up, this says that nobody asked for it. A warning, since
|
|
94
|
+
an ordinary robot doing an ordinary thing causes it.
|
|
95
|
+
"""
|
|
96
|
+
|
|
97
|
+
TRANSITION_REFUSED: Final = "TRANSITION_REFUSED"
|
|
98
|
+
"""The error code a halt carries when a node or a manager would not take a transition.
|
|
99
|
+
|
|
100
|
+
It is the code on opensmi's own error message, which joins every fault of one trigger into one text
|
|
101
|
+
on this Skill's Notification. There is no BindingEvent beside it.
|
|
102
|
+
"""
|
|
103
|
+
|
|
104
|
+
STATE_VARIABLE: Final = "state"
|
|
105
|
+
"""The leaf under which every node's lifecycle state is published in the StartupSkill's Monitoring.
|
|
106
|
+
|
|
107
|
+
A variable browses as ``<Binding>/<node path>/State``, one grouping Object per segment of the node
|
|
108
|
+
path, with ``state`` as its DisplayName. ``finalized`` is an ordinary Good value, since
|
|
109
|
+
the node is answering.
|
|
110
|
+
"""
|
|
111
|
+
|
|
112
|
+
UNKNOWN_STATE: Final = "unknown"
|
|
113
|
+
"""What a node's state variable holds before anything has been heard from that node."""
|
|
114
|
+
|
|
115
|
+
SETTLE_INTERVAL: Final = 0.1
|
|
116
|
+
"""Seconds an observation waits when the Skill is mid-transition and cannot act on it yet.
|
|
117
|
+
|
|
118
|
+
Neither state machine has a completion callback, so a held observation is re-offered at this
|
|
119
|
+
interval. It runs only while something is pending and polls the Skill, never ROS2.
|
|
120
|
+
"""
|
|
121
|
+
|
|
122
|
+
|
|
123
|
+
class ROS2LifecycleStartupSkill(MonitoringMixin, ROS2StartupSkill):
|
|
124
|
+
"""One Component's lifecycle declarations, as the StartupSkill they all land on.
|
|
125
|
+
|
|
126
|
+
The declaring Component constructs it. A Component may name a subclass through
|
|
127
|
+
``startup_skill_type``. It extends :class:`opensmi.ros2.server.required.ROS2StartupSkill` with
|
|
128
|
+
the managed nodes, so a Component with both required and lifecycle Bindings waits for the
|
|
129
|
+
required ones first and then activates its nodes (see :meth:`_start_the_ros2_side`).
|
|
130
|
+
"""
|
|
131
|
+
|
|
132
|
+
def __init__(
|
|
133
|
+
self,
|
|
134
|
+
*,
|
|
135
|
+
bindings: Sequence[LifecycleBinding],
|
|
136
|
+
runtime: RuntimeInterface,
|
|
137
|
+
**kwargs: Any,
|
|
138
|
+
) -> None:
|
|
139
|
+
"""Build a Driver per declaration, before any node exists.
|
|
140
|
+
|
|
141
|
+
:param bindings: Every lifecycle declaration on the Component. They share this one Skill,
|
|
142
|
+
and their nodes aggregate lowest-wins.
|
|
143
|
+
:param runtime: The Machine's Runtime, reached through the Component.
|
|
144
|
+
:param kwargs: What :class:`~opensmi.ros2.server.required.ROS2StartupSkill` takes.
|
|
145
|
+
:raises DeclarationError: When two declarations follow the same ROS2 node.
|
|
146
|
+
:raises MessageTypeError: When an interface package a declared flavour needs is missing.
|
|
147
|
+
"""
|
|
148
|
+
super().__init__(**kwargs)
|
|
149
|
+
self._bindings = tuple(bindings)
|
|
150
|
+
self._drivers = tuple(_driver_for(binding, runtime) for binding in self._bindings)
|
|
151
|
+
self._owner = _require_one_owner_per_node(self._drivers)
|
|
152
|
+
self._emit: Callable[..., Awaitable[None]] | None = None
|
|
153
|
+
# What each node last said about itself, as the Skill state it means. A node nobody has
|
|
154
|
+
# heard from is absent (no opinion), so a vanished node cannot tear a robot down.
|
|
155
|
+
self._observed: dict[str, SkillState] = {}
|
|
156
|
+
# Whether each node is on the graph, which is the answer to Satisfied for a lifecycle
|
|
157
|
+
# Binding. A vanished node keeps its last state and has presence False.
|
|
158
|
+
self._present: dict[str, bool] = {}
|
|
159
|
+
self._terminal: dict[str, str] = {}
|
|
160
|
+
# The transitions this Skill itself sent each node, which is all a halt may take back
|
|
161
|
+
# .
|
|
162
|
+
self._performed: dict[str, set[LifecycleNodeTransition]] = {}
|
|
163
|
+
self._variables: dict[str, Node] = {}
|
|
164
|
+
self._commanding = asyncio.Lock()
|
|
165
|
+
self._pending = asyncio.Event()
|
|
166
|
+
self._firing: TRIGGER | None = None
|
|
167
|
+
# Nodes that announced a transition while a handler was commanding. Their event carries a
|
|
168
|
+
# route and the read-back does not, so the read-back defers to them.
|
|
169
|
+
self._announced: set[str] = set()
|
|
170
|
+
self._tasks: list[asyncio.Task[None]] = []
|
|
171
|
+
|
|
172
|
+
@property
|
|
173
|
+
def bindings(self) -> tuple[LifecycleBinding, ...]:
|
|
174
|
+
"""Return the declarations this Skill was built from, in declaration order."""
|
|
175
|
+
return self._bindings
|
|
176
|
+
|
|
177
|
+
@property
|
|
178
|
+
def drivers(self) -> tuple[LifecycleDriver, ...]:
|
|
179
|
+
"""Return one Driver per declaration, in the same order."""
|
|
180
|
+
return self._drivers
|
|
181
|
+
|
|
182
|
+
@property
|
|
183
|
+
def observed(self) -> dict[str, SkillState]:
|
|
184
|
+
"""Return a copy of what each node last said about itself, as the Skill state it means."""
|
|
185
|
+
return dict(self._observed)
|
|
186
|
+
|
|
187
|
+
@property
|
|
188
|
+
def node_presence(self) -> dict[str, bool]:
|
|
189
|
+
"""Return a copy of whether each declared node is on the graph, by node path.
|
|
190
|
+
|
|
191
|
+
A vanished node keeps its last state, because absence is not a lifecycle state. Presence
|
|
192
|
+
comes from the node's ``transition_event`` subscription and answers a required lifecycle
|
|
193
|
+
Binding (:func:`opensmi.ros2.server.required.satisfaction_of`).
|
|
194
|
+
"""
|
|
195
|
+
return dict(self._present)
|
|
196
|
+
|
|
197
|
+
@property
|
|
198
|
+
def terminal(self) -> tuple[str, ...]:
|
|
199
|
+
"""Return the node paths currently latched Terminal, in sorted order.
|
|
200
|
+
|
|
201
|
+
While this is non-empty, ``Reset`` is refused.
|
|
202
|
+
"""
|
|
203
|
+
return tuple(sorted(self._terminal))
|
|
204
|
+
|
|
205
|
+
async def _init(self) -> None:
|
|
206
|
+
"""Build the Skill, give every node a state variable, and start following.
|
|
207
|
+
|
|
208
|
+
Nothing is asked of the robot here, so a Component whose robot is off still builds.
|
|
209
|
+
The first question goes out when a ``transition_event`` subscription gets a
|
|
210
|
+
publisher.
|
|
211
|
+
|
|
212
|
+
:raises DeclarationError: When the Component cannot raise a ``BindingEvent``.
|
|
213
|
+
"""
|
|
214
|
+
await super()._init()
|
|
215
|
+
self._emit = binding_event_emitter(
|
|
216
|
+
self.parent,
|
|
217
|
+
"a lifecycle Binding is declared on a Component that cannot raise a BindingEvent, "
|
|
218
|
+
"and the Terminal event is the only thing that tells a Halted StartupSkill which can "
|
|
219
|
+
"be reset from one which never can; a Component carrying ROS2 Bindings mixes in "
|
|
220
|
+
"ROS2ComponentMixin",
|
|
221
|
+
)
|
|
222
|
+
await self._create_state_variables()
|
|
223
|
+
for driver in self._drivers:
|
|
224
|
+
await driver.open()
|
|
225
|
+
self._start_following()
|
|
226
|
+
|
|
227
|
+
async def close_startup(self) -> None:
|
|
228
|
+
"""Release the gate this Skill inherits, and then its own subscriptions. Idempotent."""
|
|
229
|
+
await super().close_startup()
|
|
230
|
+
await self.close_drivers()
|
|
231
|
+
|
|
232
|
+
async def close_drivers(self) -> None:
|
|
233
|
+
"""Stop following and release every subscription. Idempotent."""
|
|
234
|
+
tasks, self._tasks = self._tasks, []
|
|
235
|
+
fired, self._fired = self._fired, set()
|
|
236
|
+
for task in (*tasks, *fired):
|
|
237
|
+
task.cancel()
|
|
238
|
+
await asyncio.gather(*tasks, *fired, return_exceptions=True)
|
|
239
|
+
for driver in self._drivers:
|
|
240
|
+
await driver.close()
|
|
241
|
+
|
|
242
|
+
async def _condition_reset_allowed(self, _user: UserAuthorization) -> bool:
|
|
243
|
+
"""Refuse ``Reset`` while any node behind this Skill is Terminal.
|
|
244
|
+
|
|
245
|
+
Commanding a transition at a ``finalized`` node returns ``success=False`` on a C++ node and
|
|
246
|
+
kills the process on an rclpy one.
|
|
247
|
+
|
|
248
|
+
:raises UaStatusCodeError: ``BadInvalidState``. It is raised because a bare ``False`` would
|
|
249
|
+
reach ``try_trigger`` as an unexplained ``Good``.
|
|
250
|
+
"""
|
|
251
|
+
if not self._terminal:
|
|
252
|
+
return True
|
|
253
|
+
listed = ", ".join(self.terminal)
|
|
254
|
+
refused = (
|
|
255
|
+
f"Denied, {listed} reached the terminal lifecycle state and can never be commanded "
|
|
256
|
+
f"again; the node object has to be destroyed and recreated, which is a process "
|
|
257
|
+
f"restart and not something this adapter or OPC UA can do"
|
|
258
|
+
)
|
|
259
|
+
await self.ua_log_error(refused, code=TERMINAL)
|
|
260
|
+
raise ua.UaStatusCodeError(StatusCodes.BadInvalidState)
|
|
261
|
+
|
|
262
|
+
async def _handle_resetting(self) -> None:
|
|
263
|
+
"""Configure every node."""
|
|
264
|
+
await self._drive("reset")
|
|
265
|
+
|
|
266
|
+
async def _start_the_ros2_side(self) -> None:
|
|
267
|
+
"""Activate every node, and check they all got there.
|
|
268
|
+
|
|
269
|
+
This is the lifecycle half of ``Starting``, run after the required Bindings were waited for.
|
|
270
|
+
A required lifecycle Binding is thus waited for before its nodes are commanded: a node that
|
|
271
|
+
is not on the graph does not answer, and that is a fault rather than a wait. Nodes of other
|
|
272
|
+
required Bindings have their publishers by now, because a managed node creates them on
|
|
273
|
+
``configure``, which ``Reset`` has already done.
|
|
274
|
+
"""
|
|
275
|
+
await self._drive("start")
|
|
276
|
+
self._require_every_node_active()
|
|
277
|
+
|
|
278
|
+
def _require_every_node_active(self) -> None:
|
|
279
|
+
"""Refuse to reach ``Running`` unless every node actually reached ``active``.
|
|
280
|
+
|
|
281
|
+
The Driver's gate is quiet: a node that offers no ``activate`` is skipped. That is right for
|
|
282
|
+
``halt`` but not for ``start``, where a node left ``unconfigured`` would let ``Start``
|
|
283
|
+
succeed while the Skill claims a robot it does not have. The read-back is checked, not the
|
|
284
|
+
command.
|
|
285
|
+
|
|
286
|
+
:raises OpenSmiRuntimeError: When any node reports anything other than ``active``.
|
|
287
|
+
"""
|
|
288
|
+
idle = [path for path, state in self._observed.items() if state is not SkillState.RUNNING]
|
|
289
|
+
if not idle:
|
|
290
|
+
return
|
|
291
|
+
message = (
|
|
292
|
+
f"start: {', '.join(idle)} did not reach active, and Running means every node is "
|
|
293
|
+
f"active; a node that offers no activate is not commanded, so one resting somewhere "
|
|
294
|
+
f"else has to halt the Skill rather than be started around"
|
|
295
|
+
)
|
|
296
|
+
raise OpenSmiRuntimeError(message, error_code=TRANSITION_REFUSED)
|
|
297
|
+
|
|
298
|
+
async def _handle_halting(self) -> None:
|
|
299
|
+
"""Take back what this Skill brought up, and let opensmi halt what it halts.
|
|
300
|
+
|
|
301
|
+
Only transitions the Skill performed itself are rolled back, so a node that was already
|
|
302
|
+
active when the Skill started is not touched. Never ``shutdown``, since
|
|
303
|
+
``finalized`` is terminal and the Skill could not be reset. opensmi's own halting runs
|
|
304
|
+
whatever the robot did, and the faults are raised afterwards.
|
|
305
|
+
"""
|
|
306
|
+
faults = await self._command("halt")
|
|
307
|
+
await super()._handle_halting()
|
|
308
|
+
_require_nothing_refused(faults, "halt")
|
|
309
|
+
|
|
310
|
+
async def _drive(self, trigger: TRIGGER) -> None:
|
|
311
|
+
"""Command one trigger at every node, and halt this Skill if anything refused.
|
|
312
|
+
|
|
313
|
+
:raises OpenSmiRuntimeError: When a node or a manager refused or did not answer.
|
|
314
|
+
"""
|
|
315
|
+
_require_nothing_refused(await self._command(trigger), trigger)
|
|
316
|
+
|
|
317
|
+
async def _command(self, trigger: TRIGGER) -> tuple[str, ...]:
|
|
318
|
+
"""Ask every Driver to drive its nodes toward what one trigger means, and collect faults.
|
|
319
|
+
|
|
320
|
+
Idempotent, since each Driver gates every transition on what its nodes report as available.
|
|
321
|
+
"""
|
|
322
|
+
faults: list[str] = []
|
|
323
|
+
async with self._commanding:
|
|
324
|
+
self._announced.clear()
|
|
325
|
+
for driver in self._drivers:
|
|
326
|
+
faults.extend(await driver.apply(trigger, self._performed))
|
|
327
|
+
await self._read_back()
|
|
328
|
+
self._pending.set()
|
|
329
|
+
return tuple(faults)
|
|
330
|
+
|
|
331
|
+
async def _read_back(self) -> None:
|
|
332
|
+
"""Ask every node where it ended up, rather than assuming the command took it there.
|
|
333
|
+
|
|
334
|
+
A node announces the transitions we caused too, and a Skill that ignored its own commands
|
|
335
|
+
would read its successful ``Reset`` as a robot that never moved. The read has no route, so
|
|
336
|
+
it gives a rank but not which state at that rank. A node that announced a transition inside
|
|
337
|
+
this window is skipped: its event carries a route, and overwriting it would turn somebody
|
|
338
|
+
else's failed activation (``Halted``) into a routeless ``inactive`` (``Ready``).
|
|
339
|
+
"""
|
|
340
|
+
for driver in self._drivers:
|
|
341
|
+
for path in driver.paths:
|
|
342
|
+
if path in self._announced:
|
|
343
|
+
continue
|
|
344
|
+
state = await driver.read_state(path)
|
|
345
|
+
if state is not None:
|
|
346
|
+
await self._note(path, state, resting_skill_state(state))
|
|
347
|
+
|
|
348
|
+
def _start_following(self) -> None:
|
|
349
|
+
"""Start one task per node per signal (transitions and appearances), plus the settling one.
|
|
350
|
+
|
|
351
|
+
An appearance is the subscription's own DDS match event, which is the "one state query on
|
|
352
|
+
first discovery".
|
|
353
|
+
"""
|
|
354
|
+
for driver in self._drivers:
|
|
355
|
+
for path in driver.paths:
|
|
356
|
+
self._spawn(self._follow_transitions(driver, path), f"transitions{path}")
|
|
357
|
+
self._spawn(self._follow_appearances(driver, path), f"appearances{path}")
|
|
358
|
+
self._spawn(self._settling(), "settling")
|
|
359
|
+
|
|
360
|
+
def _spawn(self, work: Any, label: str) -> None:
|
|
361
|
+
"""Run one following coroutine as a task this Skill owns and cancels.
|
|
362
|
+
|
|
363
|
+
These tasks stay out of opensmi's own task bookkeeping, which is cancelled when the Skill
|
|
364
|
+
halts, because a Skill that stopped watching on halt could never see its nodes come back.
|
|
365
|
+
"""
|
|
366
|
+
self._tasks.append(asyncio.create_task(work, name=f"ros2-lifecycle-{self.name}-{label}"))
|
|
367
|
+
|
|
368
|
+
async def _follow_transitions(self, driver: LifecycleDriver, path: str) -> None:
|
|
369
|
+
"""Record every transition one node announces, reading the pair and never the field."""
|
|
370
|
+
async for transition in driver.transitions(path):
|
|
371
|
+
await self._note(
|
|
372
|
+
path,
|
|
373
|
+
transition.goal,
|
|
374
|
+
skill_state_for(start=transition.start, goal=transition.goal),
|
|
375
|
+
routed=True,
|
|
376
|
+
)
|
|
377
|
+
|
|
378
|
+
async def _follow_appearances(self, driver: LifecycleDriver, path: str) -> None:
|
|
379
|
+
"""Ask a node what state it is in each time it appears, and only then.
|
|
380
|
+
|
|
381
|
+
A server starting against an already ``finalized`` node gets no transition event, and
|
|
382
|
+
without this it would offer a ``Reset`` that cannot work. The node's
|
|
383
|
+
``transition_event`` publisher is still matched, so it can be seen.
|
|
384
|
+
"""
|
|
385
|
+
there = False
|
|
386
|
+
self._present.setdefault(path, False)
|
|
387
|
+
async for publishers in driver.appearances(path):
|
|
388
|
+
self._present[path] = publishers > 0
|
|
389
|
+
if publishers == 0:
|
|
390
|
+
# Gone: what the node last said stands, because absence is not a lifecycle state.
|
|
391
|
+
there = False
|
|
392
|
+
continue
|
|
393
|
+
if there:
|
|
394
|
+
# A second publisher on the topic is not a node arriving.
|
|
395
|
+
continue
|
|
396
|
+
there = True
|
|
397
|
+
state = await driver.read_state(path)
|
|
398
|
+
if state is None:
|
|
399
|
+
continue
|
|
400
|
+
await self._note(path, state, resting_skill_state(state))
|
|
401
|
+
|
|
402
|
+
async def _note(
|
|
403
|
+
self,
|
|
404
|
+
path: str,
|
|
405
|
+
state: LifecycleNodeState | int,
|
|
406
|
+
means: SkillState | None,
|
|
407
|
+
*,
|
|
408
|
+
routed: bool = False,
|
|
409
|
+
) -> None:
|
|
410
|
+
"""Record one thing a node said about itself, and ask for the Skill to be settled.
|
|
411
|
+
|
|
412
|
+
Transition states are not news: the Skill has its own intermediate states for them.
|
|
413
|
+
|
|
414
|
+
:param routed: Whether this came from a ``transition_event`` and so carries the state pair.
|
|
415
|
+
A routed observation outranks the routeless read-back of :meth:`_read_back`.
|
|
416
|
+
"""
|
|
417
|
+
if state not in PRIMARY_STATES:
|
|
418
|
+
return
|
|
419
|
+
if routed:
|
|
420
|
+
self._announced.add(path)
|
|
421
|
+
await self._write_state(path, state)
|
|
422
|
+
await self._latch_terminal(path, state)
|
|
423
|
+
if means is not None:
|
|
424
|
+
self._observed[path] = means
|
|
425
|
+
self._pending.set()
|
|
426
|
+
|
|
427
|
+
async def _latch_terminal(self, path: str, state: LifecycleNodeState | int) -> None:
|
|
428
|
+
"""Latch or lift one node's Terminal condition, firing the event exactly on the latch.
|
|
429
|
+
|
|
430
|
+
Only observing another primary state on that path lifts it. The node vanishing does not,
|
|
431
|
+
since the last thing known is still ``finalized``.
|
|
432
|
+
"""
|
|
433
|
+
binding = self._owner[path]
|
|
434
|
+
if state != LifecycleNodeState.FINALIZED:
|
|
435
|
+
self._terminal.pop(path, None)
|
|
436
|
+
return
|
|
437
|
+
if path in self._terminal:
|
|
438
|
+
return
|
|
439
|
+
self._terminal[path] = binding.name
|
|
440
|
+
await self._report(
|
|
441
|
+
binding.name,
|
|
442
|
+
path,
|
|
443
|
+
TERMINAL,
|
|
444
|
+
f"{path} reached the terminal lifecycle state; its process is alive and no transition "
|
|
445
|
+
f"can move it again, so Reset is refused until a restart puts a non-terminal state on "
|
|
446
|
+
f"that path",
|
|
447
|
+
severity=Severity.ERROR,
|
|
448
|
+
)
|
|
449
|
+
|
|
450
|
+
async def _settling(self) -> None:
|
|
451
|
+
"""Move the Skill toward what its nodes say, whenever something new has been recorded.
|
|
452
|
+
|
|
453
|
+
An observation the Skill cannot take yet (mid-transition, or a handler is commanding) is
|
|
454
|
+
held and re-offered every :data:`SETTLE_INTERVAL`.
|
|
455
|
+
"""
|
|
456
|
+
holding = False
|
|
457
|
+
while True:
|
|
458
|
+
try:
|
|
459
|
+
await asyncio.wait_for(self._pending.wait(), timeout=SETTLE_INTERVAL if holding else None)
|
|
460
|
+
except TimeoutError:
|
|
461
|
+
# Nothing new arrived; offer the held observation again.
|
|
462
|
+
pass
|
|
463
|
+
else:
|
|
464
|
+
self._pending.clear()
|
|
465
|
+
holding = not await self._settle()
|
|
466
|
+
|
|
467
|
+
async def _settle(self) -> bool:
|
|
468
|
+
"""Fire at most one trigger toward the aggregate, and say whether the Skill could act.
|
|
469
|
+
|
|
470
|
+
:return: ``False`` when the Skill is not resting and the observation has to be re-offered;
|
|
471
|
+
``True`` when it acted, or when there was nothing for it to do.
|
|
472
|
+
"""
|
|
473
|
+
if self._commanding.locked() or self._firing is not None:
|
|
474
|
+
return False
|
|
475
|
+
target = aggregate(self._observed.values())
|
|
476
|
+
if target is None:
|
|
477
|
+
return True
|
|
478
|
+
current = self.current_state
|
|
479
|
+
trigger = trigger_toward(current, target)
|
|
480
|
+
if trigger is None:
|
|
481
|
+
return current in RESTING
|
|
482
|
+
binding, path = self._weakest(target)
|
|
483
|
+
await self._report(
|
|
484
|
+
binding.name,
|
|
485
|
+
path,
|
|
486
|
+
EXTERNALLY_MOVED,
|
|
487
|
+
f"{path} moved to what this Skill reads as {target.localized_text.Text} without being asked "
|
|
488
|
+
f"to, so the StartupSkill is following it from {current.localized_text.Text} by halting",
|
|
489
|
+
severity=Severity.WARNING,
|
|
490
|
+
)
|
|
491
|
+
self._fire(trigger)
|
|
492
|
+
# The Skill has not moved yet, so the observation is held until it has.
|
|
493
|
+
return False
|
|
494
|
+
|
|
495
|
+
def _fire(self, trigger: TRIGGER) -> None:
|
|
496
|
+
"""Fire one trigger from a task of its own, never from the task that decided to.
|
|
497
|
+
|
|
498
|
+
``AsyncMachine`` cancels the tasks processing a transition when a new one begins and
|
|
499
|
+
swallows the ``CancelledError``, so a long-lived task awaiting a trigger would silently stop
|
|
500
|
+
following.
|
|
501
|
+
"""
|
|
502
|
+
self._firing = trigger
|
|
503
|
+
task = asyncio.create_task(
|
|
504
|
+
self._trigger_once(trigger),
|
|
505
|
+
name=f"ros2-lifecycle-{self.name}-{trigger}",
|
|
506
|
+
)
|
|
507
|
+
self._fired.add(task)
|
|
508
|
+
task.add_done_callback(self._fired.discard)
|
|
509
|
+
|
|
510
|
+
async def _trigger_once(self, trigger: TRIGGER) -> None:
|
|
511
|
+
"""Offer one trigger to the state machine, tolerating a state it is not valid from.
|
|
512
|
+
|
|
513
|
+
``try_trigger`` answers with a status code instead of raising, so a transition arriving
|
|
514
|
+
mid-handler is harmless.
|
|
515
|
+
"""
|
|
516
|
+
try:
|
|
517
|
+
await self._machine.try_trigger(cast("Any", INTERNAL_USER), trigger)
|
|
518
|
+
finally:
|
|
519
|
+
self._firing = None
|
|
520
|
+
|
|
521
|
+
def _weakest(self, target: SkillState) -> tuple[LifecycleBinding, str]:
|
|
522
|
+
"""Return the Binding and node path that dragged the aggregate down to ``target``.
|
|
523
|
+
|
|
524
|
+
The first in declaration order wins a tie.
|
|
525
|
+
"""
|
|
526
|
+
path = next(path for path, state in self._observed.items() if state == target)
|
|
527
|
+
return self._owner[path], path
|
|
528
|
+
|
|
529
|
+
async def _create_state_variables(self) -> None:
|
|
530
|
+
"""Give every declared node one Monitoring variable holding its lifecycle state.
|
|
531
|
+
|
|
532
|
+
Created at init from the declaration alone, so they are readable against a robot that is
|
|
533
|
+
off. They hold :data:`UNKNOWN_STATE` until the node speaks, then the ROS2 label.
|
|
534
|
+
|
|
535
|
+
:raises DuplicateVariableNameError: When two declarations claim one state variable, which is
|
|
536
|
+
the same Binding name and node path. This differs from two declarations following one
|
|
537
|
+
node (see :func:`_require_one_owner_per_node`).
|
|
538
|
+
"""
|
|
539
|
+
for binding in self._bindings:
|
|
540
|
+
for path in binding.observed:
|
|
541
|
+
entry = PlanVariable(
|
|
542
|
+
name=browse_name(STATE_VARIABLE),
|
|
543
|
+
display_name=STATE_VARIABLE,
|
|
544
|
+
above=(
|
|
545
|
+
NodeLevel(name=binding.name, display_name=binding.name, group=True),
|
|
546
|
+
*(
|
|
547
|
+
NodeLevel(name=browse_name(segment), display_name=segment, group=True)
|
|
548
|
+
for segment in path.strip("/").split("/")
|
|
549
|
+
),
|
|
550
|
+
),
|
|
551
|
+
source=path,
|
|
552
|
+
initial=UNKNOWN_STATE,
|
|
553
|
+
variant_type=ua.VariantType.String,
|
|
554
|
+
)
|
|
555
|
+
group = await ensure_groups(self.monitoring, entry.above)
|
|
556
|
+
require_unused_name(entry.name, (group,), "this StartupSkill's Monitoring")
|
|
557
|
+
variable = await add_plan_variable(group, entry.name, entry, None)
|
|
558
|
+
self._variables[path] = variable.ua_node
|
|
559
|
+
|
|
560
|
+
async def _write_state(self, path: str, state: LifecycleNodeState | int) -> None:
|
|
561
|
+
"""Publish one node's lifecycle state as the label ROS2 spells it, with Good status."""
|
|
562
|
+
await write_value(
|
|
563
|
+
self._variables[path], LifecycleNodeState(int(state)).name.lower(), variant_type=ua.VariantType.String
|
|
564
|
+
)
|
|
565
|
+
|
|
566
|
+
async def _report(
|
|
567
|
+
self,
|
|
568
|
+
binding: str,
|
|
569
|
+
path: str,
|
|
570
|
+
code: str,
|
|
571
|
+
text: str,
|
|
572
|
+
severity: Severity = Severity.ERROR,
|
|
573
|
+
) -> None:
|
|
574
|
+
"""Fire one BindingEvent about one node, and never let the reporting be the fault.
|
|
575
|
+
|
|
576
|
+
A failure to raise the event is only logged.
|
|
577
|
+
"""
|
|
578
|
+
if self._emit is None: # pragma: no cover -- resolved at init, before anything follows
|
|
579
|
+
return
|
|
580
|
+
try:
|
|
581
|
+
await self._emit(binding=binding, path=path, code=code, severity=severity, text=text)
|
|
582
|
+
except Exception:
|
|
583
|
+
self.logger.exception(
|
|
584
|
+
"Could not fire a BindingEvent about a lifecycle node",
|
|
585
|
+
binding=binding,
|
|
586
|
+
node=path,
|
|
587
|
+
code=code,
|
|
588
|
+
)
|
|
589
|
+
|
|
590
|
+
|
|
591
|
+
def _driver_for(binding: LifecycleBinding, runtime: RuntimeInterface) -> LifecycleDriver:
|
|
592
|
+
"""Return the Driver one declaration asked for: its own class, or the flavour it named."""
|
|
593
|
+
driver_type: Any = binding.driver_class
|
|
594
|
+
built: LifecycleDriver = driver_type(binding=binding, runtime=runtime)
|
|
595
|
+
return built
|
|
596
|
+
|
|
597
|
+
|
|
598
|
+
def _require_one_owner_per_node(drivers: Sequence[LifecycleDriver]) -> dict[str, LifecycleBinding]:
|
|
599
|
+
"""Return which declaration owns each node path.
|
|
600
|
+
|
|
601
|
+
:raises DeclarationError: When one node path appears under two declarations.
|
|
602
|
+
"""
|
|
603
|
+
owner: dict[str, LifecycleBinding] = {}
|
|
604
|
+
for driver in drivers:
|
|
605
|
+
for path in driver.paths:
|
|
606
|
+
existing = owner.get(path)
|
|
607
|
+
if existing is not None:
|
|
608
|
+
message = (
|
|
609
|
+
f"the lifecycle node {path!r} is followed by both {existing.name!r} and "
|
|
610
|
+
f"{driver.binding.name!r}; one node has one Driver, because two would "
|
|
611
|
+
f"command it twice and disagree about what it last said"
|
|
612
|
+
)
|
|
613
|
+
raise DeclarationError(message)
|
|
614
|
+
owner[path] = driver.binding
|
|
615
|
+
return owner
|
|
616
|
+
|
|
617
|
+
|
|
618
|
+
def _require_nothing_refused(faults: Sequence[str], trigger: str) -> None:
|
|
619
|
+
"""Halt the Skill when anything refused a transition, naming every cause.
|
|
620
|
+
|
|
621
|
+
:raises OpenSmiRuntimeError: When there is at least one fault.
|
|
622
|
+
"""
|
|
623
|
+
if not faults:
|
|
624
|
+
return
|
|
625
|
+
refused = f"{trigger}: {'; '.join(faults)}"
|
|
626
|
+
raise OpenSmiRuntimeError(refused, error_code=TRANSITION_REFUSED)
|