openarm_ros2/openarm_bringup/scripts/joint_state_udp_broadcaster.py

#!/usr/bin/env python3

# Copyright 2026 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import json
import math
import re
import socket
import struct
from typing import Any, Dict, List, Optional

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState


class JointStateUdpBroadcaster(Node):
    """Broadcast selected joint states over UDP as JSON."""

    def __init__(self) -> None:
        super().__init__("joint_state_udp_broadcaster")

        self.declare_parameter("input_topic", "joint_states")
        self.declare_parameter(
            "joint_name_regex",
            r"^openarm_(left|right)_(joint[1-7]|finger_joint1)$",
        )
        self.declare_parameter(
            "joint_order",
            [
                "openarm_left_joint1",
                "openarm_left_joint2",
                "openarm_left_joint3",
                "openarm_left_joint4",
                "openarm_left_joint5",
                "openarm_left_joint6",
                "openarm_left_joint7",
                "openarm_left_finger_joint1",
                "openarm_right_joint1",
                "openarm_right_joint2",
                "openarm_right_joint3",
                "openarm_right_joint4",
                "openarm_right_joint5",
                "openarm_right_joint6",
                "openarm_right_joint7",
                "openarm_right_finger_joint1",
            ],
        )
        self.declare_parameter("broadcast_ip", "255.255.255.255")
        self.declare_parameter("broadcast_port", 10001)
        self.declare_parameter("payload_format", "json")

        input_topic = self.get_parameter("input_topic").get_parameter_value().string_value
        regex_str = self.get_parameter("joint_name_regex").get_parameter_value().string_value
        joint_order = list(
            self.get_parameter("joint_order").get_parameter_value().string_array_value
        )
        broadcast_ip = self.get_parameter("broadcast_ip").get_parameter_value().string_value
        broadcast_port = self.get_parameter("broadcast_port").get_parameter_value().integer_value
        payload_format = self.get_parameter("payload_format").get_parameter_value().string_value

        self._payload_format = payload_format.lower().strip()
        if self._payload_format not in ("json", "binary"):
            raise ValueError("Parameter 'payload_format' must be 'json' or 'binary'.")

        self._joint_name_pattern = re.compile(regex_str)
        self._joint_order_map = {
            name: idx for idx, name in enumerate(name.strip() for name in joint_order if name.strip())
        }

        self._udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self._udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
        self._target = (broadcast_ip, int(broadcast_port))

        self._subscription = self.create_subscription(
            JointState,
            input_topic,
            self._joint_state_callback,
            50,
        )

        self.get_logger().info(
            "UDP joint state broadcast started: "
            f"topic='{input_topic}', target={broadcast_ip}:{broadcast_port}, "
            f"regex='{regex_str}', format='{self._payload_format}', "
            f"ordered_joints={len(self._joint_order_map)}"
        )

    def _joint_state_callback(self, msg: JointState) -> None:
        selected = self._select_joint_states(msg)
        if not selected:
            return

        if self._payload_format == "binary":
            data = self._build_binary_packet(msg, selected)
        else:
            grouped = self._group_joints_by_arm(selected)
            payload = {
                "protocol": "openarm_joint_state_json_v1",
                "stamp": {
                    "sec": int(msg.header.stamp.sec),
                    "nanosec": int(msg.header.stamp.nanosec),
                },
                "frame_id": msg.header.frame_id,
                "joint_count": len(selected),
                "left_arm": self._public_joint_list(grouped["left_arm"]),
                "right_arm": self._public_joint_list(grouped["right_arm"]),
                "other": self._public_joint_list(grouped["other"]),
            }
            data = json.dumps(payload, separators=(",", ":")).encode("utf-8")

        try:
            self._udp_socket.sendto(data, self._target)
        except OSError as exc:
            self.get_logger().error(f"Failed to send UDP packet: {exc}")

    def _select_joint_states(self, msg: JointState) -> List[Dict[str, Any]]:
        out: List[Dict[str, Any]] = []

        for index, name in enumerate(msg.name):
            if not self._joint_name_pattern.match(name):
                continue

            out.append(
                {
                    "name": self._alias_joint_name(name),
                    "source_name": name,
                    "index": index,
                    "position": self._array_value(msg.position, index),
                    "velocity": self._array_value(msg.velocity, index),
                    "torque": self._array_value(msg.effort, index),
                }
            )

        out.sort(key=self._joint_sort_key)

        return out

    def _joint_sort_key(self, joint: Dict[str, Any]) -> Any:
        source_name = str(joint.get("source_name", ""))
        name = str(joint.get("name", ""))
        if source_name in self._joint_order_map:
            return (0, self._joint_order_map[source_name])
        if name in self._joint_order_map:
            return (0, self._joint_order_map[name])
        return (1, name, int(joint.get("index", 0)))

    @staticmethod
    def _alias_joint_name(name: str) -> str:
        match_joint = re.match(r"^openarm_(left|right)_joint([1-7])$", name)
        if match_joint:
            return f"joint_{match_joint.group(2)}"

        match_gripper = re.match(r"^openarm_(left|right)_finger_joint1$", name)
        if match_gripper:
            return "gripper"

        return name

    @staticmethod
    def _group_joints_by_arm(selected: List[Dict[str, Any]]) -> Dict[str, List[Dict[str, Any]]]:
        grouped: Dict[str, List[Dict[str, Any]]] = {
            "left_arm": [],
            "right_arm": [],
            "other": [],
        }

        for joint in selected:
            source_name = str(joint.get("source_name", joint.get("name", "")))
            name = str(joint.get("name", ""))
            ref_name = source_name if source_name else name
            if "_left_" in ref_name:
                grouped["left_arm"].append(joint)
            elif "_right_" in ref_name:
                grouped["right_arm"].append(joint)
            else:
                grouped["other"].append(joint)

        return grouped

    @staticmethod
    def _public_joint_list(joints: List[Dict[str, Any]]) -> List[Dict[str, Any]]:
        out: List[Dict[str, Any]] = []
        for joint in joints:
            out.append(
                {
                    "name": joint.get("name"),
                    "index": joint.get("index"),
                    "position": joint.get("position"),
                    "velocity": joint.get("velocity"),
                    "torque": joint.get("torque"),
                }
            )
        return out

    @staticmethod
    def _array_value(values: List[float], index: int) -> Optional[float]:
        if index < len(values):
            return float(values[index])
        return None

    def _build_binary_packet(
        self,
        msg: JointState,
        selected: List[Dict[str, Any]],
    ) -> bytes:
        # Packet layout (little-endian):
        # magic(4='OAJS') version(u8=1) joint_count(u16) sec(i64) nanosec(u32)
        # repeated joint entries:
        # name_len(u8) name(bytes) position(f32) velocity(f32) torque(f32)
        packet = bytearray()
        packet.extend(b"OAJS")
        packet.extend(struct.pack("<B", 1))
        packet.extend(struct.pack("<H", len(selected)))
        packet.extend(struct.pack("<q", int(msg.header.stamp.sec)))
        packet.extend(struct.pack("<I", int(msg.header.stamp.nanosec)))

        for joint in selected:
            name_bytes = str(joint["name"]).encode("utf-8")
            if len(name_bytes) > 255:
                name_bytes = name_bytes[:255]
            packet.extend(struct.pack("<B", len(name_bytes)))
            packet.extend(name_bytes)
            packet.extend(struct.pack("<f", self._to_f32(joint["position"])))
            packet.extend(struct.pack("<f", self._to_f32(joint["velocity"])))
            packet.extend(struct.pack("<f", self._to_f32(joint["torque"])))

        return bytes(packet)

    @staticmethod
    def _to_f32(value: Optional[float]) -> float:
        if value is None:
            return float("nan")
        if isinstance(value, float) and math.isnan(value):
            return value
        return float(value)

    def destroy_node(self) -> bool:
        try:
            self._udp_socket.close()
        except OSError:
            pass
        return super().destroy_node()


def main(args=None) -> None:
    rclpy.init(args=args)
    node = JointStateUdpBroadcaster()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()