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,107 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Satisfied, as a status code: telling a robot being off from this server being wrong.
|
|
6
|
+
|
|
7
|
+
A Binding is Satisfied when its ROS2 counterpart is present on the graph (a publisher on the
|
|
8
|
+
topic, a discovered service or action server), whether or not it has carried data. Every variable
|
|
9
|
+
the Binding owns reads that as a status code:
|
|
10
|
+
|
|
11
|
+
* ``BadNoCommunication``: nothing publishes on the topic. The robot is off or the topic does not
|
|
12
|
+
exist on it.
|
|
13
|
+
* ``BadWaitingForInitialData``: something publishes and no message has arrived yet.
|
|
14
|
+
* ``Good``: a sample has arrived, and the value beside the status is that sample.
|
|
15
|
+
|
|
16
|
+
A bad status is written together with the typed value (:func:`status_value`), so recovery is one
|
|
17
|
+
write and nothing stale is left. OPC UA Part 4 7.7.1 says a server receiving an error status
|
|
18
|
+
sets the value to null, and asyncua does that, so a client reading a Bad variable sees a null
|
|
19
|
+
value however it was written. :func:`transform_status` applies the same three codes to a TF frame.
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
from __future__ import annotations
|
|
23
|
+
|
|
24
|
+
from typing import Final
|
|
25
|
+
|
|
26
|
+
from asyncua import ua
|
|
27
|
+
from asyncua.ua.status_codes import StatusCodes
|
|
28
|
+
|
|
29
|
+
__all__ = [
|
|
30
|
+
"GOOD",
|
|
31
|
+
"NO_COMMUNICATION",
|
|
32
|
+
"WAITING_FOR_INITIAL_DATA",
|
|
33
|
+
"status_code",
|
|
34
|
+
"status_value",
|
|
35
|
+
"transform_status",
|
|
36
|
+
]
|
|
37
|
+
|
|
38
|
+
GOOD: Final = ua.StatusCode(ua.UInt32(StatusCodes.Good))
|
|
39
|
+
"""What a variable reads once a sample has arrived on a topic that has a publisher."""
|
|
40
|
+
|
|
41
|
+
NO_COMMUNICATION: Final = ua.StatusCode(ua.UInt32(StatusCodes.BadNoCommunication))
|
|
42
|
+
"""What a variable reads while nothing publishes on its topic: the Binding is not satisfied."""
|
|
43
|
+
|
|
44
|
+
WAITING_FOR_INITIAL_DATA: Final = ua.StatusCode(ua.UInt32(StatusCodes.BadWaitingForInitialData))
|
|
45
|
+
"""What a variable reads while its topic has a publisher and no message has arrived yet."""
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
def status_code(*, satisfied: bool, sampled: bool) -> ua.StatusCode:
|
|
49
|
+
"""Return the status code one variable reads, given its Binding's two facts.
|
|
50
|
+
|
|
51
|
+
:param satisfied: Whether the Binding's ROS2 counterpart is present on the graph.
|
|
52
|
+
:param sampled: Whether a message has ever arrived on it.
|
|
53
|
+
"""
|
|
54
|
+
# Presence outranks history: a vanished publisher is the robot being off, whatever arrived before.
|
|
55
|
+
if not satisfied:
|
|
56
|
+
return NO_COMMUNICATION
|
|
57
|
+
return GOOD if sampled else WAITING_FOR_INITIAL_DATA
|
|
58
|
+
|
|
59
|
+
|
|
60
|
+
def transform_status(
|
|
61
|
+
*,
|
|
62
|
+
present: bool,
|
|
63
|
+
static: bool,
|
|
64
|
+
rooted: bool,
|
|
65
|
+
seen: float | None,
|
|
66
|
+
now: float,
|
|
67
|
+
within: float,
|
|
68
|
+
) -> ua.StatusCode:
|
|
69
|
+
"""Return the status code one TF frame's Location reads, out of the same three codes.
|
|
70
|
+
|
|
71
|
+
A frame is satisfied when that frame was in a recent message and something is publishing on
|
|
72
|
+
the transform topics. Topic presence alone is too coarse: a dead arm driver leaves the base
|
|
73
|
+
publishing ``/tf``. Two exemptions from freshness keep the status worth reading:
|
|
74
|
+
|
|
75
|
+
* A static frame is published once and is quiet by definition. Silence says nothing, because
|
|
76
|
+
its publisher is transient-local and may exit, and its pose already carries the ``Constant``
|
|
77
|
+
access-level bit. Absence still counts: a static frame that never arrived reads
|
|
78
|
+
bad.
|
|
79
|
+
* The root of the rendered tree is nobody's child, so no message carries it. It reads the
|
|
80
|
+
presence of the transform topics only, which still turns bad when the robot is off.
|
|
81
|
+
An all-static system whose publisher exits reads bad here,
|
|
82
|
+
while its static frames stay good.
|
|
83
|
+
|
|
84
|
+
:param present: Whether anything else is publishing on the transform topics, with our own
|
|
85
|
+
broadcast discounted.
|
|
86
|
+
:param static: Whether the last message to carry this frame arrived on the static topic.
|
|
87
|
+
:param rooted: Whether this frame is the root of the rendered tree.
|
|
88
|
+
:param seen: When this frame was last in a message, or ``None`` if it never has been.
|
|
89
|
+
:param now: The current time, off the same clock ``seen`` was stamped from.
|
|
90
|
+
:param within: How many seconds a frame may go unpublished and still speak for itself.
|
|
91
|
+
"""
|
|
92
|
+
if rooted:
|
|
93
|
+
return GOOD if present else NO_COMMUNICATION
|
|
94
|
+
if seen is None:
|
|
95
|
+
# Before the static exemption: never having arrived is absence, which static does not excuse.
|
|
96
|
+
return status_code(satisfied=present, sampled=False)
|
|
97
|
+
if static:
|
|
98
|
+
return GOOD
|
|
99
|
+
return status_code(satisfied=present and now - seen <= within, sampled=True)
|
|
100
|
+
|
|
101
|
+
|
|
102
|
+
def status_value(value: object, variant_type: ua.VariantType, status: ua.StatusCode) -> ua.DataValue:
|
|
103
|
+
"""Return the DataValue one write puts on a variable: the typed value and its status.
|
|
104
|
+
|
|
105
|
+
Writing a status alone would leave the variant null.
|
|
106
|
+
"""
|
|
107
|
+
return ua.DataValue(Value=ua.Variant(value, variant_type), StatusCode=status)
|
|
@@ -0,0 +1,135 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The ROS2 side of the Structure DataTypes: a message level as a dataclass.
|
|
6
|
+
|
|
7
|
+
:func:`structure_class` makes a dataclass from a name and its fields, and ``opensmi.server.structures``
|
|
8
|
+
creates the DataType, its encoding and its definition from that class and encodes its instances.
|
|
9
|
+
What is left here is the mapping between the maps a projection works with and those instances, and
|
|
10
|
+
the checks a write needs that the encoding does not make.
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import dataclasses
|
|
16
|
+
import typing
|
|
17
|
+
from functools import cache
|
|
18
|
+
from typing import TYPE_CHECKING, Any, TypeAlias
|
|
19
|
+
|
|
20
|
+
from opensmi.ros2.server.errors import DeclarationError
|
|
21
|
+
|
|
22
|
+
if TYPE_CHECKING:
|
|
23
|
+
from collections.abc import Mapping
|
|
24
|
+
|
|
25
|
+
__all__ = [
|
|
26
|
+
"DuplicateStructureError",
|
|
27
|
+
"StructureField",
|
|
28
|
+
"structure_class",
|
|
29
|
+
"structure_elements",
|
|
30
|
+
"structure_instance",
|
|
31
|
+
]
|
|
32
|
+
|
|
33
|
+
NAME_SEPARATOR = "."
|
|
34
|
+
"""Joins the names of the levels above a field in the maps a projection works with.
|
|
35
|
+
|
|
36
|
+
It spells a key, not a BrowseName: the nodes nest.
|
|
37
|
+
"""
|
|
38
|
+
|
|
39
|
+
StructureField: TypeAlias = "tuple[str, Any, int]"
|
|
40
|
+
"""A field as its name, its annotation and the length of a fixed-size array (zero otherwise).
|
|
41
|
+
|
|
42
|
+
The annotation is a ua class, a structure class, or a list of one of them.
|
|
43
|
+
"""
|
|
44
|
+
|
|
45
|
+
|
|
46
|
+
class DuplicateStructureError(DeclarationError):
|
|
47
|
+
"""Two descriptions of one structure name disagree.
|
|
48
|
+
|
|
49
|
+
Raised before any node is made.
|
|
50
|
+
"""
|
|
51
|
+
|
|
52
|
+
|
|
53
|
+
@cache
|
|
54
|
+
def structure_class(name: str, fields: tuple[StructureField, ...]) -> type:
|
|
55
|
+
"""Return the dataclass of a name and fields, the same one for equal arguments.
|
|
56
|
+
|
|
57
|
+
The length of a fixed-size array is kept in the field's metadata, for the write check.
|
|
58
|
+
"""
|
|
59
|
+
return dataclasses.make_dataclass(
|
|
60
|
+
name,
|
|
61
|
+
[(each, annotation, dataclasses.field(metadata={"length": length})) for each, annotation, length in fields],
|
|
62
|
+
)
|
|
63
|
+
|
|
64
|
+
|
|
65
|
+
def structure_instance(cls: type, values: Mapping[str, object], prefix: str = "") -> Any:
|
|
66
|
+
"""Return the instance of a structure that a map of its values describes.
|
|
67
|
+
|
|
68
|
+
:param values: The values by flat name, the field names of the levels above joined with dots.
|
|
69
|
+
An array of structures holds one such map per element.
|
|
70
|
+
:raises KeyError: When a field has no value.
|
|
71
|
+
"""
|
|
72
|
+
built: dict[str, object] = {}
|
|
73
|
+
for each in dataclasses.fields(cls):
|
|
74
|
+
inner, array = _inner(each.type)
|
|
75
|
+
key = f"{prefix}{each.name}"
|
|
76
|
+
if inner is None:
|
|
77
|
+
built[each.name] = values[key]
|
|
78
|
+
elif array:
|
|
79
|
+
built[each.name] = [structure_instance(inner, item) for item in values[key]] # pyright: ignore[reportGeneralTypeIssues]
|
|
80
|
+
else:
|
|
81
|
+
built[each.name] = structure_instance(inner, values, f"{key}{NAME_SEPARATOR}")
|
|
82
|
+
return cls(**built)
|
|
83
|
+
|
|
84
|
+
|
|
85
|
+
def structure_elements(cls: type, value: object) -> list[dict[str, object]]:
|
|
86
|
+
"""Return the elements of an array a client wrote as maps, or refuse the array as a whole.
|
|
87
|
+
|
|
88
|
+
:raises ValueError: When the value is not an array of the structure, when a field is null, or when
|
|
89
|
+
a fixed-size array has the wrong length. The message names the element by its position.
|
|
90
|
+
"""
|
|
91
|
+
if not isinstance(value, (list, tuple)):
|
|
92
|
+
message = f"{type(value).__name__} is not an array of {cls.__name__}"
|
|
93
|
+
raise ValueError(message) # noqa: TRY004
|
|
94
|
+
elements: list[dict[str, object]] = []
|
|
95
|
+
for index, each in enumerate(value):
|
|
96
|
+
try:
|
|
97
|
+
elements.append(_values(cls, each))
|
|
98
|
+
except ValueError as refused:
|
|
99
|
+
message = f"element {index}: {refused}"
|
|
100
|
+
raise ValueError(message) from refused
|
|
101
|
+
return elements
|
|
102
|
+
|
|
103
|
+
|
|
104
|
+
def _inner(annotation: Any) -> tuple[type | None, bool]:
|
|
105
|
+
"""Return the structure class an annotation names, or ``None``, and whether it is a list."""
|
|
106
|
+
array = typing.get_origin(annotation) is list
|
|
107
|
+
element = typing.get_args(annotation)[0] if array else annotation
|
|
108
|
+
return (element if isinstance(element, type) and dataclasses.is_dataclass(element) else None), array
|
|
109
|
+
|
|
110
|
+
|
|
111
|
+
def _values(cls: type, element: object, prefix: str = "") -> dict[str, object]:
|
|
112
|
+
if not isinstance(element, cls):
|
|
113
|
+
message = f"an element of {cls.__name__} arrived as {type(element).__name__}"
|
|
114
|
+
raise ValueError(message) # noqa: TRY004
|
|
115
|
+
values: dict[str, object] = {}
|
|
116
|
+
for each in dataclasses.fields(cls):
|
|
117
|
+
value = getattr(element, each.name)
|
|
118
|
+
array = typing.get_origin(each.type) is list
|
|
119
|
+
if value is None or (array and None in value):
|
|
120
|
+
# A null is not an empty value.
|
|
121
|
+
message = f"the field {each.name!r} is null"
|
|
122
|
+
raise ValueError(message)
|
|
123
|
+
length = each.metadata["length"]
|
|
124
|
+
if length and len(value) != length:
|
|
125
|
+
message = f"the field {each.name!r} is an array of {length} elements, and {len(value)} were given"
|
|
126
|
+
raise ValueError(message)
|
|
127
|
+
inner, _ = _inner(each.type)
|
|
128
|
+
key = f"{prefix}{each.name}"
|
|
129
|
+
if inner is None:
|
|
130
|
+
values[key] = list(value) if array else value
|
|
131
|
+
elif array:
|
|
132
|
+
values[key] = [_values(inner, item) for item in value]
|
|
133
|
+
else:
|
|
134
|
+
values.update(_values(inner, value, f"{key}{NAME_SEPARATOR}"))
|
|
135
|
+
return values
|
|
@@ -0,0 +1,39 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The TF Binding, live and both ways: SpatialObjects at init, Locations on a poll, frames out.
|
|
6
|
+
|
|
7
|
+
:class:`ROS2SpatialMixin` gives a machinery item that declared a TF Binding a SpatialObject whose
|
|
8
|
+
mandatory ``PositionFrame`` is the declared head frame (Machine and Components).
|
|
9
|
+
:class:`ROS2TransformMirrorMixin` goes on the Machine alone and polls a snapshot of the facet's
|
|
10
|
+
record of ``/tf`` and ``/tf_static``, rendering one Location per frame the robot publishes
|
|
11
|
+
. A Location is opensmi's ``CartesianFrame``: a ``3DFrame`` in metres and degrees whose
|
|
12
|
+
pose is the transform its TF parent published and whose ``Base`` is the node holding that parent, or
|
|
13
|
+
the World Frame at the tree's root. A frame is filed once and never moved. Locations are
|
|
14
|
+
created through :meth:`~opensmi.ros2.server.mutation.ROS2MutationMixin.mutate_address_space_with`,
|
|
15
|
+
and the mutation cap counts Locations, not nodes: a Location is about ten nodes.
|
|
16
|
+
|
|
17
|
+
Outward, a ``TFBinding`` may name :class:`~opensmi.ros2.server.bindings.PublishedFrame` entries.
|
|
18
|
+
Each gets a client-writable Location in ``AttachPoints``, created at init and not counted by the
|
|
19
|
+
cap. A frame is inward or outward and never both, because our own broadcast comes back through our
|
|
20
|
+
own subscription. Only the pose is writable. A client's write is broadcast once it is stored; the
|
|
21
|
+
poll re-asserts moving frames every tick and static frames when they differ from what last went out.
|
|
22
|
+
The static publisher is transient-local (:data:`~opensmi.ros2.runtime.tf.TF_STATIC_QOS`), so late
|
|
23
|
+
joiners get the frame.
|
|
24
|
+
"""
|
|
25
|
+
|
|
26
|
+
from opensmi.ros2.server.transforms.conversions import METRES, inverted
|
|
27
|
+
from opensmi.ros2.server.transforms.filing import ALTERNATIVE_BASE_ABSENT, FRAME_COLLISION
|
|
28
|
+
from opensmi.ros2.server.transforms.mirror import SEPARATE_FRAME_TREES, ROS2TransformMirrorMixin
|
|
29
|
+
from opensmi.ros2.server.transforms.spatial import ROS2SpatialMixin
|
|
30
|
+
|
|
31
|
+
__all__ = [
|
|
32
|
+
"ALTERNATIVE_BASE_ABSENT",
|
|
33
|
+
"FRAME_COLLISION",
|
|
34
|
+
"METRES",
|
|
35
|
+
"SEPARATE_FRAME_TREES",
|
|
36
|
+
"ROS2SpatialMixin",
|
|
37
|
+
"ROS2TransformMirrorMixin",
|
|
38
|
+
"inverted",
|
|
39
|
+
]
|
|
@@ -0,0 +1,58 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Units and poses: what a TF transform is in OPC UA terms and back."""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
from typing import Final
|
|
10
|
+
|
|
11
|
+
from opensmi.core import Unit
|
|
12
|
+
from opensmi.server.spatial_object import Orientation, Pose, Position
|
|
13
|
+
|
|
14
|
+
from opensmi.ros2.runtime.tf import Transform
|
|
15
|
+
from opensmi.ros2.server.spatial import (
|
|
16
|
+
rotation,
|
|
17
|
+
)
|
|
18
|
+
|
|
19
|
+
METRES: Final = Unit.METRE
|
|
20
|
+
"""What a TF translation is in. opensmi's ``Position`` defaults to millimetres, so every frame is
|
|
21
|
+
created with metres explicitly.
|
|
22
|
+
"""
|
|
23
|
+
|
|
24
|
+
DEGREES: Final = Unit.DEGREE_UNIT_OF_ANGLE
|
|
25
|
+
"""What a TF orientation is in: the unit RSL's ``Orientation`` asks for."""
|
|
26
|
+
|
|
27
|
+
_ZERO: Final = Position(0.0, 0.0, 0.0, unit=METRES)
|
|
28
|
+
_LEVEL: Final = Orientation(0.0, 0.0, 0.0, unit=DEGREES)
|
|
29
|
+
_ORIGIN: Final = Pose(_ZERO, _LEVEL)
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def inverted(position: Position, orientation: Orientation) -> tuple[Position, Orientation]:
|
|
33
|
+
"""Return the pose that undoes one: if the pair is the transform ``T``, this is ``T`` inverse.
|
|
34
|
+
|
|
35
|
+
For an integrator who knows where an object sits in some frame and has to publish the opposite
|
|
36
|
+
link, because a ROS2 frame has one parent (``map -> odom`` from the robot's pose in the map). It
|
|
37
|
+
lives on this side of the seam because it constructs opensmi types.
|
|
38
|
+
|
|
39
|
+
:param position: Where the frame sits, in metres.
|
|
40
|
+
:param orientation: How it is turned, as ``A``, ``B`` and ``C`` in degrees.
|
|
41
|
+
:return: The inverse pose, in the same units.
|
|
42
|
+
"""
|
|
43
|
+
# Uses opensmi's `Pose.inverse()`, with the orientation brought to degrees first.
|
|
44
|
+
inverse = Pose(position, orientation.to(DEGREES)).inverse()
|
|
45
|
+
return inverse.position, inverse.orientation
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
def _as_transform(position: Position, orientation: Orientation) -> Transform:
|
|
49
|
+
"""Return one OPC UA pose as the transform ROS2 wants.
|
|
50
|
+
|
|
51
|
+
Metres stay metres and degrees become a unit quaternion through
|
|
52
|
+
:func:`~opensmi.ros2.server.spatial.rotation`, the inverse of the inward conversion.
|
|
53
|
+
"""
|
|
54
|
+
position, orientation = position.to(METRES), orientation.to(DEGREES)
|
|
55
|
+
return Transform(
|
|
56
|
+
translation=(position.x, position.y, position.z),
|
|
57
|
+
rotation=rotation((orientation.a, orientation.b, orientation.c)),
|
|
58
|
+
)
|
|
@@ -0,0 +1,340 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""Frame filing: new Locations, their folders and the events about frames that cannot be filed."""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
from typing import TYPE_CHECKING, Any, Final
|
|
10
|
+
|
|
11
|
+
from asyncua import ua
|
|
12
|
+
from asyncua.server.address_space import AttributeValue
|
|
13
|
+
from asyncua.ua.attribute_ids import AttributeIds
|
|
14
|
+
from asyncua.ua.object_ids import ObjectIds
|
|
15
|
+
from opensmi.core.nodesets import RSLNodeIds
|
|
16
|
+
|
|
17
|
+
from opensmi.ros2.runtime.tf import FrameLink
|
|
18
|
+
from opensmi.ros2.server.errors import (
|
|
19
|
+
DeclarationError,
|
|
20
|
+
)
|
|
21
|
+
from opensmi.ros2.server.events import Severity
|
|
22
|
+
from opensmi.ros2.server.keyed import member_browse_name
|
|
23
|
+
from opensmi.ros2.server.mutation import _write_display_name
|
|
24
|
+
from opensmi.ros2.server.spatial import (
|
|
25
|
+
ALTERNATIVE_FRAMES,
|
|
26
|
+
ATTACH_POINTS,
|
|
27
|
+
INTERNAL_FRAMES,
|
|
28
|
+
base_node_of,
|
|
29
|
+
folder_of,
|
|
30
|
+
)
|
|
31
|
+
from opensmi.ros2.server.transforms.conversions import _ORIGIN
|
|
32
|
+
from opensmi.ros2.server.transforms.records import (
|
|
33
|
+
_Frame,
|
|
34
|
+
_Observed,
|
|
35
|
+
_Owner,
|
|
36
|
+
)
|
|
37
|
+
from opensmi.ros2.server.transforms.spatial import ROS2SpatialMixin
|
|
38
|
+
|
|
39
|
+
if TYPE_CHECKING:
|
|
40
|
+
from collections.abc import Mapping, Sequence
|
|
41
|
+
|
|
42
|
+
from asyncua.common.node import Node
|
|
43
|
+
|
|
44
|
+
|
|
45
|
+
FRAME_COLLISION: Final = "FRAME_COLLISION"
|
|
46
|
+
"""The BindingEvent code a frame whose browse name is already taken carries."""
|
|
47
|
+
|
|
48
|
+
ALTERNATIVE_BASE_ABSENT: Final = "ALTERNATIVE_BASE_ABSENT"
|
|
49
|
+
"""The BindingEvent code a declared alternative whose base frame has no node carries."""
|
|
50
|
+
|
|
51
|
+
|
|
52
|
+
_CONSTANT: Final = ua.UInt32(int(ua.AccessLevelExType.CurrentRead) | int(ua.AccessLevelExType.Constant))
|
|
53
|
+
"""``AccessLevelEx`` for a value that is read and never changes. The ``Constant`` bit tells clients not
|
|
54
|
+
to subscribe.
|
|
55
|
+
"""
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
class _FrameFiling(ROS2SpatialMixin):
|
|
59
|
+
"""Creates Locations and files them under a SpatialObject, through the mutation chokepoint."""
|
|
60
|
+
|
|
61
|
+
def __init__(self, *args: Any, **kwargs: Any) -> None:
|
|
62
|
+
"""Start out with nothing rendered, and pass every argument to opensmi."""
|
|
63
|
+
super().__init__(*args, **kwargs)
|
|
64
|
+
self._transform_objects: tuple[_Owner, ...] = ()
|
|
65
|
+
self._transform_index: dict[str, ua.NodeId] = {}
|
|
66
|
+
self._transform_world: ua.NodeId | None = None
|
|
67
|
+
|
|
68
|
+
async def _create_new_locations(
|
|
69
|
+
self,
|
|
70
|
+
owner: _Owner,
|
|
71
|
+
*,
|
|
72
|
+
parents: Mapping[str, str],
|
|
73
|
+
attach: frozenset[str],
|
|
74
|
+
) -> None:
|
|
75
|
+
"""Create a Location for every frame this object owns and has not rendered yet.
|
|
76
|
+
|
|
77
|
+
One chokepoint call per tick, so a client gets one model change naming all the new frames.
|
|
78
|
+
"""
|
|
79
|
+
new = [
|
|
80
|
+
frame
|
|
81
|
+
for frame in sorted(owner.wanted)
|
|
82
|
+
if frame not in owner.frames
|
|
83
|
+
and frame not in owner.refused
|
|
84
|
+
and frame != owner.binding.head
|
|
85
|
+
# A frame has one Location, the first owner's. This excludes a frame re-parented across an
|
|
86
|
+
# ownership boundary: its node stays where it was filed and its Base re-resolves.
|
|
87
|
+
and frame not in self._transform_index
|
|
88
|
+
]
|
|
89
|
+
if not new or owner.capped:
|
|
90
|
+
return
|
|
91
|
+
planned: list[tuple[str, str, str]] = []
|
|
92
|
+
for frame in new:
|
|
93
|
+
browse = member_browse_name((frame,))
|
|
94
|
+
folder = folder_of(frame, head=owner.binding.head, attach=attach)
|
|
95
|
+
taken = owner.claimed.get(browse)
|
|
96
|
+
if taken is not None or any(browse == other for _, other, _ in planned):
|
|
97
|
+
owner.refused.add(frame)
|
|
98
|
+
await self._report_frame_collision(owner, frame, browse, taken)
|
|
99
|
+
continue
|
|
100
|
+
planned.append((frame, browse, folder or INTERNAL_FRAMES))
|
|
101
|
+
if not planned:
|
|
102
|
+
return
|
|
103
|
+
if not await self._admit(owner, planned, parents=parents):
|
|
104
|
+
owner.capped = True
|
|
105
|
+
|
|
106
|
+
async def _admit(
|
|
107
|
+
self,
|
|
108
|
+
owner: _Owner,
|
|
109
|
+
planned: Sequence[tuple[str, str, str]],
|
|
110
|
+
*,
|
|
111
|
+
parents: Mapping[str, str],
|
|
112
|
+
) -> bool:
|
|
113
|
+
"""Put one tick's new Locations through the declaring item's chokepoint.
|
|
114
|
+
|
|
115
|
+
The declaring item's cap applies and its Notification gets the BindingEvents. The
|
|
116
|
+
Machine-wide tally counts too.
|
|
117
|
+
"""
|
|
118
|
+
folders = {name for _, _, name in planned} - owner.folders
|
|
119
|
+
|
|
120
|
+
async def build() -> Sequence[tuple[Node, ua.NodeId | int]]:
|
|
121
|
+
created = [
|
|
122
|
+
await self._open_location(owner, frame, browse, folder, parents=parents)
|
|
123
|
+
for frame, browse, folder in planned
|
|
124
|
+
]
|
|
125
|
+
return [*await self._opened_folders(owner, folders), *created]
|
|
126
|
+
|
|
127
|
+
return await owner.item.mutate_address_space_with(
|
|
128
|
+
binding=owner.binding.name,
|
|
129
|
+
path=self.ros2_transform_settings.path,
|
|
130
|
+
nodes=len(planned) + len(folders),
|
|
131
|
+
build=build,
|
|
132
|
+
)
|
|
133
|
+
|
|
134
|
+
async def _opened_folders(self, owner: _Owner, names: set[str]) -> list[tuple[Node, ua.NodeId | int]]:
|
|
135
|
+
"""Return the folder nodes opensmi created with a batch, for the model change.
|
|
136
|
+
|
|
137
|
+
opensmi creates a folder on first use, so it is found after the batch. Recording it keeps
|
|
138
|
+
later batches from counting or announcing it again.
|
|
139
|
+
"""
|
|
140
|
+
opened: list[tuple[Node, ua.NodeId | int]] = []
|
|
141
|
+
for name in sorted(names):
|
|
142
|
+
node = await owner.node.get_child(ua.QualifiedName(name, self._rsl_namespace))
|
|
143
|
+
opened.append((node, ObjectIds.FolderType))
|
|
144
|
+
owner.folders.add(name)
|
|
145
|
+
return opened
|
|
146
|
+
|
|
147
|
+
async def _file(self, owner: _Owner, folder: str, location: _Observed) -> None:
|
|
148
|
+
"""File one Location in one of the three frame folders of a SpatialObject.
|
|
149
|
+
|
|
150
|
+
``attach_points`` is used only for a real attach point, because 6.1.1 forbids using an
|
|
151
|
+
internal frame as one. The folder's ``add`` is used instead of ``add_internal_frame`` and
|
|
152
|
+
its siblings, because those build a plain ``CartesianFrame`` and a Location here is a
|
|
153
|
+
subclass.
|
|
154
|
+
"""
|
|
155
|
+
if folder == ATTACH_POINTS:
|
|
156
|
+
await owner.spatial.attach_points.add(location)
|
|
157
|
+
elif folder == ALTERNATIVE_FRAMES:
|
|
158
|
+
await owner.spatial.alternative_frames.add(location)
|
|
159
|
+
else:
|
|
160
|
+
await owner.spatial.internal_frames.add(location)
|
|
161
|
+
|
|
162
|
+
async def _open_location(
|
|
163
|
+
self,
|
|
164
|
+
owner: _Owner,
|
|
165
|
+
frame: str,
|
|
166
|
+
browse: str,
|
|
167
|
+
folder: str,
|
|
168
|
+
*,
|
|
169
|
+
parents: Mapping[str, str],
|
|
170
|
+
) -> tuple[Node, ua.NodeId | int]:
|
|
171
|
+
"""Create one Location and record it, with its Base already set.
|
|
172
|
+
|
|
173
|
+
A client therefore never sees the new Location at the World Frame.
|
|
174
|
+
"""
|
|
175
|
+
base = self._base_node(frame, parents=parents)
|
|
176
|
+
location = _Observed(name=browse, initial_pose=_ORIGIN, initial_base=base)
|
|
177
|
+
await self._file(owner, folder, location)
|
|
178
|
+
# The raw frame id goes in DisplayName (application specific, localizable). The BrowseName is the
|
|
179
|
+
# sanitised form. `Identifier` is the join key for multi-server aggregation.
|
|
180
|
+
await _write_display_name(location.ua_node, frame)
|
|
181
|
+
owner.claimed[browse] = frame
|
|
182
|
+
owner.frames[frame] = _Frame(frame=frame, browse=browse, location=location, folder=folder, base=base)
|
|
183
|
+
self._transform_index[frame] = location.ua_node.nodeid
|
|
184
|
+
return location.ua_node, self._location_type()
|
|
185
|
+
|
|
186
|
+
def _alternative_base(self, base: str, frames: Mapping[str, FrameLink]) -> ua.NodeId | None:
|
|
187
|
+
"""Return the node one alternative's base frame is, or ``None`` when it has none.
|
|
188
|
+
|
|
189
|
+
It is never re-based onto an ancestor, because the pose was composed against that frame. The
|
|
190
|
+
root of the TF tree is the list's World Frame.
|
|
191
|
+
"""
|
|
192
|
+
held = self._transform_index.get(base)
|
|
193
|
+
if held is not None:
|
|
194
|
+
return held
|
|
195
|
+
if base not in frames and any(link.parent == base for link in frames.values()):
|
|
196
|
+
return self._transform_world
|
|
197
|
+
return None
|
|
198
|
+
|
|
199
|
+
async def _open_alternative(
|
|
200
|
+
self,
|
|
201
|
+
owner: _Owner,
|
|
202
|
+
base: str,
|
|
203
|
+
frame: str,
|
|
204
|
+
node: ua.NodeId,
|
|
205
|
+
) -> _Frame | None:
|
|
206
|
+
"""Create one composed Location in ``AlternativeFrames``, through the chokepoint."""
|
|
207
|
+
browse = member_browse_name((frame, "in", base))
|
|
208
|
+
folders = {ALTERNATIVE_FRAMES} - owner.folders
|
|
209
|
+
|
|
210
|
+
async def build() -> Sequence[tuple[Node, ua.NodeId | int]]:
|
|
211
|
+
created = await self._open_composed(owner, base, frame, browse, node)
|
|
212
|
+
return [*await self._opened_folders(owner, folders), (created, self._location_type())]
|
|
213
|
+
|
|
214
|
+
made = await owner.item.mutate_address_space_with(
|
|
215
|
+
binding=owner.binding.name,
|
|
216
|
+
path=self.ros2_transform_settings.path,
|
|
217
|
+
nodes=1 + len(folders),
|
|
218
|
+
build=build,
|
|
219
|
+
)
|
|
220
|
+
return owner.alternatives.get((base, frame)) if made else None
|
|
221
|
+
|
|
222
|
+
async def _open_composed(
|
|
223
|
+
self,
|
|
224
|
+
owner: _Owner,
|
|
225
|
+
base: str,
|
|
226
|
+
frame: str,
|
|
227
|
+
browse: str,
|
|
228
|
+
node: ua.NodeId,
|
|
229
|
+
) -> Node:
|
|
230
|
+
"""Create the node one alternative is, and record it."""
|
|
231
|
+
location = _Observed(name=browse, initial_pose=_ORIGIN, initial_base=node)
|
|
232
|
+
await self._file(owner, ALTERNATIVE_FRAMES, location)
|
|
233
|
+
await _write_display_name(location.ua_node, f"{frame} in {base}")
|
|
234
|
+
owner.alternatives[(base, frame)] = _Frame(
|
|
235
|
+
frame=frame,
|
|
236
|
+
browse=browse,
|
|
237
|
+
location=location,
|
|
238
|
+
folder=ALTERNATIVE_FRAMES,
|
|
239
|
+
base=node,
|
|
240
|
+
)
|
|
241
|
+
return location.ua_node
|
|
242
|
+
|
|
243
|
+
def _base_node(self, frame: str, *, parents: Mapping[str, str]) -> ua.NodeId:
|
|
244
|
+
"""Return the node one Location's ``Base`` points at.
|
|
245
|
+
|
|
246
|
+
Delegates to :func:`~opensmi.ros2.server.spatial.base_node_of` and adds the frame index and
|
|
247
|
+
World Frame node of the live Machine.
|
|
248
|
+
"""
|
|
249
|
+
return base_node_of(
|
|
250
|
+
frame,
|
|
251
|
+
parents=parents,
|
|
252
|
+
index=self._transform_index,
|
|
253
|
+
world=self._require_world(),
|
|
254
|
+
)
|
|
255
|
+
|
|
256
|
+
def _require_world(self) -> ua.NodeId:
|
|
257
|
+
"""Return the list's World Frame node, which every Base terminates at.
|
|
258
|
+
|
|
259
|
+
:raises DeclarationError: When the mirror was never initialised, which is a Machine
|
|
260
|
+
with no spatial-object list and is refused long before this.
|
|
261
|
+
"""
|
|
262
|
+
if self._transform_world is None: # pragma: no cover -- init refuses this first
|
|
263
|
+
message = f"{type(self).__name__} has no World Frame; its spatial object list was never built"
|
|
264
|
+
raise DeclarationError(message)
|
|
265
|
+
return self._transform_world
|
|
266
|
+
|
|
267
|
+
@property
|
|
268
|
+
def _rsl_namespace(self) -> int:
|
|
269
|
+
"""Return the index this server imported OPC 10000-210's namespace at."""
|
|
270
|
+
return int(self.server.ua_get_namespace_index(RSLNodeIds))
|
|
271
|
+
|
|
272
|
+
def _location_type(self) -> ua.NodeId:
|
|
273
|
+
"""Return the NodeId of the type a Location is instantiated as, for a model change.
|
|
274
|
+
|
|
275
|
+
That is ``CartesianFrameAngleOrientationType``, the angle-based subtype opensmi's
|
|
276
|
+
``CartesianFrame`` is.
|
|
277
|
+
"""
|
|
278
|
+
return self.server.ua_get_node_id(RSLNodeIds.CartesianFrameAngleOrientationType)
|
|
279
|
+
|
|
280
|
+
async def _mark_constant(self, node: Node) -> None:
|
|
281
|
+
"""Set the constant access bit on one static frame's value, so nobody subscribes to it.
|
|
282
|
+
|
|
283
|
+
This reaches into asyncua's address space, because asyncua gives no node an
|
|
284
|
+
``AccessLevelEx`` attribute and writing one through ``Node`` fails with
|
|
285
|
+
``BadAttributeIdInvalid``. The bit goes on the Location, its ``Position`` and
|
|
286
|
+
``Orientation`` and their scalars, not on the ``Base``, which moves when the tree is
|
|
287
|
+
re-partitioned.
|
|
288
|
+
"""
|
|
289
|
+
space: Any = getattr(getattr(self.server.ua_server, "iserver", None), "aspace", None)
|
|
290
|
+
if space is None: # pragma: no cover -- every server this runs on has one
|
|
291
|
+
return
|
|
292
|
+
marked = [node]
|
|
293
|
+
for child in await node.get_children():
|
|
294
|
+
name = await child.read_browse_name()
|
|
295
|
+
if name.Name in ("Position", "Orientation"):
|
|
296
|
+
marked.extend((child, *await child.get_children()))
|
|
297
|
+
for each in marked:
|
|
298
|
+
held = space.get(each.nodeid)
|
|
299
|
+
if held is None:
|
|
300
|
+
continue
|
|
301
|
+
held.attributes[AttributeIds.AccessLevelEx] = AttributeValue(
|
|
302
|
+
ua.DataValue(ua.Variant(_CONSTANT, ua.VariantType.UInt32))
|
|
303
|
+
)
|
|
304
|
+
|
|
305
|
+
async def _report_frame_collision(self, owner: _Owner, frame: str, browse: str, taken: str | None) -> None:
|
|
306
|
+
"""Say, as a BindingEvent, that a frame has no Location because its name is taken. Once per frame."""
|
|
307
|
+
self.logger.error(
|
|
308
|
+
"Refused a frame whose browse name is already taken",
|
|
309
|
+
binding=owner.binding.name,
|
|
310
|
+
frame=frame,
|
|
311
|
+
browse_name=browse,
|
|
312
|
+
)
|
|
313
|
+
held = f"{taken!r} already holds" if taken else "is already taken in this tick"
|
|
314
|
+
await owner.item.emit_binding_event(
|
|
315
|
+
binding=owner.binding.name,
|
|
316
|
+
path=self.ros2_transform_settings.path,
|
|
317
|
+
code=FRAME_COLLISION,
|
|
318
|
+
severity=Severity.ERROR,
|
|
319
|
+
text=(
|
|
320
|
+
f"the frame {frame!r} browses as {browse!r}, which {held}; it has no Location, "
|
|
321
|
+
f"because sharing one would file two frames under one name"
|
|
322
|
+
),
|
|
323
|
+
)
|
|
324
|
+
|
|
325
|
+
async def _report_absent_base(self, owner: _Owner, base: str, frame: str) -> None:
|
|
326
|
+
"""Say, once, that a declared alternative has no base node to be relative to."""
|
|
327
|
+
if (base, frame) in owner.unresolved:
|
|
328
|
+
return
|
|
329
|
+
owner.unresolved.add((base, frame))
|
|
330
|
+
await owner.item.emit_binding_event(
|
|
331
|
+
binding=owner.binding.name,
|
|
332
|
+
path=self.ros2_transform_settings.path,
|
|
333
|
+
code=ALTERNATIVE_BASE_ABSENT,
|
|
334
|
+
severity=Severity.WARNING,
|
|
335
|
+
text=(
|
|
336
|
+
f"the alternative ({base!r}, {frame!r}) has a pose and no base: {base!r} is not a "
|
|
337
|
+
f"frame this Machine renders and is not the root of the tree, so there is no "
|
|
338
|
+
f"Location for it to be relative to"
|
|
339
|
+
),
|
|
340
|
+
)
|