# wpz_LbotArm 接口说明

本工程当前使用 `src/officer_sdk/lbot_driver` 作为机器人网口连接和 ROS2 bridge。启动脚本为：

```bash
cd ~/workspace/wpz_LbotArm/scripts
./lbot_arm_control.sh start
```

停止服务：

```bash
cd ~/workspace/wpz_LbotArm/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/wpz_LbotArm/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]}"
```

## 相关代码位置

反馈发布：

```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
```