526 lines
11 KiB
Markdown
526 lines
11 KiB
Markdown
# weipz-Lbot_Bridge 接口说明
|
||
|
||
本工程当前使用 `src/officer_sdk/lbot_driver` 作为机器人网口连接和 ROS2 bridge。启动脚本为：
|
||
|
||
```bash
|
||
cd ~/workspace/weipz-Lbot_Bridge/scripts
|
||
./lbot_arm_control.sh start
|
||
```
|
||
|
||
停止服务：
|
||
|
||
```bash
|
||
cd ~/workspace/weipz-Lbot_Bridge/scripts
|
||
./lbot_arm_control.sh stop
|
||
```
|
||
|
||
默认启动的机器人 namespace 是 `/robot1`，默认连接控制器 IP 为 `192.168.10.21`。
|
||
|
||
## 机械臂反馈订阅
|
||
|
||
外部软件如果需要订阅机器人当前机械臂各关节角度、速度、电流/力矩，使用下面两个 topic：
|
||
|
||
```text
|
||
/robot1/left_arm/joint_states
|
||
/robot1/right_arm/joint_states
|
||
```
|
||
|
||
消息类型：
|
||
|
||
```text
|
||
sensor_msgs/msg/JointState
|
||
```
|
||
|
||
字段含义：
|
||
|
||
```text
|
||
name[] 关节名称
|
||
position[] 当前 7 个关节角度，单位 rad
|
||
velocity[] 当前 7 个关节速度，单位 rad/s
|
||
effort[] 当前 7 个关节 effort 字段；当前 driver 直接填入 SDK 的 effort[7]，按项目使用可视为关节电流/力矩反馈字段
|
||
```
|
||
|
||
示例：
|
||
|
||
```bash
|
||
ros2 topic echo /robot1/left_arm/joint_states
|
||
ros2 topic echo /robot1/right_arm/joint_states
|
||
```
|
||
|
||
当前反馈中没有关节加速度字段。如果外部软件必须获取关节加速度，需要在上游根据 `velocity[]` 和时间戳自行差分，或者让厂家提供 SDK 原生加速度反馈接口。
|
||
|
||
## 机器人健康/故障状态
|
||
|
||
当前 `officer_sdk/lbot_driver` 已发布系统错误信息 topic：
|
||
|
||
```text
|
||
/robot1/system_error
|
||
```
|
||
|
||
消息类型：
|
||
|
||
```text
|
||
lbot_arm_interfaces/msg/SystemError
|
||
```
|
||
|
||
消息定义：
|
||
|
||
```text
|
||
std_msgs/Header header
|
||
int32 error_code
|
||
string error_msg
|
||
bool connected
|
||
```
|
||
|
||
字段含义：
|
||
|
||
```text
|
||
error_code SDK 或 driver 上报的错误码
|
||
error_msg 错误信息
|
||
connected 发布错误时 driver 是否认为机器人处于连接状态
|
||
```
|
||
|
||
示例：
|
||
|
||
```bash
|
||
ros2 topic echo /robot1/system_error
|
||
```
|
||
|
||
目前该 topic 会在以下情况发布：
|
||
|
||
```text
|
||
SDK error callback 触发时
|
||
连接机器人失败时，error_code = -1
|
||
状态监控启动失败时，error_code = -2
|
||
```
|
||
|
||
注意：SDK 当前状态结构 `lbot_full_state_t` 只包含左右臂关节状态、末端位姿、时间戳和 IP；没有完整的故障码、告警码、使能状态、急停状态等字段。因此 `/robot1/system_error` 当前是错误事件 topic，不是完整健康状态快照。
|
||
|
||
仍然可以辅助查看 ROS 日志：
|
||
|
||
```text
|
||
/rosout
|
||
```
|
||
|
||
示例：
|
||
|
||
```bash
|
||
ros2 topic echo /rosout
|
||
```
|
||
|
||
也可以查看启动脚本日志：
|
||
|
||
```bash
|
||
tail -f ~/workspace/weipz-Lbot_Bridge/log/lbot_arm_control.log
|
||
```
|
||
|
||
如果后续需要稳定给外部软件使用的完整健康状态快照，建议在 `SystemError.msg` 之外再新增 `RobotHealth.msg`，并在连接/重连逻辑、使能/急停服务回调中维护并周期发布该状态。需要修改的位置：
|
||
|
||
```text
|
||
src/officer_sdk/lbot_arm_interfaces/msg/RobotHealth.msg
|
||
src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt
|
||
src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
|
||
src/officer_sdk/lbot_driver/src/lbot_driver.cpp
|
||
```
|
||
|
||
## 机械臂关节空间控制
|
||
|
||
外部软件如果需要发送机械臂各关节目标位置，使用 MoveJ 服务，不是 topic。
|
||
|
||
左臂：
|
||
|
||
```text
|
||
/robot1/left_arm/move_joint
|
||
```
|
||
|
||
右臂：
|
||
|
||
```text
|
||
/robot1/right_arm/move_joint
|
||
```
|
||
|
||
服务类型：
|
||
|
||
```text
|
||
lbot_arm_interfaces/srv/MoveJ
|
||
```
|
||
|
||
服务定义：
|
||
|
||
```srv
|
||
float32[] joints
|
||
float32 speed
|
||
float32 acce
|
||
bool block
|
||
---
|
||
bool success
|
||
```
|
||
|
||
字段含义：
|
||
|
||
```text
|
||
joints 7 个目标关节角，单位 rad
|
||
speed 整条机械臂统一目标速度，单位 rad/s
|
||
acce 整条机械臂统一目标加速度，单位 rad/s^2
|
||
block true 表示阻塞等待运动完成，false 表示非阻塞下发
|
||
```
|
||
|
||
示例：
|
||
|
||
```bash
|
||
ros2 service call /robot1/left_arm/move_joint lbot_arm_interfaces/srv/MoveJ \
|
||
"{joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], speed: 1.0, acce: 1.0, block: true}"
|
||
```
|
||
|
||
当前限制：
|
||
|
||
```text
|
||
MoveJ 支持每个关节独立目标位置 joints[7]
|
||
MoveJ 不支持每个关节独立目标速度 velocity[7]
|
||
MoveJ 不支持每个关节独立目标加速度 acceleration[7]
|
||
speed 和 acce 是整条机械臂统一标量
|
||
```
|
||
|
||
如果后续必须支持每个关节单独目标速度和加速度，需要修改：
|
||
|
||
```text
|
||
src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv
|
||
src/officer_sdk/lbot_driver/src/lbot_driver.cpp
|
||
src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
|
||
```
|
||
|
||
但当前 SDK 的 `lbot_move_joint()` 接口本身只接收：
|
||
|
||
```cpp
|
||
const double joints[7], double speed, double accel, bool block
|
||
```
|
||
|
||
因此即使 ROS service 增加 `velocity[7]`、`acceleration[7]` 字段，也不能直接传给现有 SDK。要真正实现每关节速度/加速度控制，需要厂家提供更底层的控制接口，或者在 ROS 层自己做轨迹插值，再周期性下发关节位置。
|
||
|
||
## L20/R20 灵巧手整手位置控制
|
||
|
||
外部软件如果需要一次性控制机器人灵巧手所有关节位置，使用下面两个 topic：
|
||
|
||
```text
|
||
/robot1/left_hand/set_l20_joint
|
||
/robot1/right_hand/set_l20_joint
|
||
```
|
||
|
||
消息类型：
|
||
|
||
```text
|
||
std_msgs/msg/Int32MultiArray
|
||
```
|
||
|
||
字段含义：
|
||
|
||
```text
|
||
data[] L20/R20 灵巧手目标位置数组
|
||
```
|
||
|
||
当前 driver 中该 topic 直接调用 SDK：
|
||
|
||
```cpp
|
||
lbot_l20_set_all_position(lbot_handle, LBOT_LEFT_ARM, hand_joints)
|
||
lbot_l20_set_all_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints)
|
||
```
|
||
|
||
SDK 头文件注释写明该接口参数是：
|
||
|
||
```text
|
||
16 个自由度的目标位置，单位 degree
|
||
```
|
||
|
||
因此当前整手控制按 16 个整数位置发送。
|
||
|
||
示例：
|
||
|
||
```bash
|
||
ros2 topic pub --once /robot1/left_hand/set_l20_joint std_msgs/msg/Int32MultiArray \
|
||
"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"
|
||
```
|
||
|
||
右手示例：
|
||
|
||
```bash
|
||
ros2 topic pub --once /robot1/right_hand/set_l20_joint std_msgs/msg/Int32MultiArray \
|
||
"{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
|
||
```
|
||
|
||
## 相关代码位置
|
||
|
||
反馈发布：
|
||
|
||
```text
|
||
src/officer_sdk/lbot_driver/src/lbot_driver.cpp
|
||
```
|
||
|
||
主要位置：
|
||
|
||
```text
|
||
LBot::state_publish_timer_callback()
|
||
```
|
||
|
||
机械臂 MoveJ 服务：
|
||
|
||
```text
|
||
LeftArmServiceNode::move_joint_callback()
|
||
RightArmServiceNode::move_joint_callback()
|
||
```
|
||
|
||
L20/R20 灵巧手 topic：
|
||
|
||
```text
|
||
LeftArmServiceNode::left_hand_l20_set_joint_callback()
|
||
RightArmServiceNode::right_hand_l20_set_joint_callback()
|
||
```
|
||
|
||
SDK API 声明：
|
||
|
||
```text
|
||
src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h
|
||
```
|
||
|
||
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
|
||
```
|