imetro_behavior.joint_behaviors

 1# Copyright (c) 2026, United States Government, as represented by the
 2# Administrator of the National Aeronautics and Space Administration.
 3#
 4# All rights reserved.
 5#
 6# This software is licensed under the Apache License, Version 2.0
 7# (the "License"); you may not use this file except in compliance with the
 8# License. You may obtain a copy of the License at
 9#
10#     http://www.apache.org/licenses/LICENSE-2.0
11#
12# Unless required by applicable law or agreed to in writing, software
13# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15# License for the specific language governing permissions and limitations
16# under the License.
17
18import yaml
19from ament_index_python.packages import get_package_share_path
20from py_trees.common import Status
21from py_trees.ports import BehaviourWithPorts, PortInformation
22
23from imetro_behavior.helpers import set_ros_node
24
25
26class JointNamesAndPositionsFromYaml(BehaviourWithPorts):
27    """Loads joint names and positions from a YAML file.
28
29    Valid yaml configuration for a joint state:
30
31    state_name:
32        joint_names:
33            - joint_1
34            - joint_2
35            - joint_3
36        positions:
37            - 0.1
38            - 0.2
39            - 0.3
40
41    No validation is done for the respective lengths of the names and positions here,
42    as these may not match up exactly in the event of multi-DOF joints.
43    Downstream consumers should do their own validation based on what they support.
44    """
45
46    INPUT_PORTS = {
47        "package_name": PortInformation(data_type=str, required=True),
48        "yaml_file": PortInformation(data_type=str, required=True),
49        "state_name": PortInformation(data_type=str, required=True),
50    }
51
52    OUTPUT_PORTS = {
53        "joint_names": PortInformation(data_type=list[str], required=True),
54        "joint_positions": PortInformation(data_type=list[float], required=True),
55    }
56
57    def setup(self, **kwargs):
58        """Get access to the node for error statements."""
59        set_ros_node(self, **kwargs)
60
61    def update(self) -> Status:
62        """Load the YAML file and set the joint name and positions as an output port."""
63        yaml_path = get_package_share_path(self.get_input("package_name")) / self.get_input("yaml_file")
64        if not yaml_path.is_file():
65            self.logger.error(f"File at {yaml_path} could not be found or is not a file")
66            return Status.FAILURE
67
68        with open(yaml_path) as file:
69            data = yaml.safe_load(file)
70        state_name = self.get_input("state_name")
71        joint_dict = data.get(state_name)
72        if joint_dict is None:
73            self.logger.error(f"Failed to find joint configuration {state_name} in {yaml_path}")
74            return Status.FAILURE
75
76        joint_names = joint_dict.get("joint_names")
77        if joint_names is None:
78            self.logger.error(f"Joint configuration '{state_name}' does not have a 'joint_names' field.")
79            return Status.FAILURE
80
81        joint_positions = joint_dict.get("positions")
82        if joint_positions is None:
83            self.logger.error(f"Joint configuration '{state_name}' does not have a 'positions' field.")
84            return Status.FAILURE
85
86        self._set_output("joint_names", joint_names)
87        self._set_output("joint_positions", joint_positions)
88        return Status.SUCCESS