#!/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(" 255: name_bytes = name_bytes[:255] packet.extend(struct.pack(" 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()