273 lines
9.6 KiB
Python
Executable File
273 lines
9.6 KiB
Python
Executable File
#!/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"jont_{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()
|