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,141 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""What the Runtime raises when the thing asked for did not happen.
|
|
6
|
+
|
|
7
|
+
An error is raised when something did not happen; an outcome is returned when it happened and went
|
|
8
|
+
badly (a stop whose executor thread would not join is a ``StopOutcome``). All errors derive from
|
|
9
|
+
:class:`ROS2RuntimeError`, which carries the ROS2 prefix so it does not collide with ``RuntimeError``.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
from __future__ import annotations
|
|
13
|
+
|
|
14
|
+
__all__ = [
|
|
15
|
+
"ActionServerUnavailableError",
|
|
16
|
+
"FieldSelectionError",
|
|
17
|
+
"FieldTypeMismatchError",
|
|
18
|
+
"FieldValueError",
|
|
19
|
+
"FieldValueOutOfRangeError",
|
|
20
|
+
"GoalClosedError",
|
|
21
|
+
"InvalidMessageTypeNameError",
|
|
22
|
+
"LoopAffinityError",
|
|
23
|
+
"MessageTypeError",
|
|
24
|
+
"MissingInterfacePackageError",
|
|
25
|
+
"PublishFailedError",
|
|
26
|
+
"ROS2RuntimeError",
|
|
27
|
+
"RuntimeAlreadyStartedError",
|
|
28
|
+
"RuntimeNotStartedError",
|
|
29
|
+
"ServiceCallTimeoutError",
|
|
30
|
+
"ServiceUnavailableError",
|
|
31
|
+
"SubscriptionClosedError",
|
|
32
|
+
"UnusableMessageTypeError",
|
|
33
|
+
]
|
|
34
|
+
|
|
35
|
+
|
|
36
|
+
class ROS2RuntimeError(Exception):
|
|
37
|
+
"""Base of every error the Runtime raises: something it was asked to do did not happen."""
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
class RuntimeNotStartedError(ROS2RuntimeError):
|
|
41
|
+
"""An operation needing the ROS2 node ran before :meth:`Runtime.start`, or after stop."""
|
|
42
|
+
|
|
43
|
+
|
|
44
|
+
class RuntimeAlreadyStartedError(ROS2RuntimeError):
|
|
45
|
+
""":meth:`Runtime.start` ran twice, which would leak the first Context, Node and executor thread."""
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
class LoopAffinityError(ROS2RuntimeError):
|
|
49
|
+
"""A Runtime operation was awaited on an event loop other than the one that started it."""
|
|
50
|
+
|
|
51
|
+
|
|
52
|
+
class SubscriptionClosedError(ROS2RuntimeError):
|
|
53
|
+
"""A message, delta or update was awaited from a Subscription or a watch whose lifetime has ended."""
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
class MessageTypeError(ROS2RuntimeError):
|
|
57
|
+
"""A declared ROS2 message type cannot be resolved, or a field has a form this Runtime does not model.
|
|
58
|
+
|
|
59
|
+
All are fatal at init: the adapter never guesses a field shape, because a wrong one reaches an
|
|
60
|
+
OPC UA client as a plausible, wrong value.
|
|
61
|
+
"""
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
class InvalidMessageTypeNameError(MessageTypeError):
|
|
65
|
+
"""A declared type name is not ``package/Name`` or ``package/kind/Name``."""
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
class MissingInterfacePackageError(MessageTypeError):
|
|
69
|
+
"""A declared type's interface package is not installed.
|
|
70
|
+
|
|
71
|
+
``rclpy`` cannot subscribe without the typesupport, so there is no degraded mode.
|
|
72
|
+
"""
|
|
73
|
+
|
|
74
|
+
|
|
75
|
+
class UnusableMessageTypeError(MessageTypeError):
|
|
76
|
+
"""The interface package is installed but the named type is absent or is not a message."""
|
|
77
|
+
|
|
78
|
+
|
|
79
|
+
class ActionServerUnavailableError(ROS2RuntimeError):
|
|
80
|
+
"""No action server answered on a path a goal was sent to.
|
|
81
|
+
|
|
82
|
+
A goal that was rejected, aborted or cancelled comes back as a
|
|
83
|
+
:class:`~opensmi.ros2.runtime.outcomes.GoalOutcome`. A goal nobody received raises this.
|
|
84
|
+
"""
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
class ServiceUnavailableError(ROS2RuntimeError):
|
|
88
|
+
"""No service server answered on a path a request was sent to.
|
|
89
|
+
|
|
90
|
+
A service that answered comes back as the response message, whatever it answered.
|
|
91
|
+
"""
|
|
92
|
+
|
|
93
|
+
|
|
94
|
+
class ServiceCallTimeoutError(ROS2RuntimeError):
|
|
95
|
+
"""A service server took the request and never answered within the call timeout.
|
|
96
|
+
|
|
97
|
+
Distinct from :class:`ServiceUnavailableError` (nothing there against something stuck).
|
|
98
|
+
"""
|
|
99
|
+
|
|
100
|
+
|
|
101
|
+
class PublishFailedError(ROS2RuntimeError):
|
|
102
|
+
"""``rclpy`` refused to put a message on a topic.
|
|
103
|
+
|
|
104
|
+
It wraps the fault ``rclpy`` raised. A field of the wrong Python type is a
|
|
105
|
+
:class:`FieldValueError` instead and never reaches ``rclpy``.
|
|
106
|
+
"""
|
|
107
|
+
|
|
108
|
+
|
|
109
|
+
class GoalClosedError(ROS2RuntimeError):
|
|
110
|
+
"""Feedback or a result was awaited from a Goal whose lifetime has ended."""
|
|
111
|
+
|
|
112
|
+
|
|
113
|
+
class FieldValueError(ROS2RuntimeError):
|
|
114
|
+
"""Base of what the Runtime refuses to put into a ROS2 message.
|
|
115
|
+
|
|
116
|
+
``rclpy`` accepts a field of the wrong Python type, but the conversion to C aborts the whole
|
|
117
|
+
process on send (pinned by a Jazzy test). The guard therefore checks at declaration time, and
|
|
118
|
+
again on send for values a client writes into a Parameter.
|
|
119
|
+
"""
|
|
120
|
+
|
|
121
|
+
|
|
122
|
+
class FieldTypeMismatchError(FieldValueError):
|
|
123
|
+
"""A field value is not the Python type its ROS2 field is.
|
|
124
|
+
|
|
125
|
+
The check uses exact types: ``1`` is not a ``float64`` and ``True`` is not an ``int32``.
|
|
126
|
+
"""
|
|
127
|
+
|
|
128
|
+
|
|
129
|
+
class FieldValueOutOfRangeError(FieldValueError):
|
|
130
|
+
"""A field value does not fit the ROS2 integer or the bound its field declares.
|
|
131
|
+
|
|
132
|
+
``2**31`` into an ``int32`` does not abort but goes onto the wire wrong. Bounds on strings
|
|
133
|
+
and sequences are advertised by OPC UA and enforced nowhere else.
|
|
134
|
+
"""
|
|
135
|
+
|
|
136
|
+
|
|
137
|
+
class FieldSelectionError(ROS2RuntimeError):
|
|
138
|
+
"""A Binding's include and exclude patterns match no field, or leave the message with none.
|
|
139
|
+
|
|
140
|
+
Fatal at init, so a mistyped pattern cannot silently produce fewer variables than declared.
|
|
141
|
+
"""
|
|
@@ -0,0 +1,232 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The Goal handle: one ROS2 goal's lifetime and its signals, in one object.
|
|
6
|
+
|
|
7
|
+
Like :mod:`opensmi.ros2.runtime.subscription`, the handle is read-only for a Skill author and a
|
|
8
|
+
separate sink is the write side. A goal has three signals:
|
|
9
|
+
|
|
10
|
+
* feedback is a stream that ends with the goal. The buffer is bounded and drops the oldest message,
|
|
11
|
+
because the newest progress report is the one worth having;
|
|
12
|
+
* the result is a single value, awaited once and readable afterwards. It is a
|
|
13
|
+
:class:`~opensmi.ros2.runtime.outcomes.GoalOutcome` and not an exception, because a goal that
|
|
14
|
+
was rejected, aborted or cancelled did happen;
|
|
15
|
+
* cancel is a request. :meth:`Goal.cancel` returns when the server has answered; the actual ending
|
|
16
|
+
arrives on :meth:`Goal.result`.
|
|
17
|
+
|
|
18
|
+
A server that rejects the goal answers, so the handle comes back settled with
|
|
19
|
+
:attr:`Goal.accepted` ``False``. A path with no action server raises
|
|
20
|
+
:class:`~opensmi.ros2.runtime.errors.ActionServerUnavailableError`. Nothing here imports ``rclpy``.
|
|
21
|
+
"""
|
|
22
|
+
|
|
23
|
+
from __future__ import annotations
|
|
24
|
+
|
|
25
|
+
import asyncio
|
|
26
|
+
from collections import deque
|
|
27
|
+
from typing import TYPE_CHECKING, final
|
|
28
|
+
|
|
29
|
+
from opensmi.ros2.runtime.errors import GoalClosedError, LoopAffinityError
|
|
30
|
+
from opensmi.ros2.runtime.outcomes import GoalOutcome, GoalStatus
|
|
31
|
+
|
|
32
|
+
if TYPE_CHECKING:
|
|
33
|
+
from collections.abc import AsyncIterator, Awaitable, Callable
|
|
34
|
+
|
|
35
|
+
from opensmi.ros2.runtime.messages import ActionTypes, Message
|
|
36
|
+
|
|
37
|
+
__all__ = ["Goal", "GoalSink", "rejected_outcome", "settled_outcome"]
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
def settled_outcome(status: GoalStatus, result: Message | None) -> GoalOutcome:
|
|
41
|
+
"""Return the outcome of an accepted goal that has finished; every ending but success has one fault."""
|
|
42
|
+
faults = () if status is GoalStatus.SUCCEEDED else (f"the goal ended {status.name.lower()}",)
|
|
43
|
+
return GoalOutcome(faults=faults, accepted=True, status=status, result=result)
|
|
44
|
+
|
|
45
|
+
|
|
46
|
+
def rejected_outcome(action: str) -> GoalOutcome:
|
|
47
|
+
"""Return the outcome of a goal the server refused, which therefore never ran."""
|
|
48
|
+
return GoalOutcome(
|
|
49
|
+
faults=(f"the action server on {action} rejected the goal",),
|
|
50
|
+
accepted=False,
|
|
51
|
+
status=GoalStatus.UNKNOWN,
|
|
52
|
+
result=None,
|
|
53
|
+
)
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
@final
|
|
57
|
+
class GoalSink:
|
|
58
|
+
"""The write side of a :class:`Goal`; every method runs on the Runtime's event loop."""
|
|
59
|
+
|
|
60
|
+
def __init__(self, *, depth: int) -> None:
|
|
61
|
+
"""Open a sink buffering at most ``depth`` feedback messages, affine to the running loop.
|
|
62
|
+
|
|
63
|
+
:param depth: How many feedback messages may wait for a consumer. When the buffer is
|
|
64
|
+
full the oldest is dropped.
|
|
65
|
+
:raises ValueError: When the depth is less than one.
|
|
66
|
+
:raises RuntimeError: When constructed with no running event loop.
|
|
67
|
+
"""
|
|
68
|
+
if depth < 1:
|
|
69
|
+
message = f"a feedback depth must be at least 1, not {depth}"
|
|
70
|
+
raise ValueError(message)
|
|
71
|
+
# An `asyncio.Event` binds to the first loop that awaits it; a second loop would hang silently.
|
|
72
|
+
self._loop = asyncio.get_running_loop()
|
|
73
|
+
self._feedback: deque[Message] = deque(maxlen=depth)
|
|
74
|
+
self._arrival = asyncio.Event()
|
|
75
|
+
self._outcome: GoalOutcome | None = None
|
|
76
|
+
self._settled = asyncio.Event()
|
|
77
|
+
|
|
78
|
+
@property
|
|
79
|
+
def settled(self) -> bool:
|
|
80
|
+
"""Return whether the goal has ended, however it ended."""
|
|
81
|
+
return self._outcome is not None
|
|
82
|
+
|
|
83
|
+
@property
|
|
84
|
+
def outcome(self) -> GoalOutcome | None:
|
|
85
|
+
"""Return how the goal ended, or ``None`` while it is still running."""
|
|
86
|
+
return self._outcome
|
|
87
|
+
|
|
88
|
+
def deliver(self, feedback: Message) -> None:
|
|
89
|
+
"""Accept one decoded feedback message, dropping the oldest if the buffer is full."""
|
|
90
|
+
self._feedback.append(feedback)
|
|
91
|
+
self._arrival.set()
|
|
92
|
+
|
|
93
|
+
def settle(self, outcome: GoalOutcome) -> None:
|
|
94
|
+
"""End the goal, waking everything waiting on it. The first settlement wins."""
|
|
95
|
+
self.require_own_loop()
|
|
96
|
+
if self._outcome is not None:
|
|
97
|
+
return
|
|
98
|
+
self._outcome = outcome
|
|
99
|
+
self._settled.set()
|
|
100
|
+
self._arrival.set()
|
|
101
|
+
|
|
102
|
+
async def take_feedback(self) -> Message:
|
|
103
|
+
"""Await the next buffered feedback message.
|
|
104
|
+
|
|
105
|
+
:raises GoalClosedError: When the goal has ended and no feedback is left buffered.
|
|
106
|
+
:raises LoopAffinityError: When awaited on a loop other than this sink's.
|
|
107
|
+
"""
|
|
108
|
+
self.require_own_loop()
|
|
109
|
+
while not self._feedback:
|
|
110
|
+
if self._outcome is not None:
|
|
111
|
+
message = "this goal has ended and has no feedback left"
|
|
112
|
+
raise GoalClosedError(message)
|
|
113
|
+
# Nothing awaits between the clear and the wait, so no message can arrive in between.
|
|
114
|
+
self._arrival.clear()
|
|
115
|
+
await self._arrival.wait()
|
|
116
|
+
return self._feedback.popleft()
|
|
117
|
+
|
|
118
|
+
async def await_outcome(self) -> GoalOutcome:
|
|
119
|
+
"""Await how the goal ended.
|
|
120
|
+
|
|
121
|
+
:raises LoopAffinityError: When awaited on a loop other than this sink's.
|
|
122
|
+
"""
|
|
123
|
+
self.require_own_loop()
|
|
124
|
+
await self._settled.wait()
|
|
125
|
+
outcome = self._outcome
|
|
126
|
+
if outcome is None: # pragma: no cover -- the event is set only by `settle`
|
|
127
|
+
message = "this goal was woken without an outcome"
|
|
128
|
+
raise GoalClosedError(message)
|
|
129
|
+
return outcome
|
|
130
|
+
|
|
131
|
+
def require_own_loop(self) -> None:
|
|
132
|
+
"""Refuse an operation running on an event loop other than the one this sink opened on.
|
|
133
|
+
|
|
134
|
+
:raises LoopAffinityError: When the running loop is not this sink's.
|
|
135
|
+
"""
|
|
136
|
+
if asyncio.get_running_loop() is not self._loop:
|
|
137
|
+
message = "this goal is affine to the event loop it was sent on; this call is on another one"
|
|
138
|
+
raise LoopAffinityError(message)
|
|
139
|
+
|
|
140
|
+
|
|
141
|
+
@final
|
|
142
|
+
class Goal:
|
|
143
|
+
"""One goal sent to a ROS2 action server, from :meth:`Runtime.send_goal`."""
|
|
144
|
+
|
|
145
|
+
def __init__(
|
|
146
|
+
self,
|
|
147
|
+
*,
|
|
148
|
+
action: str,
|
|
149
|
+
types: ActionTypes,
|
|
150
|
+
sink: GoalSink,
|
|
151
|
+
request_cancel: Callable[[], Awaitable[None]],
|
|
152
|
+
release: Callable[[], Awaitable[None]],
|
|
153
|
+
) -> None:
|
|
154
|
+
"""Bind a handle to the sink and the two operations the Runtime prepared for it."""
|
|
155
|
+
self._action = action
|
|
156
|
+
self._types = types
|
|
157
|
+
self._sink = sink
|
|
158
|
+
self._request_cancel = request_cancel
|
|
159
|
+
self._release = release
|
|
160
|
+
self._released = False
|
|
161
|
+
|
|
162
|
+
def __repr__(self) -> str:
|
|
163
|
+
"""Return a representation naming the action and where the goal has got to."""
|
|
164
|
+
outcome = self._sink.outcome
|
|
165
|
+
state = "running" if outcome is None else ("accepted" if outcome.accepted else "rejected")
|
|
166
|
+
if outcome is not None and outcome.accepted:
|
|
167
|
+
state = outcome.status.name.lower()
|
|
168
|
+
return f"<Goal {self._action} {self._types.type_name} ({state})>"
|
|
169
|
+
|
|
170
|
+
@property
|
|
171
|
+
def accepted(self) -> bool:
|
|
172
|
+
"""Return whether the server took this goal (``False`` only if it refused)."""
|
|
173
|
+
outcome = self._sink.outcome
|
|
174
|
+
return outcome is None or outcome.accepted
|
|
175
|
+
|
|
176
|
+
@property
|
|
177
|
+
def outcome(self) -> GoalOutcome | None:
|
|
178
|
+
"""Return how this goal ended, or ``None`` while it is running (never waits)."""
|
|
179
|
+
return self._sink.outcome
|
|
180
|
+
|
|
181
|
+
async def receive_feedback(self) -> Message:
|
|
182
|
+
"""Await the next feedback message.
|
|
183
|
+
|
|
184
|
+
:raises GoalClosedError: When the goal has ended and nothing is buffered.
|
|
185
|
+
"""
|
|
186
|
+
return await self._sink.take_feedback()
|
|
187
|
+
|
|
188
|
+
def __aiter__(self) -> AsyncIterator[Message]:
|
|
189
|
+
"""Return an iterator yielding feedback until the goal ends."""
|
|
190
|
+
return self._stream()
|
|
191
|
+
|
|
192
|
+
async def _stream(self) -> AsyncIterator[Message]:
|
|
193
|
+
"""Yield feedback until the goal ends, then stop cleanly."""
|
|
194
|
+
while True:
|
|
195
|
+
try:
|
|
196
|
+
yield await self._sink.take_feedback()
|
|
197
|
+
except GoalClosedError:
|
|
198
|
+
return
|
|
199
|
+
|
|
200
|
+
async def result(self) -> GoalOutcome:
|
|
201
|
+
"""Await how this goal ended; every ending (rejected, aborted, cancelled, succeeded) is returned."""
|
|
202
|
+
return await self._sink.await_outcome()
|
|
203
|
+
|
|
204
|
+
async def cancel(self) -> None:
|
|
205
|
+
"""Ask the server to cancel this goal.
|
|
206
|
+
|
|
207
|
+
Returns when the request has been answered, not when the goal has stopped; the ending
|
|
208
|
+
arrives on :meth:`result`. Does nothing for a goal that has already ended.
|
|
209
|
+
"""
|
|
210
|
+
if self._sink.settled:
|
|
211
|
+
return
|
|
212
|
+
await self._request_cancel()
|
|
213
|
+
|
|
214
|
+
async def close(self) -> None:
|
|
215
|
+
"""End this goal's handle, releasing its ROS2 resources. Idempotent.
|
|
216
|
+
|
|
217
|
+
It does not cancel the goal; a goal still running is settled as cancelled from this side,
|
|
218
|
+
so nothing waits on a result that will never arrive.
|
|
219
|
+
"""
|
|
220
|
+
if self._released:
|
|
221
|
+
return
|
|
222
|
+
self._released = True
|
|
223
|
+
self._sink.settle(
|
|
224
|
+
self._sink.outcome
|
|
225
|
+
or GoalOutcome(
|
|
226
|
+
faults=(f"the handle on {self._action} was closed while the goal was still running",),
|
|
227
|
+
accepted=True,
|
|
228
|
+
status=GoalStatus.CANCELED,
|
|
229
|
+
result=None,
|
|
230
|
+
)
|
|
231
|
+
)
|
|
232
|
+
await self._release()
|
|
@@ -0,0 +1,288 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The graph facet: what is on the ROS2 graph, learned by polling because ``rclpy`` has no change event.
|
|
6
|
+
|
|
7
|
+
The facet reads only while a watch is registered and stops when the last one closes. A Machine whose
|
|
8
|
+
Bindings all declare their types never polls: their publisher presence comes from DDS match events
|
|
9
|
+
(:attr:`~opensmi.ros2.runtime.subscription.Subscription.publisher_count`). The poll serves Bindings
|
|
10
|
+
with no declared type, which cannot create a subscription.
|
|
11
|
+
|
|
12
|
+
A watch receives a delta against what it last saw, so a consumer that skipped polls gets one delta
|
|
13
|
+
covering all of them. Nothing here imports ``rclpy``; the Runtime hands the facet its read.
|
|
14
|
+
"""
|
|
15
|
+
|
|
16
|
+
from __future__ import annotations
|
|
17
|
+
|
|
18
|
+
import asyncio
|
|
19
|
+
from dataclasses import dataclass, field
|
|
20
|
+
from typing import TYPE_CHECKING, Final, final
|
|
21
|
+
|
|
22
|
+
import structlog
|
|
23
|
+
|
|
24
|
+
from opensmi.ros2.runtime.errors import SubscriptionClosedError
|
|
25
|
+
|
|
26
|
+
if TYPE_CHECKING:
|
|
27
|
+
from collections.abc import AsyncIterator, Awaitable, Callable, Mapping
|
|
28
|
+
|
|
29
|
+
__all__ = [
|
|
30
|
+
"EMPTY_GRAPH",
|
|
31
|
+
"POLL_INTERVAL",
|
|
32
|
+
"GraphDelta",
|
|
33
|
+
"GraphFacet",
|
|
34
|
+
"GraphSnapshot",
|
|
35
|
+
"GraphWatch",
|
|
36
|
+
"NameDelta",
|
|
37
|
+
"diff",
|
|
38
|
+
]
|
|
39
|
+
|
|
40
|
+
POLL_INTERVAL: Final = 5.0
|
|
41
|
+
"""Seconds between graph reads while anything is watching (each read walks every endpoint)."""
|
|
42
|
+
|
|
43
|
+
_LOGGER = structlog.get_logger("opensmi.ros2.runtime.graph")
|
|
44
|
+
|
|
45
|
+
_NAMES = ("topics", "services", "actions")
|
|
46
|
+
"""The three graph vocabularies, which diff identically and are listed once here."""
|
|
47
|
+
|
|
48
|
+
|
|
49
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
50
|
+
class GraphSnapshot:
|
|
51
|
+
"""Everything on the ROS2 graph at one moment, by name and by the types on that name.
|
|
52
|
+
|
|
53
|
+
One ROS2 name may carry more than one type (publishers can disagree about a topic), so each
|
|
54
|
+
value is a tuple.
|
|
55
|
+
|
|
56
|
+
:ivar topics: Every topic name, mapped to the types advertised on it.
|
|
57
|
+
:ivar services: Every service name, mapped to its types.
|
|
58
|
+
:ivar actions: Every action name, mapped to its types.
|
|
59
|
+
"""
|
|
60
|
+
|
|
61
|
+
topics: Mapping[str, tuple[str, ...]] = field(default_factory=dict)
|
|
62
|
+
services: Mapping[str, tuple[str, ...]] = field(default_factory=dict)
|
|
63
|
+
actions: Mapping[str, tuple[str, ...]] = field(default_factory=dict)
|
|
64
|
+
|
|
65
|
+
|
|
66
|
+
EMPTY_GRAPH: Final = GraphSnapshot()
|
|
67
|
+
"""The graph before anything has been read: what a new watch diffs its first delta against."""
|
|
68
|
+
|
|
69
|
+
|
|
70
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
71
|
+
class NameDelta:
|
|
72
|
+
"""What changed in one of the graph's three vocabularies.
|
|
73
|
+
|
|
74
|
+
:ivar appeared: Names that were not there before, with the types they carry.
|
|
75
|
+
:ivar vanished: Names that are no longer there, in sorted order.
|
|
76
|
+
:ivar retyped: Names that stayed and whose types changed, with the types they carry now.
|
|
77
|
+
Separate from ``appeared`` because a Binding that already took its nodes from a type
|
|
78
|
+
cannot take them again.
|
|
79
|
+
"""
|
|
80
|
+
|
|
81
|
+
appeared: Mapping[str, tuple[str, ...]] = field(default_factory=dict)
|
|
82
|
+
vanished: tuple[str, ...] = ()
|
|
83
|
+
retyped: Mapping[str, tuple[str, ...]] = field(default_factory=dict)
|
|
84
|
+
|
|
85
|
+
@property
|
|
86
|
+
def empty(self) -> bool:
|
|
87
|
+
"""Return whether nothing changed in this vocabulary."""
|
|
88
|
+
return not (self.appeared or self.vanished or self.retyped)
|
|
89
|
+
|
|
90
|
+
def types_now_on(self, name: str) -> tuple[str, ...] | None:
|
|
91
|
+
"""Return the types this delta leaves on one name, or ``None`` if it carries no news of it.
|
|
92
|
+
|
|
93
|
+
Appearing and being retyped count the same here, for a consumer still waiting to learn a
|
|
94
|
+
type. A name that vanished answers ``None``.
|
|
95
|
+
"""
|
|
96
|
+
if name in self.appeared:
|
|
97
|
+
return self.appeared[name]
|
|
98
|
+
return self.retyped.get(name)
|
|
99
|
+
|
|
100
|
+
|
|
101
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
102
|
+
class GraphDelta:
|
|
103
|
+
"""What changed on the graph between two snapshots, one entry per vocabulary."""
|
|
104
|
+
|
|
105
|
+
topics: NameDelta = NameDelta()
|
|
106
|
+
services: NameDelta = NameDelta()
|
|
107
|
+
actions: NameDelta = NameDelta()
|
|
108
|
+
|
|
109
|
+
@property
|
|
110
|
+
def empty(self) -> bool:
|
|
111
|
+
"""Return whether the graph did not move at all."""
|
|
112
|
+
return all(getattr(self, name).empty for name in _NAMES)
|
|
113
|
+
|
|
114
|
+
|
|
115
|
+
def diff(previous: GraphSnapshot, current: GraphSnapshot) -> GraphDelta:
|
|
116
|
+
"""Return what changed between two snapshots (pure, so a watch can compute its delta late)."""
|
|
117
|
+
return GraphDelta(**{name: _name_delta(getattr(previous, name), getattr(current, name)) for name in _NAMES})
|
|
118
|
+
|
|
119
|
+
|
|
120
|
+
@final
|
|
121
|
+
class GraphWatch:
|
|
122
|
+
"""One consumer's view of the graph, from :meth:`GraphFacet.watch`. Close it so the poll can stop."""
|
|
123
|
+
|
|
124
|
+
def __init__(self, *, facet: GraphFacet, release: Callable[[GraphWatch], None]) -> None:
|
|
125
|
+
"""Bind a watch to the facet it reads and the deregistration the facet prepared."""
|
|
126
|
+
self._facet = facet
|
|
127
|
+
self._release = release
|
|
128
|
+
self._seen = EMPTY_GRAPH
|
|
129
|
+
self._arrival = asyncio.Event()
|
|
130
|
+
self._closed = False
|
|
131
|
+
self._arrival.set()
|
|
132
|
+
|
|
133
|
+
def __repr__(self) -> str:
|
|
134
|
+
"""Return a representation naming the watch's state."""
|
|
135
|
+
return f"<GraphWatch ({'closed' if self._closed else 'open'})>"
|
|
136
|
+
|
|
137
|
+
@property
|
|
138
|
+
def closed(self) -> bool:
|
|
139
|
+
"""Return whether this watch's lifetime has ended."""
|
|
140
|
+
return self._closed
|
|
141
|
+
|
|
142
|
+
def wake(self) -> None:
|
|
143
|
+
"""Tell this watch the facet has read the graph again."""
|
|
144
|
+
self._arrival.set()
|
|
145
|
+
|
|
146
|
+
async def receive(self) -> GraphDelta:
|
|
147
|
+
"""Await the next change to the graph, as a delta against what this watch last received.
|
|
148
|
+
|
|
149
|
+
:raises SubscriptionClosedError: When the watch has been closed.
|
|
150
|
+
"""
|
|
151
|
+
while True:
|
|
152
|
+
if self._closed:
|
|
153
|
+
message = "this graph watch is closed; the graph is no longer being read for it"
|
|
154
|
+
raise SubscriptionClosedError(message)
|
|
155
|
+
delta = diff(self._seen, self._facet.snapshot)
|
|
156
|
+
self._seen = self._facet.snapshot
|
|
157
|
+
if not delta.empty:
|
|
158
|
+
return delta
|
|
159
|
+
# Nothing awaits between the clear and the wait, so no poll can land in between.
|
|
160
|
+
self._arrival.clear()
|
|
161
|
+
await self._arrival.wait()
|
|
162
|
+
|
|
163
|
+
def __aiter__(self) -> AsyncIterator[GraphDelta]:
|
|
164
|
+
"""Return an iterator yielding deltas until the watch is closed."""
|
|
165
|
+
return self._stream()
|
|
166
|
+
|
|
167
|
+
async def _stream(self) -> AsyncIterator[GraphDelta]:
|
|
168
|
+
"""Yield deltas until the watch is closed, then stop cleanly."""
|
|
169
|
+
while True:
|
|
170
|
+
try:
|
|
171
|
+
yield await self.receive()
|
|
172
|
+
except SubscriptionClosedError:
|
|
173
|
+
return
|
|
174
|
+
|
|
175
|
+
async def close(self) -> None:
|
|
176
|
+
"""End this watch, and with it the poll if it was the last one. Idempotent."""
|
|
177
|
+
if self._closed:
|
|
178
|
+
return
|
|
179
|
+
self._closed = True
|
|
180
|
+
self._arrival.set()
|
|
181
|
+
self._release(self)
|
|
182
|
+
await self._facet.settle()
|
|
183
|
+
|
|
184
|
+
|
|
185
|
+
@final
|
|
186
|
+
class GraphFacet:
|
|
187
|
+
"""What is on the ROS2 graph, read on a timer while anything is watching (see ``Runtime.graph``)."""
|
|
188
|
+
|
|
189
|
+
def __init__(
|
|
190
|
+
self,
|
|
191
|
+
*,
|
|
192
|
+
read: Callable[[], Awaitable[GraphSnapshot]],
|
|
193
|
+
interval: float = POLL_INTERVAL,
|
|
194
|
+
) -> None:
|
|
195
|
+
"""Describe a facet without reading anything.
|
|
196
|
+
|
|
197
|
+
:param read: How to read the graph once.
|
|
198
|
+
:param interval: Seconds between reads while anything is watching.
|
|
199
|
+
"""
|
|
200
|
+
self._read = read
|
|
201
|
+
self._interval = interval
|
|
202
|
+
self._snapshot = EMPTY_GRAPH
|
|
203
|
+
self._watches: list[GraphWatch] = []
|
|
204
|
+
self._task: asyncio.Task[None] | None = None
|
|
205
|
+
self._polls = 0
|
|
206
|
+
|
|
207
|
+
def __repr__(self) -> str:
|
|
208
|
+
"""Return a representation naming the poll state and how often it has read."""
|
|
209
|
+
state = "polling" if self.polling else "resting"
|
|
210
|
+
return f"<GraphFacet {state} after {self._polls} polls>"
|
|
211
|
+
|
|
212
|
+
@property
|
|
213
|
+
def interval(self) -> float:
|
|
214
|
+
"""Return the seconds between reads while anything is watching."""
|
|
215
|
+
return self._interval
|
|
216
|
+
|
|
217
|
+
@property
|
|
218
|
+
def polls(self) -> int:
|
|
219
|
+
"""Return how often the graph has been read (0 for a fully declared Machine)."""
|
|
220
|
+
return self._polls
|
|
221
|
+
|
|
222
|
+
@property
|
|
223
|
+
def polling(self) -> bool:
|
|
224
|
+
"""Return whether the poll is running, which is exactly while a watch is open."""
|
|
225
|
+
return self._task is not None
|
|
226
|
+
|
|
227
|
+
@property
|
|
228
|
+
def snapshot(self) -> GraphSnapshot:
|
|
229
|
+
"""Return the graph as the last read left it (empty before the first one)."""
|
|
230
|
+
return self._snapshot
|
|
231
|
+
|
|
232
|
+
def watch(self) -> GraphWatch:
|
|
233
|
+
"""Register interest in the graph, starting the poll if it was not running.
|
|
234
|
+
|
|
235
|
+
:return: A handle carrying the deltas. The poll stops when the last watch closes.
|
|
236
|
+
"""
|
|
237
|
+
watch = GraphWatch(facet=self, release=self._watches.remove)
|
|
238
|
+
self._watches.append(watch)
|
|
239
|
+
self._start()
|
|
240
|
+
return watch
|
|
241
|
+
|
|
242
|
+
async def refresh(self) -> GraphSnapshot:
|
|
243
|
+
"""Read the graph once, now, and hold the result. Counts as a poll."""
|
|
244
|
+
self._polls += 1
|
|
245
|
+
self._snapshot = await self._read()
|
|
246
|
+
return self._snapshot
|
|
247
|
+
|
|
248
|
+
async def settle(self) -> None:
|
|
249
|
+
"""Stop the poll if nothing is watching any more. Called by a watch as it closes."""
|
|
250
|
+
if self._watches or self._task is None:
|
|
251
|
+
return
|
|
252
|
+
task, self._task = self._task, None
|
|
253
|
+
task.cancel()
|
|
254
|
+
await asyncio.gather(task, return_exceptions=True)
|
|
255
|
+
|
|
256
|
+
async def close(self) -> None:
|
|
257
|
+
"""End every watch and stop the poll, which is what stopping the Runtime does to it."""
|
|
258
|
+
for watch in tuple(self._watches):
|
|
259
|
+
await watch.close()
|
|
260
|
+
await self.settle()
|
|
261
|
+
|
|
262
|
+
def _start(self) -> None:
|
|
263
|
+
"""Start the poll loop, unless it is already running or nothing is watching."""
|
|
264
|
+
if self._task is not None or not self._watches:
|
|
265
|
+
return
|
|
266
|
+
self._task = asyncio.create_task(self._poll(), name="ros2-graph-poll")
|
|
267
|
+
|
|
268
|
+
async def _poll(self) -> None:
|
|
269
|
+
"""Read the graph on the interval and wake every watch, until cancelled."""
|
|
270
|
+
while True:
|
|
271
|
+
await asyncio.sleep(self._interval)
|
|
272
|
+
try:
|
|
273
|
+
await self.refresh()
|
|
274
|
+
except Exception:
|
|
275
|
+
# A background read has no caller to raise at, and the next read will probably work.
|
|
276
|
+
_LOGGER.exception("Could not read the ROS2 graph; the poll continues")
|
|
277
|
+
continue
|
|
278
|
+
for watch in tuple(self._watches):
|
|
279
|
+
watch.wake()
|
|
280
|
+
|
|
281
|
+
|
|
282
|
+
def _name_delta(previous: Mapping[str, tuple[str, ...]], current: Mapping[str, tuple[str, ...]]) -> NameDelta:
|
|
283
|
+
"""Return what changed in one vocabulary between two snapshots."""
|
|
284
|
+
return NameDelta(
|
|
285
|
+
appeared={name: types for name, types in current.items() if name not in previous},
|
|
286
|
+
vanished=tuple(sorted(name for name in previous if name not in current)),
|
|
287
|
+
retyped={name: types for name, types in current.items() if name in previous and previous[name] != types},
|
|
288
|
+
)
|