From 6427597624d1e9f423b611e5e6df30edd86b6129 Mon Sep 17 00:00:00 2001 From: weipengzhe <3389096668@qq.com> Date: Sun, 20 Sep 2026 13:39:08 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E6=B5=8B=E8=AF=95=E8=84=9A?= =?UTF-8?q?=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/linker_hand_double.launch.py | 4 +- test_tools/README.md | 37 +++ test_tools/l20_long_run_test.py | 266 ++++++++++++++++++ 3 files changed, 305 insertions(+), 2 deletions(-) create mode 100644 test_tools/README.md create mode 100755 test_tools/l20_long_run_test.py diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py index 4f50f53..f5c283a 100644 --- a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py +++ b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写 'hand_joint': "L20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False - 'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 + 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* }], ), @@ -28,7 +28,7 @@ def generate_launch_description(): 'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写 'hand_joint': "L20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False - 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 + 'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1 "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* }], ), diff --git a/test_tools/README.md b/test_tools/README.md new file mode 100644 index 0000000..5a5301d --- /dev/null +++ b/test_tools/README.md @@ -0,0 +1,37 @@ +# L20 Dual Arm Long-Run Test + +该目录下的 `l20_long_run_test.py` 是独立长测工具，只负责发布/调用动作接口，不负责启动机器人、机械臂 driver、灵巧手 driver、CAN 或 rosbag。 + +先启动机器人相关服务后，再运行： + +```bash +cd ~/workspace/weipz-Lbot_Bridge +source install/setup.bash +python3 test_tools/l20_long_run_test.py +``` + +有限循环示例，执行 10 轮动作 1/动作 2： + +```bash +python3 test_tools/l20_long_run_test.py --cycles 10 +``` + +常用参数： + +```text +--cycles 动作 1/动作 2 的循环轮数，0 表示无限循环 +--arm-speed MoveJ 速度标量，默认 1.0 +--arm-acce MoveJ 加速度标量，默认 1.0 +--arm-tolerance 机械臂关节到位容差，单位 rad，默认 0.03 +--action-timeout 单个动作到位超时时间，默认 30 s +--hand-publish-count 灵巧手开环命令每个动作重复发布次数，默认 5 +--hand-publish-rate 灵巧手开环命令发布频率，默认 20 Hz +``` + +判定逻辑： + +```text +灵巧手：开环发布 /l20_left_hand_control_cmd 和 /l20_right_hand_control_cmd，不做反馈校验 +机械臂：调用 /robot1/left_arm/move_joint 和 /robot1/right_arm/move_joint，再订阅 joint_states 判断是否到位 +错误：收到 /robot1/system_error、MoveJ 返回失败、服务超时、joint_states 超时或机械臂一直不到位时打印 ERROR 并退出 +``` diff --git a/test_tools/l20_long_run_test.py b/test_tools/l20_long_run_test.py new file mode 100755 index 0000000..1c0d7d2 --- /dev/null +++ b/test_tools/l20_long_run_test.py @@ -0,0 +1,266 @@ +#!/usr/bin/env python3 +"""Long-run robustness test for LBot dual arms and L20 hands. + +This tool only publishes/calls existing ROS interfaces. It does not start or +stop robot drivers, CAN devices, or rosbag recorders. +""" + +import argparse +import math +import signal +import sys +import time +from dataclasses import dataclass +from typing import Dict, List, Optional, Tuple + +import rclpy +from lbot_arm_interfaces.msg import SystemError +from lbot_arm_interfaces.srv import MoveJ +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +LEFT_HAND_ACTION_1 = [252, 253, 239, 237, 234, 255, 245, 190, 112, 40, 228, 0, 0, 0, 0, 254, 247, 252, 253, 252] +LEFT_HAND_ACTION_2 = [252, 194, 203, 193, 194, 254, 195, 165, 118, 116, 246, 0, 0, 0, 0, 249, 254, 254, 254, 254] + +RIGHT_HAND_ACTION_1 = [252, 194, 203, 193, 194, 254, 195, 165, 118, 116, 246, 0, 0, 0, 0, 249, 254, 254, 254, 254] +RIGHT_HAND_ACTION_2 = [252, 1, 1, 0, 0, 65, 133, 133, 127, 129, 137, 0, 0, 0, 0, 249, 197, 195, 211, 215] + +LEFT_ARM_ACTION_1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +LEFT_ARM_ACTION_2 = [0.5, -0.5, 0.5, 0.5, 0.5, 0.5, 0.5] + +RIGHT_ARM_ACTION_1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +RIGHT_ARM_ACTION_2 = [-0.5, 0.5, -0.5, -0.5, -0.5, -0.5, -0.5] + + +@dataclass(frozen=True) +class TestAction: + name: str + left_hand: List[int] + right_hand: List[int] + left_arm: List[float] + right_arm: List[float] + + +ACTIONS = [ + TestAction("action_1", LEFT_HAND_ACTION_1, RIGHT_HAND_ACTION_1, LEFT_ARM_ACTION_1, RIGHT_ARM_ACTION_1), + TestAction("action_2", LEFT_HAND_ACTION_2, RIGHT_HAND_ACTION_2, LEFT_ARM_ACTION_2, RIGHT_ARM_ACTION_2), +] + + +class L20LongRunTester(Node): + def __init__(self, args: argparse.Namespace): + super().__init__("l20_long_run_test") + self.args = args + self.shutdown_requested = False + self.last_system_error: Optional[SystemError] = None + self.arm_states: Dict[str, Tuple[List[float], float]] = {} + + self.left_hand_pub = self.create_publisher(JointState, "/l20_left_hand_control_cmd", 10) + self.right_hand_pub = self.create_publisher(JointState, "/l20_right_hand_control_cmd", 10) + + self.left_move_client = self.create_client(MoveJ, "/robot1/left_arm/move_joint") + self.right_move_client = self.create_client(MoveJ, "/robot1/right_arm/move_joint") + + self.create_subscription(JointState, "/robot1/left_arm/joint_states", self._left_arm_state_cb, 10) + self.create_subscription(JointState, "/robot1/right_arm/joint_states", self._right_arm_state_cb, 10) + self.create_subscription(SystemError, "/robot1/system_error", self._system_error_cb, 10) + + def request_shutdown(self) -> None: + self.shutdown_requested = True + + def _left_arm_state_cb(self, msg: JointState) -> None: + self.arm_states["left"] = (list(msg.position), time.monotonic()) + + def _right_arm_state_cb(self, msg: JointState) -> None: + self.arm_states["right"] = (list(msg.position), time.monotonic()) + + def _system_error_cb(self, msg: SystemError) -> None: + if msg.error_code != 0 or not msg.connected: + self.last_system_error = msg + + def wait_until_ready(self) -> bool: + self._info("waiting for MoveJ services...") + if not self.left_move_client.wait_for_service(timeout_sec=self.args.service_timeout): + self._error("left MoveJ service is not available") + return False + if not self.right_move_client.wait_for_service(timeout_sec=self.args.service_timeout): + self._error("right MoveJ service is not available") + return False + + self._info("waiting for arm joint state feedback...") + deadline = time.monotonic() + self.args.state_timeout + while time.monotonic() < deadline and not self.shutdown_requested: + rclpy.spin_once(self, timeout_sec=0.05) + if "left" in self.arm_states and "right" in self.arm_states: + self._info("joint state feedback is ready") + return True + self._error("joint state feedback timeout") + return False + + def execute_action(self, action: TestAction, cycle_index: int) -> bool: + self.last_system_error = None + self._info(f"cycle={cycle_index} start {action.name}") + + self.publish_hands(action.left_hand, action.right_hand) + + if not self.call_movej("left", self.left_move_client, action.left_arm): + return False + if not self.call_movej("right", self.right_move_client, action.right_arm): + return False + + reached = self.wait_arms_reached(action.left_arm, action.right_arm) + if not reached: + return False + + self._info(f"cycle={cycle_index} reached {action.name}") + return True + + def publish_hands(self, left_position: List[int], right_position: List[int]) -> None: + left_msg = JointState() + left_msg.position = [float(value) for value in left_position] + right_msg = JointState() + right_msg.position = [float(value) for value in right_position] + + for _ in range(self.args.hand_publish_count): + now_msg = self.get_clock().now().to_msg() + left_msg.header.stamp = now_msg + right_msg.header.stamp = now_msg + self.left_hand_pub.publish(left_msg) + self.right_hand_pub.publish(right_msg) + rclpy.spin_once(self, timeout_sec=0.0) + time.sleep(1.0 / self.args.hand_publish_rate) + + self._info("hand commands published open-loop") + + def call_movej(self, side: str, client, joints: List[float]) -> bool: + req = MoveJ.Request() + req.joints = [float(value) for value in joints] + req.speed = float(self.args.arm_speed) + req.acce = float(self.args.arm_acce) + req.block = False + + self._info(f"send {side} arm MoveJ joints={self._fmt(joints)} speed={req.speed:.3f} acce={req.acce:.3f}") + future = client.call_async(req) + deadline = time.monotonic() + self.args.service_timeout + while time.monotonic() < deadline and not self.shutdown_requested: + rclpy.spin_once(self, timeout_sec=0.05) + if future.done(): + try: + response = future.result() + except Exception as exc: # noqa: BLE001 - log ROS service failure directly. + self._error(f"{side} arm MoveJ service exception: {exc}") + return False + if not response.success: + self._error(f"{side} arm MoveJ returned success=false") + return False + return True + self._error(f"{side} arm MoveJ service response timeout") + return False + + def wait_arms_reached(self, left_target: List[float], right_target: List[float]) -> bool: + deadline = time.monotonic() + self.args.action_timeout + next_log_time = 0.0 + + while time.monotonic() < deadline and not self.shutdown_requested: + rclpy.spin_once(self, timeout_sec=0.05) + + if self.last_system_error is not None: + err = self.last_system_error + self._error( + f"system error received: code={err.error_code}, connected={err.connected}, msg={err.error_msg}" + ) + return False + + left_ok, left_err = self._arm_reached("left", left_target) + right_ok, right_err = self._arm_reached("right", right_target) + if left_ok and right_ok: + return True + + now = time.monotonic() + if now >= next_log_time: + self._info(f"waiting reach: left_max_err={left_err:.4f}, right_max_err={right_err:.4f}") + next_log_time = now + self.args.progress_period + + left_ok, left_err = self._arm_reached("left", left_target) + right_ok, right_err = self._arm_reached("right", right_target) + self._error( + f"arm reach timeout: left_ok={left_ok}, left_max_err={left_err:.4f}, " + f"right_ok={right_ok}, right_max_err={right_err:.4f}" + ) + return False + + def _arm_reached(self, side: str, target: List[float]) -> Tuple[bool, float]: + state = self.arm_states.get(side) + if state is None: + return False, math.inf + positions, stamp = state + if time.monotonic() - stamp > self.args.state_stale_timeout: + return False, math.inf + if len(positions) < len(target): + return False, math.inf + max_error = max(abs(float(current) - float(goal)) for current, goal in zip(positions[: len(target)], target)) + return max_error <= self.args.arm_tolerance, max_error + + @staticmethod + def _fmt(values: List[float]) -> str: + return "[" + ", ".join(f"{value:.3f}" for value in values) + "]" + + def _info(self, msg: str) -> None: + print(f"[{time.strftime('%Y-%m-%d %H:%M:%S')}] INFO {msg}", flush=True) + + def _error(self, msg: str) -> None: + print(f"[{time.strftime('%Y-%m-%d %H:%M:%S')}] ERROR {msg}", flush=True) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Long-run alternating action test for dual arms and L20 hands.") + parser.add_argument("--cycles", type=int, default=0, help="number of action pairs to run; 0 means infinite") + parser.add_argument("--arm-speed", type=float, default=1.0, help="MoveJ speed scalar") + parser.add_argument("--arm-acce", type=float, default=1.0, help="MoveJ acceleration scalar") + parser.add_argument("--arm-tolerance", type=float, default=0.03, help="arm joint reach tolerance in rad") + parser.add_argument("--action-timeout", type=float, default=30.0, help="seconds to wait for both arms to reach one action") + parser.add_argument("--service-timeout", type=float, default=5.0, help="seconds to wait for services and service responses") + parser.add_argument("--state-timeout", type=float, default=5.0, help="seconds to wait for initial joint state feedback") + parser.add_argument("--state-stale-timeout", type=float, default=1.0, help="joint state older than this is treated as stale") + parser.add_argument("--progress-period", type=float, default=1.0, help="seconds between progress logs while waiting") + parser.add_argument("--hand-publish-count", type=int, default=5, help="open-loop hand command publish repeats per action") + parser.add_argument("--hand-publish-rate", type=float, default=20.0, help="open-loop hand command publish rate in Hz") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + rclpy.init() + node = L20LongRunTester(args) + + def _signal_handler(_signum, _frame): + node.request_shutdown() + + signal.signal(signal.SIGINT, _signal_handler) + signal.signal(signal.SIGTERM, _signal_handler) + + try: + if not node.wait_until_ready(): + return 1 + + cycle = 1 + while rclpy.ok() and not node.shutdown_requested: + for action in ACTIONS: + if node.shutdown_requested: + break + if not node.execute_action(action, cycle): + return 2 + cycle += 1 + if args.cycles > 0 and cycle > args.cycles: + node._info("requested cycles completed") + return 0 + finally: + node.destroy_node() + rclpy.shutdown() + + return 0 + + +if __name__ == "__main__": + sys.exit(main())