4 Commits

4 changed files with 580 additions and 6 deletions
+275 -4
View File
@@ -1,16 +1,16 @@
# wpz_LbotArm 接口说明
# weipz-Lbot_Bridge 接口说明
本工程当前使用 `src/officer_sdk/lbot_driver` 作为机器人网口连接和 ROS2 bridge。启动脚本为:
```bash
cd ~/workspace/wpz_LbotArm/scripts
cd ~/workspace/weipz-Lbot_Bridge/scripts
./lbot_arm_control.sh start
```
停止服务:
```bash
cd ~/workspace/wpz_LbotArm/scripts
cd ~/workspace/weipz-Lbot_Bridge/scripts
./lbot_arm_control.sh stop
```
@@ -111,7 +111,7 @@ ros2 topic echo /rosout
也可以查看启动脚本日志:
```bash
tail -f ~/workspace/wpz_LbotArm/log/lbot_arm_control.log
tail -f ~/workspace/weipz-Lbot_Bridge/log/lbot_arm_control.log
```
如果后续需要稳定给外部软件使用的完整健康状态快照,建议在 `SystemError.msg` 之外再新增 `RobotHealth.msg`,并在连接/重连逻辑、使能/急停服务回调中维护并周期发布该状态。需要修改的位置:
@@ -247,6 +247,235 @@ ros2 topic pub --once /robot1/right_hand/set_l20_joint std_msgs/msg/Int32MultiAr
"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"
```
## Linker Hand L20 独立 CAN 控制
当前工程也集成了 `src/linker_hand_ros2_sdk`,用于在灵巧手单独接入域控 CAN 口时控制左右 L20 灵巧手,并获取关节状态和矩阵触觉合力反馈。
启动服务:
```bash
cd ~/workspace/weipz-Lbot_Bridge
./scripts/linker_hand_control.sh start
```
停止服务:
```bash
cd ~/workspace/weipz-Lbot_Bridge
./scripts/linker_hand_control.sh stop
```
查看状态:
```bash
./scripts/linker_hand_control.sh status
```
脚本当前会启动 `can0` 和 `can1`,波特率为 `1000000`。默认 launch 配置为:
```text
左手:can1,hand_type = left,hand_joint = L20
右手:can0,hand_type = right,hand_joint = L20
```
脚本会强制设置:
```text
ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
```
这样当前机器只发现本机 ROS 2 节点,避免同一局域网内旧的 `/linker_hand_sdk` 节点污染话题。
### 灵巧手控制话题
左手整手位置控制:
```text
/l20_left_hand_control_cmd
```
右手整手位置控制:
```text
/l20_right_hand_control_cmd
```
消息类型:
```text
sensor_msgs/msg/JointState
```
字段含义:
```text
position[] L20 目标关节位置数组,当前按 20 个 uint8 位置值使用,范围 0~255
velocity[] 可选速度数组;不需要设置速度时可以不填
effort[] 当前控制链路不使用
```
左手发布示例:
```bash
ros2 topic pub --once /l20_left_hand_control_cmd sensor_msgs/msg/JointState \
"{position: [205, 206, 230, 220, 226, 159, 127, 127, 32, 101, 80, 0, 0, 0, 0, 226, 254, 254, 248, 253]}"
```
右手发布示例:
```bash
ros2 topic pub --once /l20_right_hand_control_cmd sensor_msgs/msg/JointState \
"{position: [252, 147, 150, 136, 96, 255, 254, 202, 88, 0, 198, 0, 0, 0, 0, 254, 244, 241, 235, 211]}"
```
如果需要同时设置速度,可以填入 `velocity[]`:
```bash
ros2 topic pub --once /l20_right_hand_control_cmd sensor_msgs/msg/JointState \
"{position: [252, 147, 150, 136, 96, 255, 254, 202, 88, 0, 198, 0, 0, 0, 0, 254, 244, 241, 235, 211], velocity: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]}"
```
注意:这里的 `position[]` 不是弧度,是 Linker Hand CAN 协议中的 0~255 位置值。
### 灵巧手状态反馈
左手状态:
```text
/l20_left_hand_state
```
右手状态:
```text
/l20_right_hand_state
```
消息类型:
```text
sensor_msgs/msg/JointState
```
字段含义:
```text
name[] 关节名称
position[] 当前 L20 关节位置反馈,0~255
velocity[] 当前 L20 速度/速度阈值反馈,按 SDK 原始值发布
effort[] 当前未填有效力矩,通常为 0
```
订阅示例:
```bash
ros2 topic echo /l20_left_hand_state
ros2 topic echo /l20_right_hand_state
```
### 灵巧手矩阵触觉合力反馈
左手触觉合力:
```text
/l20_left_hand_matrix_touch_mass
```
右手触觉合力:
```text
/l20_right_hand_matrix_touch_mass
```
消息类型:
```text
std_msgs/msg/Float32MultiArray
```
字段含义:
```text
data[0] thumb_mass,拇指矩阵触觉合力
data[1] index_mass,食指矩阵触觉合力
data[2] middle_mass,中指矩阵触觉合力
data[3] ring_mass,无名指矩阵触觉合力
data[4] little_mass,小指矩阵触觉合力
```
订阅示例:
```bash
ros2 topic echo /l20_left_hand_matrix_touch_mass
ros2 topic echo /l20_right_hand_matrix_touch_mass
```
完整矩阵触觉和点云话题也会发布:
```text
/l20_left_hand_matrix_touch
/l20_left_hand_matrix_touch_pc
/l20_right_hand_matrix_touch
/l20_right_hand_matrix_touch_pc
```
其中 `matrix_touch` 当前仍是 JSON 字符串,`matrix_touch_pc` 为 `sensor_msgs/msg/PointCloud2`。
### 灵巧手配置指令
配置指令话题:
```text
/l20_hand_setting_cmd
```
消息类型:
```text
std_msgs/msg/String
```
该话题用于向 SDK 发送设置类 JSON 指令,例如清故障、读取故障、设置电流等。一般控制整手位置不需要使用该话题。
### 灵巧手 rosbag 录制
`linker_hand_control.sh start` 会同时启动 rosbag 录制,`stop` 会停止录制。每满 1 小时自动切一个新包。
录制文件命名:
```text
/tmp/L20_Hand_YYYYmmdd_HHMMSS.mcap
```
录制话题:
```text
/l20_left_hand_control_cmd
/l20_left_hand_state
/l20_left_hand_matrix_touch_mass
/l20_right_hand_control_cmd
/l20_right_hand_state
/l20_right_hand_matrix_touch_mass
```
录制日志:
```bash
tail -f ~/workspace/weipz-Lbot_Bridge/log/linker_hand_rosbag.log
```
查看录包进程状态:
```bash
./scripts/linker_hand_control.sh status
```
回放示例:
```bash
ros2 bag play /tmp/L20_Hand_YYYYmmdd_HHMMSS.mcap
```
## 相关代码位置
反馈发布:
@@ -286,3 +515,45 @@ ROS 接口定义:
```text
src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv
```
Linker Hand L20 独立 CAN 控制:
```text
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
scripts/linker_hand_control.sh
```
## 左右灵巧手 CAN 口对调
如果后续需要再次颠倒左右灵巧手的 CAN 口,只需要修改:
```text
src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
```
当前默认配置为:
```python
左手:'hand_type': 'left', 'can': 'can1'
右手:'hand_type': 'right', 'can': 'can0'
```
如果要左右 CAN 口对调,改成:
```python
左手:'hand_type': 'left', 'can': 'can0'
右手:'hand_type': 'right', 'can': 'can1'
```
修改后重新编译并重启服务:
```bash
cd ~/workspace/weipz-Lbot_Bridge
colcon build --packages-select linker_hand_ros2_sdk
source install/setup.bash
./scripts/linker_hand_control.sh stop
./scripts/linker_hand_control.sh start
```
注意:只改 `can` 字段,不要交换 `hand_type`。`hand_type` 决定 ROS 话题名和左右手设备 ID,交换它会导致 `/l20_left_*` 和 `/l20_right_*` 语义反掉。
@@ -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*
}],
),
+37
View File
@@ -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 并退出
```
+266
View File
@@ -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())