From 8aad6559b30bf84530e7c32f0d38f795e8b13d87 Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Tue, 31 Mar 2026 19:53:30 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9udp=E5=B9=BF=E6=92=AD?= =?UTF-8?q?=E7=9A=84json=E6=A0=BC=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/joint_state_udp_broadcaster.py | 98 ++++++++++++++++++- openarm_bringup/scripts/udp_recv_json.py | 28 ++++-- 2 files changed, 116 insertions(+), 10 deletions(-) diff --git a/openarm_bringup/scripts/joint_state_udp_broadcaster.py b/openarm_bringup/scripts/joint_state_udp_broadcaster.py index 1855048..f8625a8 100755 --- a/openarm_bringup/scripts/joint_state_udp_broadcaster.py +++ b/openarm_bringup/scripts/joint_state_udp_broadcaster.py @@ -37,12 +37,36 @@ class JointStateUdpBroadcaster(Node): "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 @@ -52,6 +76,9 @@ class JointStateUdpBroadcaster(Node): 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) @@ -67,7 +94,8 @@ class JointStateUdpBroadcaster(Node): 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"regex='{regex_str}', format='{self._payload_format}', " + f"ordered_joints={len(self._joint_order_map)}" ) def _joint_state_callback(self, msg: JointState) -> None: @@ -78,6 +106,7 @@ class JointStateUdpBroadcaster(Node): 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": { @@ -86,7 +115,9 @@ class JointStateUdpBroadcaster(Node): }, "frame_id": msg.header.frame_id, "joint_count": len(selected), - "joints": 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") @@ -104,13 +135,74 @@ class JointStateUdpBroadcaster(Node): out.append( { - "name": name, + "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 diff --git a/openarm_bringup/scripts/udp_recv_json.py b/openarm_bringup/scripts/udp_recv_json.py index 0dde5b9..af4ab7b 100755 --- a/openarm_bringup/scripts/udp_recv_json.py +++ b/openarm_bringup/scripts/udp_recv_json.py @@ -30,15 +30,29 @@ def print_packet(packet: Dict[str, Any]) -> None: stamp = packet.get("stamp", {}) sec = stamp.get("sec", "?") nanosec = stamp.get("nanosec", "?") - joints = packet.get("joints", []) + if "joints" in packet: + joints = packet.get("joints", []) + left_arm = [j for j in joints if "_left_" in str(j.get("name", ""))] + right_arm = [j for j in joints if "_right_" in str(j.get("name", ""))] + other = [j for j in joints if j not in left_arm and j not in right_arm] + else: + left_arm = packet.get("left_arm", []) + right_arm = packet.get("right_arm", []) + other = packet.get("other", []) + joints = left_arm + right_arm + other print(f"protocol={protocol} stamp={sec}.{nanosec} joints={len(joints)}") - for joint in joints: - name = joint.get("name", "?") - pos = joint.get("position", None) - vel = joint.get("velocity", None) - tor = joint.get("torque", None) - print(f" {name:28s} pos={pos!s:>12s} vel={vel!s:>12s} tau={tor!s:>12s}") + + for group_name, group_joints in (("left_arm", left_arm), ("right_arm", right_arm), ("other", other)): + if not group_joints: + continue + print(f"{group_name}:") + for joint in group_joints: + name = joint.get("name", "?") + pos = joint.get("position", None) + vel = joint.get("velocity", None) + tor = joint.get("torque", None) + print(f" {name:28s} pos={pos!s:>12s} vel={vel!s:>12s} tau={tor!s:>12s}") def main() -> int: