#!/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())