修改udp广播的json格式

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

openarm_bringup/scripts/udp_recv_json.py

@@ -30,10 +30,24 @@ def print_packet(packet: Dict[str, Any]) -> None:
     stamp = packet.get("stamp", {})
     sec = stamp.get("sec", "?")
     nanosec = stamp.get("nanosec", "?")
+    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:
+
+    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)