Compare commits
4 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 3d44400521 | |||
| 6427597624 | |||
| 14fa685f7f | |||
| 3ac6498bcb |
@@ -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*
|
||||
}],
|
||||
),
|
||||
|
||||
@@ -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 并退出
|
||||
```
|
||||
Executable
+266
@@ -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())
|
||||
Reference in New Issue
Block a user