267 lines
11 KiB
Python
Executable File
267 lines
11 KiB
Python
Executable File
#!/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())
|