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,704 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""A ROS2 parameter is a Parameter: an operator tunes a value, and the node decides the write.
|
|
6
|
+
|
|
7
|
+
Each declared ROS2 parameter becomes one UA Parameter on the declaring Component's ParameterSet,
|
|
8
|
+
writable unless the declaration says otherwise. A client write goes out as ``set_parameters`` and a
|
|
9
|
+
change observed on ``/parameter_events`` comes back in, so the Parameter means the current value on
|
|
10
|
+
the node, whoever moved it.
|
|
11
|
+
|
|
12
|
+
A client's write is decided by ROS2 before it is stored. Every Parameter carries an async
|
|
13
|
+
write check, which opensmi awaits before storing and which answers the client with the status code it
|
|
14
|
+
raises. A value equal to what ROS2 holds is accepted at once (this is how the inward mirror passes).
|
|
15
|
+
The node's descriptor refuses without a round trip: ``BadNotWritable`` for a read-only parameter,
|
|
16
|
+
``BadTypeMismatch`` for another type, ``BadOutOfRange`` outside the declared range. Otherwise
|
|
17
|
+
``set_parameters`` goes to the node with a timeout below the client's own. A refusal is
|
|
18
|
+
``BadOutOfRange`` when the node's reason is a range and ``BadInvalidArgument`` otherwise, and no
|
|
19
|
+
answer in time is ``BadTimeout``. Nothing wrong is ever stored, so nothing is reverted, and writes to
|
|
20
|
+
one Parameter are serialised.
|
|
21
|
+
|
|
22
|
+
The node is also read and followed, and faults a client did not cause are BindingEvents. A node that
|
|
23
|
+
does not answer sets every Parameter of the Binding to ``BadNoCommunication`` and fires
|
|
24
|
+
:data:`PARAMETER_UNREACHABLE`. A declared name that is absent fires :data:`PARAMETER_ABSENT`, and one
|
|
25
|
+
held at another type fires :data:`PARAMETER_TYPE_MISMATCH`; both leave the Parameter
|
|
26
|
+
``BadNoCommunication``, since types are declared and never discovered.
|
|
27
|
+
|
|
28
|
+
Nothing waits at init. The Parameters exist from boot with typed zeros under ``BadNoCommunication``,
|
|
29
|
+
and the first read happens on a task. A name that is not confirmed is read again every
|
|
30
|
+
``retry_interval`` seconds, and nothing is read once every name is confirmed. This is not a liveness
|
|
31
|
+
probe: a node that vanishes silently is not noticed here, which is the StartupSkill's and a
|
|
32
|
+
lifecycle Binding's job.
|
|
33
|
+
"""
|
|
34
|
+
|
|
35
|
+
from __future__ import annotations
|
|
36
|
+
|
|
37
|
+
import asyncio
|
|
38
|
+
import math
|
|
39
|
+
from dataclasses import dataclass, field
|
|
40
|
+
from typing import TYPE_CHECKING, Any, Final, NoReturn
|
|
41
|
+
|
|
42
|
+
from asyncua import ua
|
|
43
|
+
from asyncua.ua import UaStatusCodeError
|
|
44
|
+
from asyncua.ua.status_codes import StatusCodes
|
|
45
|
+
from opensmi.server.mixins import ParameterSetMixin
|
|
46
|
+
|
|
47
|
+
from opensmi.ros2.runtime.errors import FieldTypeMismatchError, FieldValueOutOfRangeError, ROS2RuntimeError
|
|
48
|
+
from opensmi.ros2.runtime.parameters import ParameterType, ParameterValue, absolute
|
|
49
|
+
from opensmi.ros2.server.errors import DeclarationError
|
|
50
|
+
from opensmi.ros2.server.events import ROS2EventsMixin, Severity
|
|
51
|
+
from opensmi.ros2.server.mutation import add_plan_variable, ended, ensure_groups, require_unused_name
|
|
52
|
+
from opensmi.ros2.server.projection import (
|
|
53
|
+
NodeLevel,
|
|
54
|
+
PlanVariable,
|
|
55
|
+
join_path,
|
|
56
|
+
ros2_value,
|
|
57
|
+
ua_shape,
|
|
58
|
+
ua_value,
|
|
59
|
+
)
|
|
60
|
+
from opensmi.ros2.server.status import status_code, status_value
|
|
61
|
+
|
|
62
|
+
if TYPE_CHECKING:
|
|
63
|
+
from collections.abc import Awaitable, Callable, Iterable, Mapping
|
|
64
|
+
|
|
65
|
+
from opensmi.server import UaVariable
|
|
66
|
+
|
|
67
|
+
from opensmi.ros2.runtime.interface import RuntimeInterface
|
|
68
|
+
from opensmi.ros2.runtime.parameters import ParameterDescription, ParameterReading, ParameterWatch
|
|
69
|
+
from opensmi.ros2.server.bindings import ROS2ParameterBinding
|
|
70
|
+
|
|
71
|
+
__all__ = [
|
|
72
|
+
"PARAMETER_ABSENT",
|
|
73
|
+
"PARAMETER_TYPE_MISMATCH",
|
|
74
|
+
"PARAMETER_UNREACHABLE",
|
|
75
|
+
"ROS2ParameterMirrorMixin",
|
|
76
|
+
"refusal_code",
|
|
77
|
+
"ros2_parameter_value",
|
|
78
|
+
"ua_parameter_shape",
|
|
79
|
+
]
|
|
80
|
+
|
|
81
|
+
PARAMETER_UNREACHABLE: Final = "PARAMETER_UNREACHABLE"
|
|
82
|
+
"""The BindingEvent code for a parameter call that did not happen at all."""
|
|
83
|
+
|
|
84
|
+
PARAMETER_ABSENT: Final = "PARAMETER_ABSENT"
|
|
85
|
+
"""The BindingEvent code for a declared name the node does not have."""
|
|
86
|
+
|
|
87
|
+
PARAMETER_TYPE_MISMATCH: Final = "PARAMETER_TYPE_MISMATCH"
|
|
88
|
+
"""The BindingEvent code for a declared name the node holds at another type."""
|
|
89
|
+
|
|
90
|
+
_ELEMENTS: Final[Mapping[ParameterType, ParameterType]] = {
|
|
91
|
+
ParameterType.BOOL_ARRAY: ParameterType.BOOL,
|
|
92
|
+
ParameterType.INTEGER_ARRAY: ParameterType.INTEGER,
|
|
93
|
+
ParameterType.DOUBLE_ARRAY: ParameterType.DOUBLE,
|
|
94
|
+
ParameterType.STRING_ARRAY: ParameterType.STRING,
|
|
95
|
+
}
|
|
96
|
+
"""What one element of each array parameter type is, for the local type check."""
|
|
97
|
+
|
|
98
|
+
_RANGED: Final[frozenset[ParameterType]] = frozenset({ParameterType.INTEGER, ParameterType.DOUBLE})
|
|
99
|
+
"""The parameter types a node's declared range is checked against here.
|
|
100
|
+
|
|
101
|
+
An array is left to the node, which knows whether it applies its range per element.
|
|
102
|
+
"""
|
|
103
|
+
|
|
104
|
+
|
|
105
|
+
def ua_parameter_shape(parameter_type: ParameterType) -> tuple[ua.VariantType, object]:
|
|
106
|
+
"""Return the OPC UA type one declared ROS2 parameter type renders as, and its typed zero.
|
|
107
|
+
|
|
108
|
+
This is :func:`opensmi.ros2.server.projection.ua_shape` over the field type the parameter type
|
|
109
|
+
is, so a ROS2 ``integer`` renders like an ``int64`` message field. It is not a Projection:
|
|
110
|
+
the declaration already chose the union member. An array's zero is a fresh list on
|
|
111
|
+
every call.
|
|
112
|
+
"""
|
|
113
|
+
zero, variant_type = ua_shape(parameter_type.field_type)
|
|
114
|
+
return variant_type, zero
|
|
115
|
+
|
|
116
|
+
|
|
117
|
+
def ros2_parameter_value(value: object, parameter_type: ParameterType) -> ParameterValue:
|
|
118
|
+
"""Return what a client wrote into a Parameter as the ROS2 value it stands for.
|
|
119
|
+
|
|
120
|
+
This is :func:`opensmi.ros2.server.projection.ros2_value` over the declared field type, which
|
|
121
|
+
narrows OPC UA scalar types to the exact Python types the Runtime's guard tests. A value of a
|
|
122
|
+
kind the type cannot hold is left as it came, for the guard to refuse.
|
|
123
|
+
"""
|
|
124
|
+
return ParameterValue(type=parameter_type, value=ros2_value(value, parameter_type.field_type))
|
|
125
|
+
|
|
126
|
+
|
|
127
|
+
def refusal_code(reason: str) -> int:
|
|
128
|
+
"""Return the status code a node's refusal of a write is answered with.
|
|
129
|
+
|
|
130
|
+
``BadOutOfRange`` when the node's reason mentions a range, ``BadInvalidArgument`` otherwise. A
|
|
131
|
+
Write response carries only a status code.
|
|
132
|
+
"""
|
|
133
|
+
return StatusCodes.BadOutOfRange if "range" in reason.lower() else StatusCodes.BadInvalidArgument
|
|
134
|
+
|
|
135
|
+
|
|
136
|
+
@dataclass(slots=True)
|
|
137
|
+
class _Parameter:
|
|
138
|
+
"""One declared ROS2 parameter, live: its node, what the robot last said, and what that means.
|
|
139
|
+
|
|
140
|
+
``ros2_name`` is what the node calls it (dotted), ``name`` its node path joined with dots.
|
|
141
|
+
"""
|
|
142
|
+
|
|
143
|
+
ros2_name: str
|
|
144
|
+
name: str
|
|
145
|
+
parameter_type: ParameterType
|
|
146
|
+
variant_type: ua.VariantType
|
|
147
|
+
# The value last known from ROS2, which is also the value the variable holds.
|
|
148
|
+
value: object
|
|
149
|
+
writable: bool
|
|
150
|
+
variable: UaVariable[Any] | None = None
|
|
151
|
+
# What the node said about this name the last time it was confirmed, or nothing yet.
|
|
152
|
+
description: ParameterDescription | None = None
|
|
153
|
+
satisfied: bool = False
|
|
154
|
+
sampled: bool = False
|
|
155
|
+
# Conditions already reported, so a retry does not make an event storm. Cleared with the condition.
|
|
156
|
+
reported: set[str] = field(default_factory=set)
|
|
157
|
+
# Bumped by every event applied, so a read or write in flight can tell newer news landed.
|
|
158
|
+
epoch: int = 0
|
|
159
|
+
# Serialises everything that decides or writes this value: client writes and inward writes.
|
|
160
|
+
writing: asyncio.Lock = field(default_factory=asyncio.Lock)
|
|
161
|
+
# The task holding `writing` for an inward write, so its own check does not wait on the lock.
|
|
162
|
+
inward: asyncio.Task[Any] | None = None
|
|
163
|
+
|
|
164
|
+
@property
|
|
165
|
+
def status(self) -> ua.StatusCode:
|
|
166
|
+
"""Return the status code this Parameter currently reads."""
|
|
167
|
+
return status_code(satisfied=self.satisfied, sampled=self.sampled)
|
|
168
|
+
|
|
169
|
+
@property
|
|
170
|
+
def node(self) -> UaVariable[Any]:
|
|
171
|
+
"""Return the opensmi variable this Parameter is, which exists from init on."""
|
|
172
|
+
if self.variable is None: # pragma: no cover -- created before anything reads it
|
|
173
|
+
message = f"the Parameter {self.name!r} has no variable yet"
|
|
174
|
+
raise RuntimeError(message)
|
|
175
|
+
return self.variable
|
|
176
|
+
|
|
177
|
+
def holds(self, value: object) -> None:
|
|
178
|
+
"""Record that the node has this value, which is the only good news there is."""
|
|
179
|
+
self.satisfied = True
|
|
180
|
+
self.sampled = True
|
|
181
|
+
self.reported.clear()
|
|
182
|
+
self.value = value
|
|
183
|
+
|
|
184
|
+
|
|
185
|
+
@dataclass(slots=True)
|
|
186
|
+
class _Tuned:
|
|
187
|
+
"""One ROS2-parameter Binding, live: its watch, its Parameters and the tasks driving them."""
|
|
188
|
+
|
|
189
|
+
binding: ROS2ParameterBinding
|
|
190
|
+
watch: ParameterWatch
|
|
191
|
+
parameters: dict[str, _Parameter] = field(default_factory=dict)
|
|
192
|
+
unconfirmed: asyncio.Event = field(default_factory=asyncio.Event)
|
|
193
|
+
tasks: list[asyncio.Task[None]] = field(default_factory=list)
|
|
194
|
+
|
|
195
|
+
@property
|
|
196
|
+
def confirmed(self) -> bool:
|
|
197
|
+
"""Return whether every declared name is confirmed present, which is Satisfied here."""
|
|
198
|
+
return all(each.satisfied for each in self.parameters.values())
|
|
199
|
+
|
|
200
|
+
|
|
201
|
+
class ROS2ParameterMirrorMixin(ParameterSetMixin, ROS2EventsMixin):
|
|
202
|
+
"""Keeps a Component's ParameterSet in sync with the ROS2 nodes it declared.
|
|
203
|
+
|
|
204
|
+
Mixed in ahead of opensmi's own class. It extends the events mixin, because a node that stops
|
|
205
|
+
answering is a BindingEvent.
|
|
206
|
+
"""
|
|
207
|
+
|
|
208
|
+
def __init__(self, *args: Any, **kwargs: Any) -> None:
|
|
209
|
+
"""Start out tuning nothing, and pass every argument to opensmi's constructor."""
|
|
210
|
+
super().__init__(*args, **kwargs)
|
|
211
|
+
self._tuned: list[_Tuned] = []
|
|
212
|
+
self._parameter_runtime: RuntimeInterface | None = None
|
|
213
|
+
|
|
214
|
+
@property
|
|
215
|
+
def ros2_unconfirmed_parameters(self) -> Mapping[str, tuple[str, ...]]:
|
|
216
|
+
"""Return, per Binding name, the declared ROS2 parameter names not confirmed present.
|
|
217
|
+
|
|
218
|
+
Empty everywhere means every ROS2-parameter Binding is Satisfied. A name here is a typo in a
|
|
219
|
+
declaration or a robot that is not answering; the BindingEvent says which.
|
|
220
|
+
"""
|
|
221
|
+
return {
|
|
222
|
+
tuned.binding.name: tuple(name for name, parameter in tuned.parameters.items() if not parameter.satisfied)
|
|
223
|
+
for tuned in self._tuned
|
|
224
|
+
}
|
|
225
|
+
|
|
226
|
+
async def _init_parameter_mirror(
|
|
227
|
+
self,
|
|
228
|
+
bindings: Iterable[ROS2ParameterBinding],
|
|
229
|
+
*,
|
|
230
|
+
runtime: RuntimeInterface,
|
|
231
|
+
) -> None:
|
|
232
|
+
"""Create every declared Parameter, then start following the nodes they came from.
|
|
233
|
+
|
|
234
|
+
Fatal on anything wrong with a declaration, such as a name written twice; what was opened is
|
|
235
|
+
closed again first. No ROS2 call happens here, so the endpoint opens before anything waits
|
|
236
|
+
for a robot.
|
|
237
|
+
"""
|
|
238
|
+
self._parameter_runtime = runtime
|
|
239
|
+
try:
|
|
240
|
+
for binding in bindings:
|
|
241
|
+
self._tuned.append(await self._open_parameters(binding, runtime))
|
|
242
|
+
except BaseException:
|
|
243
|
+
await self._stop_parameter_mirror()
|
|
244
|
+
raise
|
|
245
|
+
for tuned in self._tuned:
|
|
246
|
+
await self._start_tuning(tuned)
|
|
247
|
+
|
|
248
|
+
async def _stop_parameter_mirror(self) -> None:
|
|
249
|
+
"""Stop following every ROS2-parameter Binding and close its watch. Idempotent.
|
|
250
|
+
|
|
251
|
+
It runs before the Runtime is stopped, for a Component torn down while the process goes on
|
|
252
|
+
serving.
|
|
253
|
+
"""
|
|
254
|
+
for tuned in self._tuned:
|
|
255
|
+
for task in tuned.tasks:
|
|
256
|
+
task.cancel()
|
|
257
|
+
for tuned in self._tuned:
|
|
258
|
+
for task in tuned.tasks:
|
|
259
|
+
await ended(task)
|
|
260
|
+
tuned.tasks = []
|
|
261
|
+
await tuned.watch.close()
|
|
262
|
+
self._tuned = []
|
|
263
|
+
|
|
264
|
+
async def _open_parameters(self, binding: ROS2ParameterBinding, runtime: RuntimeInterface) -> _Tuned:
|
|
265
|
+
"""Create one Binding's Parameters and register interest in the node they live on.
|
|
266
|
+
|
|
267
|
+
:raises DuplicateVariableNameError: When a name is already taken on this ParameterSet.
|
|
268
|
+
"""
|
|
269
|
+
tuned = _Tuned(binding=binding, watch=runtime.parameters.watch(binding.path))
|
|
270
|
+
try:
|
|
271
|
+
for parameter, parameter_type in binding.parameters.items():
|
|
272
|
+
tuned.parameters[parameter] = await self._add_parameter(tuned, parameter, parameter_type)
|
|
273
|
+
except BaseException:
|
|
274
|
+
await tuned.watch.close()
|
|
275
|
+
raise
|
|
276
|
+
return tuned
|
|
277
|
+
|
|
278
|
+
async def _add_parameter(self, tuned: _Tuned, parameter: str, parameter_type: ParameterType) -> _Parameter:
|
|
279
|
+
"""Create one Parameter, with the access level its declaration asked for and its check.
|
|
280
|
+
|
|
281
|
+
Read-only is applied here because an access level cannot be moved after ``_init``.
|
|
282
|
+
A read-only Parameter still gets the write check, which answers a client's
|
|
283
|
+
write ``BadNotWritable``.
|
|
284
|
+
"""
|
|
285
|
+
binding = tuned.binding
|
|
286
|
+
variant_type, zero = ua_parameter_shape(parameter_type)
|
|
287
|
+
*namespaces, leaf = binding.node_path(parameter)
|
|
288
|
+
*raw_namespaces, raw_leaf = parameter.split(".")
|
|
289
|
+
entry = PlanVariable(
|
|
290
|
+
name=leaf,
|
|
291
|
+
display_name=raw_leaf,
|
|
292
|
+
above=tuple(
|
|
293
|
+
NodeLevel(name=name, display_name=raw, group=True)
|
|
294
|
+
for name, raw in zip((binding.name, *namespaces), (binding.name, *raw_namespaces), strict=True)
|
|
295
|
+
),
|
|
296
|
+
source=join_path(absolute(binding.path), parameter),
|
|
297
|
+
initial=zero,
|
|
298
|
+
variant_type=variant_type,
|
|
299
|
+
)
|
|
300
|
+
group = await ensure_groups(self.parameter_set, entry.above)
|
|
301
|
+
require_unused_name(leaf, (group,), "this ParameterSet")
|
|
302
|
+
live = _Parameter(
|
|
303
|
+
ros2_name=parameter,
|
|
304
|
+
name=entry.full_name,
|
|
305
|
+
parameter_type=parameter_type,
|
|
306
|
+
variant_type=variant_type,
|
|
307
|
+
value=zero,
|
|
308
|
+
writable=binding.writable(parameter),
|
|
309
|
+
)
|
|
310
|
+
# Created as an inward write: opensmi writes the typed zero through the check.
|
|
311
|
+
async with live.writing:
|
|
312
|
+
live.inward = asyncio.current_task()
|
|
313
|
+
try:
|
|
314
|
+
live.variable = await add_plan_variable(
|
|
315
|
+
group,
|
|
316
|
+
leaf,
|
|
317
|
+
entry,
|
|
318
|
+
None,
|
|
319
|
+
writable=live.writable,
|
|
320
|
+
write_check=self._write_check(tuned, live),
|
|
321
|
+
)
|
|
322
|
+
finally:
|
|
323
|
+
live.inward = None
|
|
324
|
+
return live
|
|
325
|
+
|
|
326
|
+
async def _start_tuning(self, tuned: _Tuned) -> None:
|
|
327
|
+
"""Put the first status on one Binding's Parameters, and start its tasks.
|
|
328
|
+
|
|
329
|
+
The status is written here so that a Parameter says ``BadNoCommunication`` from the moment
|
|
330
|
+
it exists, whatever order the tasks run in.
|
|
331
|
+
"""
|
|
332
|
+
for parameter in tuned.parameters.values():
|
|
333
|
+
async with parameter.writing:
|
|
334
|
+
await self._write_inward(parameter)
|
|
335
|
+
# Nothing is confirmed yet, so the refresh task reads on its first pass.
|
|
336
|
+
tuned.unconfirmed.set()
|
|
337
|
+
name = tuned.binding.name
|
|
338
|
+
tuned.tasks = [
|
|
339
|
+
asyncio.create_task(self._follow_parameters(tuned), name=f"ros2-parameter-events-{name}"),
|
|
340
|
+
asyncio.create_task(self._refresh_parameters(tuned), name=f"ros2-parameter-refresh-{name}"),
|
|
341
|
+
]
|
|
342
|
+
|
|
343
|
+
def _write_check(self, tuned: _Tuned, parameter: _Parameter) -> Callable[[Any], Awaitable[None]]:
|
|
344
|
+
"""Return the async check opensmi awaits before it stores a value written to one Parameter."""
|
|
345
|
+
|
|
346
|
+
async def decide(value: Any) -> None:
|
|
347
|
+
await self._decide_write(tuned, parameter, value)
|
|
348
|
+
|
|
349
|
+
return decide
|
|
350
|
+
|
|
351
|
+
async def _decide_write(self, tuned: _Tuned, parameter: _Parameter, value: object) -> None:
|
|
352
|
+
"""Decide one write to one Parameter: return to store it, raise to answer it with a code.
|
|
353
|
+
|
|
354
|
+
The equality test comes first, because it needs no call. It only
|
|
355
|
+
counts once ROS2 has said something: before that the Parameter holds a typed zero, and a
|
|
356
|
+
client writing that zero goes to the node like any other write. Inward writes pass as the
|
|
357
|
+
lock holder's own.
|
|
358
|
+
|
|
359
|
+
:raises UaStatusCodeError: With the code the client is answered with.
|
|
360
|
+
"""
|
|
361
|
+
if parameter.inward is not None and parameter.inward is asyncio.current_task():
|
|
362
|
+
if _same(value, parameter.value):
|
|
363
|
+
return
|
|
364
|
+
# An inward write carries the value just recorded, so this is our fault; refuse rather than deadlock.
|
|
365
|
+
_refuse(StatusCodes.BadInternalError)
|
|
366
|
+
if _known(parameter, value):
|
|
367
|
+
return
|
|
368
|
+
self._check_locally(parameter, value)
|
|
369
|
+
async with parameter.writing:
|
|
370
|
+
if _known(parameter, value):
|
|
371
|
+
# An event or another client's write landed while this one waited.
|
|
372
|
+
return
|
|
373
|
+
await self._send_parameter_write(tuned, parameter, value)
|
|
374
|
+
|
|
375
|
+
def _check_locally(self, parameter: _Parameter, value: object) -> None:
|
|
376
|
+
"""Refuse what the declaration and the node's descriptor already rule out, without a call.
|
|
377
|
+
|
|
378
|
+
:raises UaStatusCodeError: ``BadTypeMismatch``, ``BadNotWritable`` or ``BadOutOfRange``.
|
|
379
|
+
"""
|
|
380
|
+
if not _carries(value, parameter.parameter_type):
|
|
381
|
+
_refuse(StatusCodes.BadTypeMismatch)
|
|
382
|
+
description = parameter.description
|
|
383
|
+
if not parameter.writable or (description is not None and description.read_only):
|
|
384
|
+
_refuse(StatusCodes.BadNotWritable)
|
|
385
|
+
declared = None if description is None else description.range
|
|
386
|
+
if declared is not None and parameter.parameter_type in _RANGED and not declared.admits(value): # type: ignore[arg-type]
|
|
387
|
+
_refuse(StatusCodes.BadOutOfRange)
|
|
388
|
+
|
|
389
|
+
async def _send_parameter_write(self, tuned: _Tuned, parameter: _Parameter, value: object) -> None:
|
|
390
|
+
"""Ask the node to take one value, and answer with what it said.
|
|
391
|
+
|
|
392
|
+
Called with the Parameter's lock held. Returning means the node took the value.
|
|
393
|
+
|
|
394
|
+
:raises UaStatusCodeError: ``BadTypeMismatch`` or ``BadOutOfRange`` when the guard behind
|
|
395
|
+
the seam refused to put the value in a message; ``BadOutOfRange`` or
|
|
396
|
+
``BadInvalidArgument`` when the node refused it; ``BadTimeout`` when the node did not
|
|
397
|
+
answer in time.
|
|
398
|
+
"""
|
|
399
|
+
runtime = self._require_parameter_runtime()
|
|
400
|
+
binding = tuned.binding
|
|
401
|
+
sent = ros2_parameter_value(value, parameter.parameter_type)
|
|
402
|
+
# An event that landed during the call is newer than the value sent (an `on_set` callback
|
|
403
|
+
# may have adjusted it), so the value is not claimed then.
|
|
404
|
+
epoch = parameter.epoch
|
|
405
|
+
try:
|
|
406
|
+
async with asyncio.timeout(binding.write_timeout):
|
|
407
|
+
outcomes = await runtime.parameters.set(node=binding.path, values={parameter.ros2_name: sent})
|
|
408
|
+
except FieldTypeMismatchError:
|
|
409
|
+
_refuse(StatusCodes.BadTypeMismatch)
|
|
410
|
+
except FieldValueOutOfRangeError:
|
|
411
|
+
_refuse(StatusCodes.BadOutOfRange)
|
|
412
|
+
except (TimeoutError, ROS2RuntimeError) as error:
|
|
413
|
+
self.logger.warning(
|
|
414
|
+
"A ROS2 node did not answer a parameter write in time",
|
|
415
|
+
binding=binding.name,
|
|
416
|
+
node=binding.path,
|
|
417
|
+
parameter=parameter.ros2_name,
|
|
418
|
+
error=str(error) or type(error).__name__,
|
|
419
|
+
)
|
|
420
|
+
self._lose_track(tuned, parameter)
|
|
421
|
+
_refuse(StatusCodes.BadTimeout)
|
|
422
|
+
outcome = outcomes.get(parameter.ros2_name)
|
|
423
|
+
if outcome is None or not outcome.answered:
|
|
424
|
+
# Unknown whether the value landed, as after a timeout: fetch the truth, do not guess.
|
|
425
|
+
self._lose_track(tuned, parameter)
|
|
426
|
+
_refuse(StatusCodes.BadTimeout)
|
|
427
|
+
if not outcome.successful:
|
|
428
|
+
self.logger.info(
|
|
429
|
+
"A ROS2 node refused a parameter write",
|
|
430
|
+
binding=binding.name,
|
|
431
|
+
node=binding.path,
|
|
432
|
+
parameter=parameter.ros2_name,
|
|
433
|
+
reason=outcome.reason,
|
|
434
|
+
)
|
|
435
|
+
_refuse(refusal_code(outcome.reason))
|
|
436
|
+
if parameter.epoch == epoch:
|
|
437
|
+
parameter.holds(ua_value(sent.value))
|
|
438
|
+
|
|
439
|
+
def _lose_track(self, tuned: _Tuned, parameter: _Parameter) -> None:
|
|
440
|
+
"""Note that what the node holds for one name is unknown, so the refresh reads it again."""
|
|
441
|
+
parameter.satisfied = False
|
|
442
|
+
tuned.unconfirmed.set()
|
|
443
|
+
|
|
444
|
+
async def _follow_parameters(self, tuned: _Tuned) -> None:
|
|
445
|
+
"""Mirror every change the node announces, for the names this Binding declared.
|
|
446
|
+
|
|
447
|
+
Events caused by a client's write are applied like any other, since the node just confirmed
|
|
448
|
+
the value. Other names are skipped.
|
|
449
|
+
"""
|
|
450
|
+
async for update in tuned.watch:
|
|
451
|
+
for reading in update.readings:
|
|
452
|
+
parameter = tuned.parameters.get(reading.name)
|
|
453
|
+
if parameter is None:
|
|
454
|
+
continue
|
|
455
|
+
parameter.epoch += 1
|
|
456
|
+
try:
|
|
457
|
+
await self._apply(tuned, parameter, reading)
|
|
458
|
+
except Exception:
|
|
459
|
+
# Runs on a task nobody awaits; one unwritable value must not stop the Binding following.
|
|
460
|
+
self.logger.exception(
|
|
461
|
+
"Could not apply a ROS2 parameter change",
|
|
462
|
+
binding=tuned.binding.name,
|
|
463
|
+
node=tuned.binding.path,
|
|
464
|
+
parameter=reading.name,
|
|
465
|
+
)
|
|
466
|
+
|
|
467
|
+
async def _refresh_parameters(self, tuned: _Tuned) -> None:
|
|
468
|
+
"""Read every unconfirmed name, and keep reading until they are all confirmed.
|
|
469
|
+
|
|
470
|
+
It is self-disabling: while every name is confirmed it waits on an event and makes no calls.
|
|
471
|
+
A name going unconfirmed (absent node, timed-out write, deleted parameter) sets the event.
|
|
472
|
+
Without this, a node that was off at boot and comes up silently would never be seen.
|
|
473
|
+
"""
|
|
474
|
+
while True:
|
|
475
|
+
await self._read_the_unconfirmed(tuned)
|
|
476
|
+
if tuned.confirmed:
|
|
477
|
+
# Nothing awaits between the clear and the wait, so no name can slip in between.
|
|
478
|
+
tuned.unconfirmed.clear()
|
|
479
|
+
await tuned.unconfirmed.wait()
|
|
480
|
+
await asyncio.sleep(tuned.binding.retry_interval)
|
|
481
|
+
|
|
482
|
+
async def _read_the_unconfirmed(self, tuned: _Tuned) -> None:
|
|
483
|
+
"""Read each name that is not confirmed present, one parameter per request.
|
|
484
|
+
|
|
485
|
+
One request per name, since the batch call answers with an empty list when any one name is
|
|
486
|
+
undeclared, which would blank every value on one typo. A transport failure ends
|
|
487
|
+
the pass, as the remaining names would time out for the same reason.
|
|
488
|
+
"""
|
|
489
|
+
for parameter in tuple(tuned.parameters.values()):
|
|
490
|
+
if parameter.satisfied:
|
|
491
|
+
continue
|
|
492
|
+
if not await self._look(tuned, parameter):
|
|
493
|
+
return
|
|
494
|
+
|
|
495
|
+
async def _look(self, tuned: _Tuned, parameter: _Parameter) -> bool:
|
|
496
|
+
"""Read one name and its descriptor off the node and make its Parameter say what came back.
|
|
497
|
+
|
|
498
|
+
:return: Whether the node answered at all. ``False`` is about the whole Binding, so a caller
|
|
499
|
+
working through several names stops.
|
|
500
|
+
"""
|
|
501
|
+
runtime = self._require_parameter_runtime()
|
|
502
|
+
node = tuned.binding.path
|
|
503
|
+
# An event that landed during the read is newer news and must not be overwritten.
|
|
504
|
+
epoch = parameter.epoch
|
|
505
|
+
try:
|
|
506
|
+
reading = await runtime.parameters.get(node=node, name=parameter.ros2_name)
|
|
507
|
+
if reading.present:
|
|
508
|
+
parameter.description = await runtime.parameters.describe(node=node, name=parameter.ros2_name)
|
|
509
|
+
except ROS2RuntimeError as error:
|
|
510
|
+
await self._unreachable(tuned, str(error))
|
|
511
|
+
return False
|
|
512
|
+
if parameter.epoch == epoch:
|
|
513
|
+
await self._apply(tuned, parameter, reading)
|
|
514
|
+
return True
|
|
515
|
+
|
|
516
|
+
async def _apply(self, tuned: _Tuned, parameter: _Parameter, reading: ParameterReading) -> None:
|
|
517
|
+
"""Take one look at one ROS2 parameter and make the Parameter say what it found.
|
|
518
|
+
|
|
519
|
+
Runs under the Parameter's lock, so a client's write waiting on the node is answered first.
|
|
520
|
+
"""
|
|
521
|
+
async with parameter.writing:
|
|
522
|
+
# The node answered, so it is no longer unreachable and a later silence is news again.
|
|
523
|
+
parameter.reported.discard(PARAMETER_UNREACHABLE)
|
|
524
|
+
if not reading.present:
|
|
525
|
+
parameter.satisfied = False
|
|
526
|
+
await self._report(
|
|
527
|
+
tuned,
|
|
528
|
+
parameter,
|
|
529
|
+
PARAMETER_ABSENT,
|
|
530
|
+
Severity.ERROR,
|
|
531
|
+
f"{tuned.binding.path} has no parameter called {parameter.ros2_name!r}, so this "
|
|
532
|
+
f"Parameter has nothing to show; the names are declared, so this is a "
|
|
533
|
+
f"declaration to correct rather than a value to wait for",
|
|
534
|
+
)
|
|
535
|
+
elif reading.value is None:
|
|
536
|
+
# Present but empty: satisfied, not sampled, so the Parameter says it is waiting.
|
|
537
|
+
parameter.satisfied = True
|
|
538
|
+
parameter.sampled = False
|
|
539
|
+
parameter.reported.clear()
|
|
540
|
+
elif reading.value.type is not parameter.parameter_type:
|
|
541
|
+
parameter.satisfied = False
|
|
542
|
+
await self._report(
|
|
543
|
+
tuned,
|
|
544
|
+
parameter,
|
|
545
|
+
PARAMETER_TYPE_MISMATCH,
|
|
546
|
+
Severity.ERROR,
|
|
547
|
+
f"{parameter.ros2_name!r} on {tuned.binding.path} is a "
|
|
548
|
+
f"{reading.value.type.value!r} and this Binding declares it a "
|
|
549
|
+
f"{parameter.parameter_type.value!r}; a type is declared and never discovered, "
|
|
550
|
+
f"so the Parameter stays bad rather than showing a value of the wrong kind",
|
|
551
|
+
)
|
|
552
|
+
else:
|
|
553
|
+
parameter.holds(ua_value(reading.value.value))
|
|
554
|
+
if not parameter.satisfied:
|
|
555
|
+
tuned.unconfirmed.set()
|
|
556
|
+
await self._write_inward(parameter)
|
|
557
|
+
|
|
558
|
+
async def _unreachable(self, tuned: _Tuned, text: str) -> None:
|
|
559
|
+
"""Say that a node did not answer, and take every one of its Parameters bad with it.
|
|
560
|
+
|
|
561
|
+
Reported once per Binding, since the fact is about the node.
|
|
562
|
+
"""
|
|
563
|
+
for parameter in tuned.parameters.values():
|
|
564
|
+
async with parameter.writing:
|
|
565
|
+
parameter.satisfied = False
|
|
566
|
+
await self._write_inward(parameter)
|
|
567
|
+
tuned.unconfirmed.set()
|
|
568
|
+
await self._report_once(
|
|
569
|
+
tuned,
|
|
570
|
+
PARAMETER_UNREACHABLE,
|
|
571
|
+
Severity.ERROR,
|
|
572
|
+
f"{tuned.binding.path} did not answer, so what it holds is no longer known: {text}",
|
|
573
|
+
)
|
|
574
|
+
|
|
575
|
+
async def _write_inward(self, parameter: _Parameter) -> None:
|
|
576
|
+
"""Write one Parameter's value and status, as an ordinary write through opensmi.
|
|
577
|
+
|
|
578
|
+
Called with the Parameter's lock held, and the write passes the check as the holder's own.
|
|
579
|
+
The value always travels with the status, because a status alone leaves the variant null.
|
|
580
|
+
"""
|
|
581
|
+
parameter.inward = asyncio.current_task()
|
|
582
|
+
try:
|
|
583
|
+
await parameter.node.ua_node.write_value(
|
|
584
|
+
status_value(parameter.value, parameter.variant_type, parameter.status),
|
|
585
|
+
)
|
|
586
|
+
finally:
|
|
587
|
+
parameter.inward = None
|
|
588
|
+
|
|
589
|
+
async def _report(
|
|
590
|
+
self,
|
|
591
|
+
tuned: _Tuned,
|
|
592
|
+
parameter: _Parameter,
|
|
593
|
+
code: str,
|
|
594
|
+
severity: Severity,
|
|
595
|
+
text: str,
|
|
596
|
+
) -> None:
|
|
597
|
+
"""Fire one BindingEvent about one Parameter, at most once per condition per name.
|
|
598
|
+
|
|
599
|
+
The latch is cleared when the name reads well again. The parameter goes in the event's
|
|
600
|
+
path (the node plus the dotted name), not in the text.
|
|
601
|
+
"""
|
|
602
|
+
if code in parameter.reported:
|
|
603
|
+
return
|
|
604
|
+
parameter.reported.add(code)
|
|
605
|
+
await self._emit(tuned.binding, code, severity, text, parameter=parameter.ros2_name)
|
|
606
|
+
|
|
607
|
+
async def _report_once(self, tuned: _Tuned, code: str, severity: Severity, text: str) -> None:
|
|
608
|
+
"""Fire one BindingEvent about a whole Binding, at most once per condition.
|
|
609
|
+
|
|
610
|
+
The latch lives on the Parameters, because every one of them clears it when it reads well.
|
|
611
|
+
"""
|
|
612
|
+
if all(code in each.reported for each in tuned.parameters.values()):
|
|
613
|
+
return
|
|
614
|
+
for each in tuned.parameters.values():
|
|
615
|
+
each.reported.add(code)
|
|
616
|
+
await self._emit(tuned.binding, code, severity, text)
|
|
617
|
+
|
|
618
|
+
async def _emit(
|
|
619
|
+
self,
|
|
620
|
+
binding: ROS2ParameterBinding,
|
|
621
|
+
code: str,
|
|
622
|
+
severity: Severity,
|
|
623
|
+
text: str,
|
|
624
|
+
parameter: str | None = None,
|
|
625
|
+
) -> None:
|
|
626
|
+
"""Raise one BindingEvent, and never let the reporting itself be the fault.
|
|
627
|
+
|
|
628
|
+
:param parameter: The ROS2 parameter this is about, when it is about one rather than the
|
|
629
|
+
whole node. It is joined onto the event's ``ROS2Path``.
|
|
630
|
+
"""
|
|
631
|
+
node = absolute(binding.path)
|
|
632
|
+
path = node if parameter is None else join_path(node, parameter)
|
|
633
|
+
try:
|
|
634
|
+
await self.emit_binding_event(
|
|
635
|
+
binding=binding.name,
|
|
636
|
+
path=path,
|
|
637
|
+
code=code,
|
|
638
|
+
severity=severity,
|
|
639
|
+
text=text,
|
|
640
|
+
)
|
|
641
|
+
except Exception:
|
|
642
|
+
self.logger.exception(
|
|
643
|
+
"Could not fire a BindingEvent about a ROS2 parameter",
|
|
644
|
+
binding=binding.name,
|
|
645
|
+
node=path,
|
|
646
|
+
code=code,
|
|
647
|
+
)
|
|
648
|
+
|
|
649
|
+
def _require_parameter_runtime(self) -> RuntimeInterface:
|
|
650
|
+
"""Return the Runtime this mirror was initialised with.
|
|
651
|
+
|
|
652
|
+
:raises DeclarationError: When there is none, which is a wiring fault.
|
|
653
|
+
"""
|
|
654
|
+
if self._parameter_runtime is None: # pragma: no cover -- every path here runs after init
|
|
655
|
+
message = f"{type(self).__name__} has no Runtime; its ParameterSet was never initialised"
|
|
656
|
+
raise DeclarationError(message)
|
|
657
|
+
return self._parameter_runtime
|
|
658
|
+
|
|
659
|
+
|
|
660
|
+
def _refuse(code: int) -> NoReturn:
|
|
661
|
+
"""Answer the write being checked with one status code, which opensmi hands to the client."""
|
|
662
|
+
raise UaStatusCodeError(code)
|
|
663
|
+
|
|
664
|
+
|
|
665
|
+
def _known(parameter: _Parameter, value: object) -> bool:
|
|
666
|
+
"""Return whether a written value is the one ROS2 last said it holds, having said one at all."""
|
|
667
|
+
return parameter.sampled and _same(value, parameter.value)
|
|
668
|
+
|
|
669
|
+
|
|
670
|
+
def _same(written: object, held: object) -> bool:
|
|
671
|
+
"""Return whether a written value is the value held, kind for kind.
|
|
672
|
+
|
|
673
|
+
Exact, so ``True`` is not ``1`` and ``1`` is not ``1.0``. ``NaN`` equals ``NaN``, because a
|
|
674
|
+
double parameter can hold one and its inward write has to pass.
|
|
675
|
+
"""
|
|
676
|
+
if isinstance(written, (list, tuple)) and isinstance(held, (list, tuple)):
|
|
677
|
+
return len(written) == len(held) and all(_same(a, b) for a, b in zip(written, held, strict=True))
|
|
678
|
+
if type(written) is not type(held):
|
|
679
|
+
return False
|
|
680
|
+
if isinstance(written, float) and isinstance(held, float) and math.isnan(written) and math.isnan(held):
|
|
681
|
+
return True
|
|
682
|
+
return written == held
|
|
683
|
+
|
|
684
|
+
|
|
685
|
+
def _carries(value: object, parameter_type: ParameterType) -> bool:
|
|
686
|
+
"""Return whether a written value is of the declared ROS2 parameter type.
|
|
687
|
+
|
|
688
|
+
A scalar needs exactly its Python type, so a boolean is not an integer and an integer is not a
|
|
689
|
+
double.
|
|
690
|
+
"""
|
|
691
|
+
if parameter_type is ParameterType.BYTE_ARRAY:
|
|
692
|
+
return isinstance(value, (bytes, bytearray))
|
|
693
|
+
element = _ELEMENTS.get(parameter_type)
|
|
694
|
+
if element is not None:
|
|
695
|
+
return isinstance(value, (list, tuple)) and all(_carries(each, element) for each in value)
|
|
696
|
+
match parameter_type:
|
|
697
|
+
case ParameterType.BOOL:
|
|
698
|
+
return isinstance(value, bool)
|
|
699
|
+
case ParameterType.INTEGER:
|
|
700
|
+
return isinstance(value, int) and not isinstance(value, bool)
|
|
701
|
+
case ParameterType.DOUBLE:
|
|
702
|
+
return isinstance(value, float)
|
|
703
|
+
case _:
|
|
704
|
+
return isinstance(value, str)
|