# arm_control 包（运动模块）

arm_control 是一个简易的 ROS 2 运动模块，提供 ArmSpaceControl 动作（Action）服务端，用于控制机械臂末端在空间中的目标位姿（geometry_msgs/PoseStamped）。该模块当前以模拟方式汇报进度、支持取消，并在完成后返回结果，方便上层流程联调与验证。

## 功能概览
- 动作接口：`interfaces/action/ArmSpaceControl`
- 默认动作名称：`/ArmSpaceControl`（可通过参数 `action_name` 修改，参数默认值为 `ArmSpaceControl`，统一使用首字母大写形式）
- 反馈（Feedback）：`progress`（0.0 ~ 100.0）
- 支持取消（Cancel）
- 结果（Result）：`success`、`message`

## 依赖
- ROS 2 Humble
- rclcpp、rclcpp_action
- interfaces（自定义接口包）
- geometry_msgs

## 构建
从工作空间根目录执行：
```bash
colcon build --packages-select arm_control
source install/setup.bash
```

## 运行服务端
两种方式择一：
```bash
# 方式一：使用多节点运行脚本，仅启动 arm_control 节点
./src/scripts/run.sh --motion-only

# 方式二：直接运行可执行文件
ros2 run arm_control arm_control_node
```
启动成功后日志类似：

```text
[INFO] [arm_control_node]: ArmSpaceControl action server started on 'ArmSpaceControl'
```


## 使用 send_arm_goal.sh 发送目标（推荐）
为避免命令行转义和 YAML 拼写问题，提供了辅助脚本：`src/scripts/send_arm_goal.sh`。

查看帮助：
```bash
./src/scripts/send_arm_goal.sh --help
```
常用示例：
```bash
# 1) 使用默认参数（/ArmSpaceControl，base_link，x=0.5 y=0.0 z=0.3，yaw=0deg）
./src/scripts/send_arm_goal.sh

# 2) 指定位置和偏航角（角度）
./src/scripts/send_arm_goal.sh --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg

# 3) 使用弧度制的偏航角
./src/scripts/send_arm_goal.sh --yaw 1.57 --rad

# 4) 更改目标坐标系和动作名称
./src/scripts/send_arm_goal.sh --frame map --action /ArmSpaceControl

# 5) 仅查看将要发送的 YAML（不真正发送）
./src/scripts/send_arm_goal.sh --dry-run
```
脚本会：
- 将 yaw（仅绕 Z 轴）转换为四元数（roll=pitch=0）
- 组装 `PoseStamped` 的 YAML 负载
- 调用：
```
ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl <yaml>
```

若脚本没有执行权限，可赋权：
```bash
chmod +x src/scripts/send_arm_goal.sh
```

## 直接使用 ros2 CLI 发送（手工方式）
若更偏好手工命令，确保已 `source install/setup.bash`，并将 YAML 保持单行以避免转义问题：
```bash
ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"
```

## 取消目标
从发送端输出中获取 `goal_id` 后执行：
```bash
ros2 action cancel /ArmSpaceControl <goal-id>
```

## 查看与排查
- 查看 action 列表与类型：
```bash
ros2 action list
ros2 action info /ArmSpaceControl -t
```
- 查看接口定义：
```bash
ros2 interface show interfaces/action/ArmSpaceControl
ros2 interface show geometry_msgs/msg/PoseStamped
```
- 常见问题：
  - 报错 “The passed action type is invalid”
    - 确认已在当前终端执行 `source install/setup.bash`
    - 类型字符串必须精确：`interfaces/action/ArmSpaceControl`
    - 避免使用反斜杠换行，改用单行 YAML 或使用脚本
  - 一直提示 “Waiting for an action server to become available…”
  - 确认服务端是否在运行：`ros2 action list` 应显示 `/ArmSpaceControl`
    - 检查命名空间或参数 `action_name` 是否与预期一致
  - 四元数异常
    - 脚本仅使用 yaw 生成姿态，确保 yaw 合理且单位正确（`--deg` 或 `--rad`）

## 与多节点运行脚本配合
如果需要同时启动多个节点，并输出到文件：
```bash
./src/scripts/run.sh --motion-only --log-to-files
# 查看日志
ls -1 log/run/*/arm_control.log | tail -n1 | xargs tail -f
```

## 实现说明
服务端实现位于：`src/modules/motion/arm_control/src/arm_control_node.cpp`
- 参数 `action_name` 控制动作名称（默认 `ArmSpaceControl`）
- 回调 `handle_goal / handle_cancel / handle_accepted / execute` 分别处理目标、取消与执行流程
- 当前为模拟执行逻辑，可在 `execute()` 中接入真实控制器

---
如需将本模块接入上层状态机或其他组件，可继续在此 README 补充接口约定和联调步骤。
