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()