From 6f163bff004634c5460b9c0ac010942964eb8cfb Mon Sep 17 00:00:00 2001
From: shen <664376944@qq.com>
Date: Mon, 30 Mar 2026 15:54:20 +0800
Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=90=84=E4=B8=AA=E5=85=B3?=
=?UTF-8?q?=E8=8A=82=E7=8A=B6=E6=80=81=E9=80=9A=E8=BF=87udp=E5=8F=91?=
=?UTF-8?q?=E5=B8=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
openarm_bringup/CMakeLists.txt | 1 +
.../launch/openarm.bimanual.launch.py | 50 +++++
openarm_bringup/package.xml | 1 +
.../scripts/joint_state_udp_broadcaster.py | 180 ++++++++++++++++++
4 files changed, 232 insertions(+)
create mode 100755 openarm_bringup/scripts/joint_state_udp_broadcaster.py
diff --git a/openarm_bringup/CMakeLists.txt b/openarm_bringup/CMakeLists.txt
index fc904e9..7132ab5 100644
--- a/openarm_bringup/CMakeLists.txt
+++ b/openarm_bringup/CMakeLists.txt
@@ -43,6 +43,7 @@ install(DIRECTORY launch config
install(PROGRAMS
scripts/motor_disable_service.py
+ scripts/joint_state_udp_broadcaster.py
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py
index fa8297e..93991bc 100644
--- a/openarm_bringup/launch/openarm.bimanual.launch.py
+++ b/openarm_bringup/launch/openarm.bimanual.launch.py
@@ -20,6 +20,7 @@ from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction, OpaqueFunction
+from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
LaunchConfiguration,
@@ -306,6 +307,32 @@ def generate_launch_description():
default_value="",
description="Tip link override for KDL chain. Empty uses arm-specific link7.",
),
+ DeclareLaunchArgument(
+ "enable_joint_state_udp_broadcast",
+ default_value="false",
+ description="Enable UDP broadcast for arm joint position/velocity/torque.",
+ ),
+ DeclareLaunchArgument(
+ "joint_state_udp_broadcast_ip",
+ default_value="255.255.255.255",
+ description="UDP target IP for joint state broadcast (broadcast or unicast).",
+ ),
+ DeclareLaunchArgument(
+ "joint_state_udp_broadcast_port",
+ default_value="10001",
+ description="UDP target port for joint state broadcast.",
+ ),
+ DeclareLaunchArgument(
+ "joint_state_udp_joint_regex",
+ default_value="^openarm_(left|right)_(joint[1-7]|finger_joint1)$",
+ description="Regex to select joint names included in UDP payload.",
+ ),
+ DeclareLaunchArgument(
+ "joint_state_udp_payload_format",
+ default_value="json",
+ choices=["json", "binary"],
+ description="UDP payload format for joint state broadcast.",
+ ),
]
# Initialize launch configurations
@@ -333,6 +360,11 @@ def generate_launch_description():
kdl_urdf_path = LaunchConfiguration("kdl_urdf_path")
kdl_base_link = LaunchConfiguration("kdl_base_link")
kdl_tip_link = LaunchConfiguration("kdl_tip_link")
+ enable_joint_state_udp_broadcast = LaunchConfiguration("enable_joint_state_udp_broadcast")
+ joint_state_udp_broadcast_ip = LaunchConfiguration("joint_state_udp_broadcast_ip")
+ joint_state_udp_broadcast_port = LaunchConfiguration("joint_state_udp_broadcast_port")
+ joint_state_udp_joint_regex = LaunchConfiguration("joint_state_udp_joint_regex")
+ joint_state_udp_payload_format = LaunchConfiguration("joint_state_udp_payload_format")
controllers_file = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config",
@@ -397,6 +429,23 @@ def generate_launch_description():
args=[arm_prefix],
)
+ joint_state_udp_broadcaster_node = OpaqueFunction(
+ function=lambda context: [Node(
+ package="openarm_bringup",
+ executable="joint_state_udp_broadcaster.py",
+ namespace=namespace_from_context(context, arm_prefix),
+ output="screen",
+ condition=IfCondition(enable_joint_state_udp_broadcast),
+ parameters=[{
+ "input_topic": "joint_states",
+ "broadcast_ip": joint_state_udp_broadcast_ip,
+ "broadcast_port": joint_state_udp_broadcast_port,
+ "joint_name_regex": joint_state_udp_joint_regex,
+ "payload_format": joint_state_udp_payload_format,
+ }],
+ )]
+ )
+
# Timing and sequencing
LAUNCH_DELAY_SECONDS = 1.0
delayed_joint_state_broadcaster = TimerAction(
@@ -418,6 +467,7 @@ def generate_launch_description():
robot_nodes_spawner_func,
rviz_node,
motor_disable_service_spawner_func,
+ joint_state_udp_broadcaster_node,
] +
[
delayed_joint_state_broadcaster,
diff --git a/openarm_bringup/package.xml b/openarm_bringup/package.xml
index d344058..3d7bd4c 100644
--- a/openarm_bringup/package.xml
+++ b/openarm_bringup/package.xml
@@ -27,6 +27,7 @@
ros2_controllers
controller_manager
rclpy
+ sensor_msgs
std_srvs
controller_manager_msgs
lifecycle_msgs
diff --git a/openarm_bringup/scripts/joint_state_udp_broadcaster.py b/openarm_bringup/scripts/joint_state_udp_broadcaster.py
new file mode 100755
index 0000000..1855048
--- /dev/null
+++ b/openarm_bringup/scripts/joint_state_udp_broadcaster.py
@@ -0,0 +1,180 @@
+#!/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("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
+ 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._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}'"
+ )
+
+ 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:
+ 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),
+ "joints": selected,
+ }
+ 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": name,
+ "position": self._array_value(msg.position, index),
+ "velocity": self._array_value(msg.velocity, index),
+ "torque": self._array_value(msg.effort, index),
+ }
+ )
+
+ 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()