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,421 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Drivers: which service, which encoding, which topic, and no state machine.
|
|
6
|
+
|
|
7
|
+
A Driver is the part of a lifecycle Binding that knows how one flavour of coordinator is commanded.
|
|
8
|
+
The mapping onto Skill states is shared and lives in :mod:`opensmi.ros2.server.lifecycle`, so
|
|
9
|
+
Components driven by different Drivers present the same OPC UA surface. Two ship:
|
|
10
|
+
|
|
11
|
+
* :class:`StandardLifecycleDriver` is the ROS2 standard, ``lifecycle_msgs``, one node at a time via
|
|
12
|
+
``<node>/change_state``.
|
|
13
|
+
* :class:`Nav2ManagerDriver` is a nav2-style coordinator that drives a set of nodes with one
|
|
14
|
+
``<manager>/manage_nodes`` call.
|
|
15
|
+
|
|
16
|
+
Observing is common to both, because a nav2-managed node is an ordinary lifecycle node. Every
|
|
17
|
+
Driver gates each transition on what the node reports as available. An invalid change request kills
|
|
18
|
+
an rclpy node (Jazzy 7.1.11: the ``RCLError`` propagates out of ``rclpy.spin()``), and a
|
|
19
|
+
``finalized`` node offers nothing, so a node that does not offer a transition is skipped. Discovery
|
|
20
|
+
uses each node's ``transition_event`` subscription (transitions and publisher matches) instead of
|
|
21
|
+
polling the graph. A third flavour subclasses :class:`LifecycleDriver`, implements
|
|
22
|
+
:meth:`~LifecycleDriver.send` and is given as the Binding's ``driver``.
|
|
23
|
+
|
|
24
|
+
Nothing here imports ``rclpy`` or touches OPC UA, so a Driver is tested with the fake Runtime.
|
|
25
|
+
"""
|
|
26
|
+
|
|
27
|
+
from __future__ import annotations
|
|
28
|
+
|
|
29
|
+
from abc import ABC, abstractmethod
|
|
30
|
+
from typing import TYPE_CHECKING, Any, ClassVar, Final
|
|
31
|
+
|
|
32
|
+
from opensmi.ros2.runtime.errors import ServiceCallTimeoutError, ServiceUnavailableError
|
|
33
|
+
from opensmi.ros2.server.lifecycle import TRANSITIONS_FOR, UNDONE_BY, LifecycleNodeState, LifecycleNodeTransition
|
|
34
|
+
|
|
35
|
+
if TYPE_CHECKING:
|
|
36
|
+
from collections.abc import AsyncIterator, Iterable, Mapping, Sequence
|
|
37
|
+
|
|
38
|
+
from opensmi.server.mixins.skill_logic_mixins import TRIGGER
|
|
39
|
+
|
|
40
|
+
from opensmi.ros2.runtime import Message, RuntimeInterface, ServiceTypes, Subscription
|
|
41
|
+
from opensmi.ros2.server.bindings import LifecycleBinding
|
|
42
|
+
|
|
43
|
+
__all__ = [
|
|
44
|
+
"CHANGE_STATE_TYPE",
|
|
45
|
+
"DRIVERS",
|
|
46
|
+
"GET_AVAILABLE_TRANSITIONS_TYPE",
|
|
47
|
+
"GET_STATE_TYPE",
|
|
48
|
+
"MANAGE_NODES_TYPE",
|
|
49
|
+
"NAV2_COMMANDS",
|
|
50
|
+
"TRANSITION_EVENT_TYPE",
|
|
51
|
+
"LifecycleDriver",
|
|
52
|
+
"Nav2ManagerDriver",
|
|
53
|
+
"NodeTransition",
|
|
54
|
+
"StandardLifecycleDriver",
|
|
55
|
+
]
|
|
56
|
+
|
|
57
|
+
TRANSITION_EVENT_TYPE: Final = "lifecycle_msgs/msg/TransitionEvent"
|
|
58
|
+
"""The topic type every managed node announces its transitions on, whoever coordinates it."""
|
|
59
|
+
|
|
60
|
+
GET_STATE_TYPE: Final = "lifecycle_msgs/srv/GetState"
|
|
61
|
+
"""The service type behind ``<node>/get_state``: one query, answered by the node itself."""
|
|
62
|
+
|
|
63
|
+
CHANGE_STATE_TYPE: Final = "lifecycle_msgs/srv/ChangeState"
|
|
64
|
+
"""The service type behind ``<node>/change_state``, which only the standard flavour sends."""
|
|
65
|
+
|
|
66
|
+
GET_AVAILABLE_TRANSITIONS_TYPE: Final = "lifecycle_msgs/srv/GetAvailableTransitions"
|
|
67
|
+
"""The service type behind ``<node>/get_available_transitions``, which is the gate."""
|
|
68
|
+
|
|
69
|
+
MANAGE_NODES_TYPE: Final = "nav2_msgs/srv/ManageLifecycleNodes"
|
|
70
|
+
"""The service type a nav2-style manager takes: one ``uint8`` command covering a whole set.
|
|
71
|
+
|
|
72
|
+
Its interface package is nav2's, so naming this flavour without nav2 installed is fatal at init.
|
|
73
|
+
"""
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
NAV2_COMMANDS: Final[Mapping[LifecycleNodeTransition, int]] = {
|
|
77
|
+
LifecycleNodeTransition.CONFIGURE: 5, # ManageLifecycleNodes.CONFIGURE
|
|
78
|
+
LifecycleNodeTransition.CLEANUP: 6, # ManageLifecycleNodes.CLEANUP
|
|
79
|
+
LifecycleNodeTransition.ACTIVATE: 2, # ManageLifecycleNodes.RESUME
|
|
80
|
+
LifecycleNodeTransition.DEACTIVATE: 1, # ManageLifecycleNodes.PAUSE
|
|
81
|
+
}
|
|
82
|
+
"""Which nav2 manager command sends which lifecycle transition at the set it drives.
|
|
83
|
+
|
|
84
|
+
The numbers are spelled out because the constants live on a generated nav2 class that cannot be
|
|
85
|
+
imported without nav2. ``RESUME`` and ``PAUSE`` are the manager's words for ``activate`` and
|
|
86
|
+
``deactivate``. ``STARTUP`` and ``RESET`` are compounds already expressed in
|
|
87
|
+
:data:`opensmi.ros2.server.lifecycle.TRANSITIONS_FOR`, and ``SHUTDOWN`` ends in ``finalized``,
|
|
88
|
+
which this project never commands.
|
|
89
|
+
"""
|
|
90
|
+
|
|
91
|
+
|
|
92
|
+
class NodeTransition:
|
|
93
|
+
"""One transition a node announced: the node's path and its start and goal state ids.
|
|
94
|
+
|
|
95
|
+
The event's ``transition`` and ``timestamp`` fields are not kept, because ``rcl_lifecycle``
|
|
96
|
+
leaves them empty.
|
|
97
|
+
"""
|
|
98
|
+
|
|
99
|
+
__slots__ = ("goal", "path", "start")
|
|
100
|
+
|
|
101
|
+
def __init__(self, *, path: str, start: int, goal: int) -> None:
|
|
102
|
+
"""Record one node's move between two lifecycle states."""
|
|
103
|
+
self.path = path
|
|
104
|
+
self.start = start
|
|
105
|
+
self.goal = goal
|
|
106
|
+
|
|
107
|
+
def __repr__(self) -> str:
|
|
108
|
+
"""Return a representation naming the node and both ends of the move."""
|
|
109
|
+
return f"<NodeTransition {self.path} {_state_name(self.start)} -> {_state_name(self.goal)}>"
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
class LifecycleDriver(ABC):
|
|
113
|
+
"""One coordinator flavour, commanded and observed. Holds no state machine.
|
|
114
|
+
|
|
115
|
+
Constructed by the StartupSkill the Binding lands on. A declaration may name a subclass to add
|
|
116
|
+
a third flavour.
|
|
117
|
+
|
|
118
|
+
:cvar flavour: The name a declaration selects this Driver by, and its key in :data:`DRIVERS`.
|
|
119
|
+
:cvar coordinates_a_set: Whether ``path`` names a coordinator other than the node it drives,
|
|
120
|
+
so that the nodes need to be listed. True for a nav2 manager, false for the standard flavour.
|
|
121
|
+
"""
|
|
122
|
+
|
|
123
|
+
flavour: ClassVar[str]
|
|
124
|
+
coordinates_a_set: ClassVar[bool] = False
|
|
125
|
+
|
|
126
|
+
def __init__(self, *, binding: LifecycleBinding, runtime: RuntimeInterface) -> None:
|
|
127
|
+
"""Resolve every ROS2 type this flavour needs, before anything is subscribed or called.
|
|
128
|
+
|
|
129
|
+
A missing interface package is a declaration error and fatal at init.
|
|
130
|
+
|
|
131
|
+
:param binding: What to drive.
|
|
132
|
+
:param runtime: The Machine's Runtime, reached through the Component.
|
|
133
|
+
:raises MessageTypeError: When an interface package this flavour needs is not installed.
|
|
134
|
+
"""
|
|
135
|
+
self._binding = binding
|
|
136
|
+
self._runtime = runtime
|
|
137
|
+
self._event_type = runtime.describe_message(TRANSITION_EVENT_TYPE)
|
|
138
|
+
self._get_state: ServiceTypes = runtime.describe_service(GET_STATE_TYPE)
|
|
139
|
+
self._get_available: ServiceTypes = runtime.describe_service(GET_AVAILABLE_TRANSITIONS_TYPE)
|
|
140
|
+
self._subscriptions: dict[str, Subscription] = {}
|
|
141
|
+
|
|
142
|
+
def __repr__(self) -> str:
|
|
143
|
+
"""Return a representation naming the flavour and what it drives."""
|
|
144
|
+
return f"<{type(self).__name__} {self._binding.path} driving {', '.join(self.paths)}>"
|
|
145
|
+
|
|
146
|
+
@property
|
|
147
|
+
def binding(self) -> LifecycleBinding:
|
|
148
|
+
"""Return the declaration this Driver was built from."""
|
|
149
|
+
return self._binding
|
|
150
|
+
|
|
151
|
+
@property
|
|
152
|
+
def paths(self) -> tuple[str, ...]:
|
|
153
|
+
"""Return the lifecycle nodes this Driver observes, in declaration order."""
|
|
154
|
+
return self._binding.observed
|
|
155
|
+
|
|
156
|
+
async def open(self) -> None:
|
|
157
|
+
"""Subscribe to every node's ``transition_event``.
|
|
158
|
+
|
|
159
|
+
Subscribing is local, so a Component whose robot is off still finishes building.
|
|
160
|
+
The first question goes out later, when the match events say a node is there.
|
|
161
|
+
"""
|
|
162
|
+
for path in self.paths:
|
|
163
|
+
self._subscriptions[path] = await self._runtime.subscribe(
|
|
164
|
+
topic=f"{path}/transition_event",
|
|
165
|
+
message_type=self._event_type,
|
|
166
|
+
depth=self._binding.depth,
|
|
167
|
+
)
|
|
168
|
+
|
|
169
|
+
async def close(self) -> None:
|
|
170
|
+
"""Close every subscription this Driver opened. Idempotent."""
|
|
171
|
+
opened, self._subscriptions = self._subscriptions, {}
|
|
172
|
+
for subscription in opened.values():
|
|
173
|
+
await subscription.close()
|
|
174
|
+
|
|
175
|
+
async def transitions(self, path: str) -> AsyncIterator[NodeTransition]:
|
|
176
|
+
"""Yield every transition one node announces, until its subscription closes."""
|
|
177
|
+
async for message in self._subscriptions[path]:
|
|
178
|
+
yield NodeTransition(
|
|
179
|
+
path=path,
|
|
180
|
+
start=_state_id(message, "start_state"),
|
|
181
|
+
goal=_state_id(message, "goal_state"),
|
|
182
|
+
)
|
|
183
|
+
|
|
184
|
+
async def appearances(self, path: str) -> AsyncIterator[int]:
|
|
185
|
+
"""Yield how many publishers one node's ``transition_event`` has, now and on each change.
|
|
186
|
+
|
|
187
|
+
The current count arrives at once, so a Driver opened against a node that is already there
|
|
188
|
+
looks immediately.
|
|
189
|
+
"""
|
|
190
|
+
async for count in self._subscriptions[path].publisher_counts():
|
|
191
|
+
yield count
|
|
192
|
+
|
|
193
|
+
async def read_state(self, path: str) -> LifecycleNodeState | None:
|
|
194
|
+
"""Ask one node what state it is in. One call, never a poll.
|
|
195
|
+
|
|
196
|
+
:return: The state, or ``None`` when nobody answered (the node is off).
|
|
197
|
+
"""
|
|
198
|
+
answer = await self._call(f"{path}/get_state", self._get_state, self._get_state.request())
|
|
199
|
+
if answer is None:
|
|
200
|
+
return None
|
|
201
|
+
return _as_state(_state_id(answer, "current_state"))
|
|
202
|
+
|
|
203
|
+
async def offered_transitions(self, path: str) -> frozenset[LifecycleNodeTransition] | None:
|
|
204
|
+
"""Ask one node which transitions it will accept right now.
|
|
205
|
+
|
|
206
|
+
This is the gate. It is read from the robot, because a stale belief about the state would
|
|
207
|
+
kill an rclpy node. The name avoids "available", which this project does not use for a state of the robot.
|
|
208
|
+
|
|
209
|
+
:return: The transitions this project can command among those the node offers. The set is
|
|
210
|
+
empty for a ``finalized`` node, and ``None`` when nobody answered. The two differ: a node
|
|
211
|
+
that offers nothing is skipped, a node that is not there is a fault.
|
|
212
|
+
"""
|
|
213
|
+
answer = await self._call(
|
|
214
|
+
f"{path}/get_available_transitions", self._get_available, self._get_available.request()
|
|
215
|
+
)
|
|
216
|
+
if answer is None:
|
|
217
|
+
return None
|
|
218
|
+
# Reached dynamically: message dataclasses are built at run time and lifecycle_msgs has no stubs.
|
|
219
|
+
offered: Any = getattr(answer, "available_transitions", ()) or ()
|
|
220
|
+
known = {each.value for each in LifecycleNodeTransition}
|
|
221
|
+
return frozenset(
|
|
222
|
+
LifecycleNodeTransition(int(described.transition.id))
|
|
223
|
+
for described in offered
|
|
224
|
+
if int(described.transition.id) in known
|
|
225
|
+
)
|
|
226
|
+
|
|
227
|
+
async def apply(
|
|
228
|
+
self, trigger: TRIGGER, ledger: dict[str, set[LifecycleNodeTransition]] | None = None
|
|
229
|
+
) -> tuple[str, ...]:
|
|
230
|
+
"""Drive every node toward what one Skill trigger means, and return what went wrong.
|
|
231
|
+
|
|
232
|
+
The trigger's transitions run in order (:data:`TRANSITIONS_FOR`), each gated per node on
|
|
233
|
+
what that node offers at that moment. The gate is read before the first transition and
|
|
234
|
+
again between transitions, because deactivating is what makes cleaning up available.
|
|
235
|
+
|
|
236
|
+
A node that answered and does not offer the transition is skipped without a fault, which
|
|
237
|
+
makes the handlers idempotent. A node that did not answer is a fault, so that a robot that
|
|
238
|
+
is off cannot let ``Start`` reach ``Running``; it is asked once per trigger.
|
|
239
|
+
|
|
240
|
+
The ledger records what the Skill performed itself, and ``halt`` rolls back those
|
|
241
|
+
transitions and no others. A node that was already ``active`` before ``Start``
|
|
242
|
+
has an empty ledger and is not touched.
|
|
243
|
+
|
|
244
|
+
:param trigger: The Skill trigger to carry out.
|
|
245
|
+
:param ledger: What the Skill performed on each node, by path. Updated in place. ``None``
|
|
246
|
+
is an empty ledger.
|
|
247
|
+
:return: One fault per node or coordinator that did not answer or said no. Empty is
|
|
248
|
+
success, including having had nothing to do.
|
|
249
|
+
"""
|
|
250
|
+
if ledger is None:
|
|
251
|
+
ledger = {}
|
|
252
|
+
rolling_back = trigger == "halt"
|
|
253
|
+
stuck: set[str] = set()
|
|
254
|
+
faults: list[str] = []
|
|
255
|
+
asked = tuple(path for path in self.paths if ledger.get(path)) if rolling_back else self.paths
|
|
256
|
+
offered, unanswered = await self._gate(asked)
|
|
257
|
+
faults.extend(unanswered)
|
|
258
|
+
for index, transition in enumerate(TRANSITIONS_FOR[trigger]):
|
|
259
|
+
if index:
|
|
260
|
+
offered, unanswered = await self._gate(offered)
|
|
261
|
+
faults.extend(unanswered)
|
|
262
|
+
undone = UNDONE_BY.get(transition) if rolling_back else None
|
|
263
|
+
if rolling_back and undone is None: # pragma: no cover -- every halt transition undoes one
|
|
264
|
+
continue
|
|
265
|
+
ours = tuple(path for path in offered if undone is None or undone in ledger.get(path, ()))
|
|
266
|
+
wanted = tuple(path for path in ours if transition in offered[path])
|
|
267
|
+
refused = await self.send(transition, wanted) if wanted else ()
|
|
268
|
+
faults.extend(refused)
|
|
269
|
+
if undone is None:
|
|
270
|
+
for path in wanted:
|
|
271
|
+
ledger.setdefault(path, set()).add(transition)
|
|
272
|
+
continue
|
|
273
|
+
for path in ours:
|
|
274
|
+
# Kept once a node refused: the Skill's work on it still stands.
|
|
275
|
+
if refused and path in wanted:
|
|
276
|
+
stuck.add(path)
|
|
277
|
+
if path not in stuck:
|
|
278
|
+
ledger[path].discard(undone)
|
|
279
|
+
return tuple(faults)
|
|
280
|
+
|
|
281
|
+
async def _gate(self, paths: Iterable[str]) -> tuple[dict[str, frozenset[LifecycleNodeTransition]], list[str]]:
|
|
282
|
+
"""Ask each node which transitions it offers, keeping silence apart from none.
|
|
283
|
+
|
|
284
|
+
A node that offered nothing is carried with an empty set. A node that did not answer is a
|
|
285
|
+
fault and is dropped from the set the following transitions gate on.
|
|
286
|
+
|
|
287
|
+
:param paths: The nodes to ask, in the order their faults should be reported in.
|
|
288
|
+
:return: What each node that answered offers, and one fault per node that did not.
|
|
289
|
+
"""
|
|
290
|
+
offered: dict[str, frozenset[LifecycleNodeTransition]] = {}
|
|
291
|
+
faults: list[str] = []
|
|
292
|
+
for path in paths:
|
|
293
|
+
answered = await self.offered_transitions(path)
|
|
294
|
+
if answered is None:
|
|
295
|
+
faults.append(f"{path} did not answer when asked which transitions it offers")
|
|
296
|
+
else:
|
|
297
|
+
offered[path] = answered
|
|
298
|
+
return offered, faults
|
|
299
|
+
|
|
300
|
+
@abstractmethod
|
|
301
|
+
async def send(self, transition: LifecycleNodeTransition, paths: tuple[str, ...]) -> Sequence[str]:
|
|
302
|
+
"""Command one transition at the nodes that will accept it, the way this flavour does.
|
|
303
|
+
|
|
304
|
+
Called only for a transition at least one of ``paths`` reported as available, so an
|
|
305
|
+
implementation does not gate again.
|
|
306
|
+
|
|
307
|
+
:param transition: What to command.
|
|
308
|
+
:param paths: The nodes that offered it, a non-empty subset of :attr:`paths`.
|
|
309
|
+
:return: One human-readable fault per refusal or missing answer; empty on success. Faults
|
|
310
|
+
are returned so that an operator sees every cause, not the first.
|
|
311
|
+
"""
|
|
312
|
+
|
|
313
|
+
async def _call(self, service: str, types: ServiceTypes, request: Message) -> Message | None:
|
|
314
|
+
"""Call one ROS2 service, answering ``None`` when the call does not happen.
|
|
315
|
+
|
|
316
|
+
A node that is off is a runtime condition, not a fault. The commanding path
|
|
317
|
+
turns the missing answer into a message.
|
|
318
|
+
"""
|
|
319
|
+
try:
|
|
320
|
+
return await self._runtime.call_service(
|
|
321
|
+
service=service,
|
|
322
|
+
service_type=types,
|
|
323
|
+
request=request,
|
|
324
|
+
server_timeout=self._binding.server_timeout,
|
|
325
|
+
call_timeout=self._binding.call_timeout,
|
|
326
|
+
)
|
|
327
|
+
except (ServiceUnavailableError, ServiceCallTimeoutError):
|
|
328
|
+
return None
|
|
329
|
+
|
|
330
|
+
|
|
331
|
+
class StandardLifecycleDriver(LifecycleDriver):
|
|
332
|
+
"""The ROS2 standard: ``lifecycle_msgs``, one ``change_state`` call per node.
|
|
333
|
+
|
|
334
|
+
The default flavour. Its ``path`` is the node itself.
|
|
335
|
+
"""
|
|
336
|
+
|
|
337
|
+
flavour: ClassVar[str] = "lifecycle"
|
|
338
|
+
|
|
339
|
+
def __init__(self, *, binding: LifecycleBinding, runtime: RuntimeInterface) -> None:
|
|
340
|
+
"""Resolve the standard services, and the ``ChangeState`` request this flavour builds."""
|
|
341
|
+
super().__init__(binding=binding, runtime=runtime)
|
|
342
|
+
self._change: ServiceTypes = runtime.describe_service(CHANGE_STATE_TYPE)
|
|
343
|
+
self._request_type: Any = self._change.request
|
|
344
|
+
self._transition_type: Any = self._change.request.ros2_nested["transition"]
|
|
345
|
+
|
|
346
|
+
async def send(self, transition: LifecycleNodeTransition, paths: tuple[str, ...]) -> Sequence[str]:
|
|
347
|
+
"""Call ``<node>/change_state`` once per node, and report every node that said no."""
|
|
348
|
+
request = self._request_type(transition=self._transition_type(id=int(transition), label=""))
|
|
349
|
+
faults: list[str] = []
|
|
350
|
+
for path in paths:
|
|
351
|
+
answer: Any = await self._call(f"{path}/change_state", self._change, request)
|
|
352
|
+
if answer is None:
|
|
353
|
+
faults.append(f"{path} did not answer a {transition.name.lower()} request")
|
|
354
|
+
elif not answer.success:
|
|
355
|
+
faults.append(f"{path} refused to {transition.name.lower()}")
|
|
356
|
+
return faults
|
|
357
|
+
|
|
358
|
+
|
|
359
|
+
class Nav2ManagerDriver(LifecycleDriver):
|
|
360
|
+
"""A nav2-style manager: one ``manage_nodes`` call covering the whole set it drives.
|
|
361
|
+
|
|
362
|
+
The declaration's ``path`` is the manager and ``nodes`` are the lifecycle nodes it drives,
|
|
363
|
+
which are observed and gated like the standard flavour's. The manager commands its whole
|
|
364
|
+
set and this Driver cannot narrow that: if some nodes are ``active`` and some ``inactive``,
|
|
365
|
+
the manager answers ``success=False`` for the half that could not take the transition, and
|
|
366
|
+
that is reported. A halt also reaches nodes the Skill did not start. The gate still prevents a
|
|
367
|
+
call when no node in the set can take the transition.
|
|
368
|
+
"""
|
|
369
|
+
|
|
370
|
+
flavour: ClassVar[str] = "nav2"
|
|
371
|
+
coordinates_a_set: ClassVar[bool] = True
|
|
372
|
+
|
|
373
|
+
def __init__(self, *, binding: LifecycleBinding, runtime: RuntimeInterface) -> None:
|
|
374
|
+
"""Resolve the standard observation services, and nav2's own management service."""
|
|
375
|
+
super().__init__(binding=binding, runtime=runtime)
|
|
376
|
+
self._manage: ServiceTypes = runtime.describe_service(MANAGE_NODES_TYPE)
|
|
377
|
+
self._request_type: Any = self._manage.request
|
|
378
|
+
|
|
379
|
+
async def send(self, transition: LifecycleNodeTransition, paths: tuple[str, ...]) -> Sequence[str]:
|
|
380
|
+
"""Call ``<manager>/manage_nodes`` once, with the command that sends this transition."""
|
|
381
|
+
manager = self._binding.path
|
|
382
|
+
request = self._request_type(command=NAV2_COMMANDS[transition])
|
|
383
|
+
answer: Any = await self._call(f"{manager}/manage_nodes", self._manage, request)
|
|
384
|
+
listed = ", ".join(paths)
|
|
385
|
+
if answer is None:
|
|
386
|
+
return [f"the manager {manager} did not answer a {transition.name.lower()} request for {listed}"]
|
|
387
|
+
if not answer.success:
|
|
388
|
+
return [f"the manager {manager} refused to {transition.name.lower()} {listed}"]
|
|
389
|
+
return []
|
|
390
|
+
|
|
391
|
+
|
|
392
|
+
DRIVERS: Final[Mapping[str, type[LifecycleDriver]]] = {
|
|
393
|
+
StandardLifecycleDriver.flavour: StandardLifecycleDriver,
|
|
394
|
+
Nav2ManagerDriver.flavour: Nav2ManagerDriver,
|
|
395
|
+
}
|
|
396
|
+
"""Which coordinator flavour a declaration's ``driver=`` selects.
|
|
397
|
+
|
|
398
|
+
A fixed table, so that a Binding file chooses among these by name and cannot introduce a class.
|
|
399
|
+
A third flavour subclasses :class:`LifecycleDriver` and is passed as the class.
|
|
400
|
+
"""
|
|
401
|
+
|
|
402
|
+
|
|
403
|
+
def _state_id(message: object, field: str) -> int:
|
|
404
|
+
"""Return the ``id`` of one ``lifecycle_msgs/msg/State`` field, or ``UNKNOWN`` if it is missing.
|
|
405
|
+
|
|
406
|
+
``UNKNOWN`` reads as no opinion instead of a traceback on a later task.
|
|
407
|
+
"""
|
|
408
|
+
return int(getattr(getattr(message, field, None), "id", LifecycleNodeState.UNKNOWN))
|
|
409
|
+
|
|
410
|
+
|
|
411
|
+
def _as_state(identifier: object) -> LifecycleNodeState:
|
|
412
|
+
"""Return the lifecycle state one id names, or ``UNKNOWN`` for a number ROS2 has not defined."""
|
|
413
|
+
try:
|
|
414
|
+
return LifecycleNodeState(int(identifier)) # type: ignore[arg-type]
|
|
415
|
+
except (TypeError, ValueError):
|
|
416
|
+
return LifecycleNodeState.UNKNOWN
|
|
417
|
+
|
|
418
|
+
|
|
419
|
+
def _state_name(identifier: int) -> str:
|
|
420
|
+
"""Return a readable name for one lifecycle state id, for a representation or a log line."""
|
|
421
|
+
return _as_state(identifier).name.lower()
|
|
@@ -0,0 +1,159 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Errors for declarations the server half refuses to boot with.
|
|
6
|
+
|
|
7
|
+
A declaration error is fatal at ``init`` and a runtime condition never is. Each error here
|
|
8
|
+
can be known before a node exists, independent of the ROS2 graph. They form their own family, not
|
|
9
|
+
leaves of :class:`opensmi.ros2.runtime.errors.ROS2RuntimeError`, so an application catching the
|
|
10
|
+
Runtime's errors does not also catch a wrong declaration. The field-selection faults are the
|
|
11
|
+
exception: a glob over ROS2 field paths is resolved in the runtime half, so its error lives there.
|
|
12
|
+
"""
|
|
13
|
+
|
|
14
|
+
from __future__ import annotations
|
|
15
|
+
|
|
16
|
+
from typing import TYPE_CHECKING
|
|
17
|
+
|
|
18
|
+
if TYPE_CHECKING:
|
|
19
|
+
from collections.abc import Iterable
|
|
20
|
+
|
|
21
|
+
__all__ = [
|
|
22
|
+
"BindingNameError",
|
|
23
|
+
"DeclarationError",
|
|
24
|
+
"DuplicateBindingNameError",
|
|
25
|
+
"DuplicateProjectionError",
|
|
26
|
+
"DuplicateVariableNameError",
|
|
27
|
+
"FileDeclarationError",
|
|
28
|
+
"FrameDirectionError",
|
|
29
|
+
"ImplementationClassError",
|
|
30
|
+
"NotInvertibleError",
|
|
31
|
+
"ParameterDeclarationError",
|
|
32
|
+
"RequiredDeclarationError",
|
|
33
|
+
"SuccessFieldError",
|
|
34
|
+
"UnprojectableFieldError",
|
|
35
|
+
]
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
class DeclarationError(Exception):
|
|
39
|
+
"""A declaration this half refuses to boot with. The message names the case.
|
|
40
|
+
|
|
41
|
+
Also raised for a wiring fault (no Runtime injected, no event node), a contradicting keyword
|
|
42
|
+
(continuity, ``max_rate``, a reserved name, a Machine-wide setting on a Binding) and a duplicate
|
|
43
|
+
lifecycle node. Only the classes below are told apart.
|
|
44
|
+
"""
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
class FileDeclarationError(DeclarationError):
|
|
48
|
+
"""A machine file has one or more mistakes in it.
|
|
49
|
+
|
|
50
|
+
A file is checked whole before anything is built, so this is the one declaration error that
|
|
51
|
+
carries several problems. Each names its place as a path through the file, such as
|
|
52
|
+
``component[1].monitoring[2].message_type``.
|
|
53
|
+
|
|
54
|
+
:ivar problems: ``(path, message)`` for each mistake, in file order. A mistake about the file as a
|
|
55
|
+
whole, such as TOML that does not parse, has the empty path.
|
|
56
|
+
"""
|
|
57
|
+
|
|
58
|
+
def __init__(self, problems: Iterable[tuple[str, str]], *, source: str | None = None) -> None:
|
|
59
|
+
"""Keep the problems, and print each of them under a heading that counts them."""
|
|
60
|
+
self.problems = tuple(problems)
|
|
61
|
+
self.source = source
|
|
62
|
+
count = len(self.problems)
|
|
63
|
+
head = f"{source}: " if source else ""
|
|
64
|
+
head += f"{count} problem{'s' if count != 1 else ''} in the machine file"
|
|
65
|
+
lines = [f" {path}: {message}" if path else f" {message}" for path, message in self.problems]
|
|
66
|
+
super().__init__("\n".join([head, *lines]))
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
class DuplicateBindingNameError(DeclarationError):
|
|
70
|
+
"""Two Bindings on one Component carry the same name.
|
|
71
|
+
|
|
72
|
+
Raised within one source and between the class attribute and the TOML source.
|
|
73
|
+
"""
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
class DuplicateVariableNameError(DeclarationError):
|
|
77
|
+
"""Two variables on one container would take the same name.
|
|
78
|
+
|
|
79
|
+
An opensmi container files a variable with ``setattr`` without looking anything up, so the second
|
|
80
|
+
would replace the first. This check names the Binding and field path that collided.
|
|
81
|
+
"""
|
|
82
|
+
|
|
83
|
+
|
|
84
|
+
class UnprojectableFieldError(DeclarationError):
|
|
85
|
+
"""A field has no rendering under the Projection that covers its type.
|
|
86
|
+
|
|
87
|
+
A repeated message is not one of these: it renders as one array Variable whose element is a
|
|
88
|
+
structure derived from the message type. This error is for what still has no answer:
|
|
89
|
+
an element type whose Projection renders a browsable Object, a field of an element whose type no
|
|
90
|
+
structure field can carry, and a plan that asks for an Object anywhere, since a message nests as
|
|
91
|
+
complex Variables and not as Objects.
|
|
92
|
+
"""
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
class DuplicateProjectionError(DeclarationError):
|
|
96
|
+
"""Two Projections in one registry cover the same ROS2 type and neither says which wins.
|
|
97
|
+
|
|
98
|
+
Shadowing is allowed (a Component's entry beats the Machine's), but it must be explicit: an entry
|
|
99
|
+
that replaces another says ``override=True``.
|
|
100
|
+
"""
|
|
101
|
+
|
|
102
|
+
|
|
103
|
+
class NotInvertibleError(DeclarationError):
|
|
104
|
+
"""A Projection that cannot run backwards was asked to.
|
|
105
|
+
|
|
106
|
+
The inverse is derived from the recipe: the drop recipe never inverts, a blob inverts
|
|
107
|
+
only if it is byte-identity, and any other inverse is supplied explicitly. Nothing is filled in
|
|
108
|
+
from defaults, since an invented field would be a value nobody sent.
|
|
109
|
+
"""
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
class ImplementationClassError(DeclarationError):
|
|
113
|
+
"""A declaration names a custom implementation class that is not a subclass of the stock class.
|
|
114
|
+
|
|
115
|
+
The check lives on the declaration because a TOML file can only produce a string, and refusing it
|
|
116
|
+
there keeps an editable configuration file from choosing a Python class.
|
|
117
|
+
"""
|
|
118
|
+
|
|
119
|
+
|
|
120
|
+
class SuccessFieldError(DeclarationError):
|
|
121
|
+
"""A Method declaration names a response field that cannot carry a service's verdict.
|
|
122
|
+
|
|
123
|
+
The response has no such field, or the field is not a ``bool``. Nothing looks for a field called
|
|
124
|
+
``success`` by itself, so a typo in ``success_field`` would otherwise give a Method that never
|
|
125
|
+
reports a refusal.
|
|
126
|
+
"""
|
|
127
|
+
|
|
128
|
+
|
|
129
|
+
class ParameterDeclarationError(DeclarationError):
|
|
130
|
+
"""A ROS2-parameter declaration contradicts itself.
|
|
131
|
+
|
|
132
|
+
Cases: a declaration that exposes no parameter, one that marks a name read-only without declaring
|
|
133
|
+
it, and a retry interval that is not a positive number of seconds. Names and types are declared,
|
|
134
|
+
never discovered, so there is nothing on the robot to correct them against.
|
|
135
|
+
"""
|
|
136
|
+
|
|
137
|
+
|
|
138
|
+
class BindingNameError(DeclarationError):
|
|
139
|
+
"""A name cannot be a browse name: it is empty, contains a ``/``, or does not follow the naming rule."""
|
|
140
|
+
|
|
141
|
+
|
|
142
|
+
class RequiredDeclarationError(DeclarationError):
|
|
143
|
+
"""A declaration's ``required`` mark and its startup timeout contradict each other.
|
|
144
|
+
|
|
145
|
+
Either a ``startup_timeout`` on a Binding nothing waits for, or one that is not a positive number
|
|
146
|
+
of seconds. Every Binding is optional unless marked required. A publishing Binding can
|
|
147
|
+
never be required and carries neither field.
|
|
148
|
+
"""
|
|
149
|
+
|
|
150
|
+
|
|
151
|
+
class FrameDirectionError(DeclarationError):
|
|
152
|
+
"""One TF frame is declared both as read inward and as published outward.
|
|
153
|
+
|
|
154
|
+
An outward frame's Location is where its pose is decided, an inward frame's Location is written
|
|
155
|
+
from ``/tf``. A frame that were both would publish the client's pose, read it back from our own
|
|
156
|
+
subscription and overwrite the client's value on the next tick. Refusing the declaration rules the
|
|
157
|
+
loop out. Echo suppression is the answer for values a client and a robot genuinely share, such as
|
|
158
|
+
ROS2 parameters; one frame has one publisher.
|
|
159
|
+
"""
|