# leg_control

提供 `LegControl.action` 的 ROS2 Action Server 示例。

## Action 定义
文件：`interfaces/action/LegControl.action`
```
# Goal: leg control parameters
geometry_msgs/TwistStamped target
---
bool success
string message
---
float32 progress
```

- Goal: TwistStamped
- Result: `success`, `message`
- Feedback: `progress` (0.0~1.0)

## 节点
可执行文件：`leg_control_node`
- Action 名称：`/leg_control`
- 行为：接受目标后按 10 个步骤模拟执行，发布进度反馈，最终返回成功。
- 验证：当 `effort < 0` 会拒绝目标。

## 依赖
`package.xml` 中已声明：
- rclcpp
- rclcpp_action
- interfaces

## 编译
在工作空间根目录：
```
# Goal: leg control parameters
geometry_msgs/TwistStamped target
---
bool success
string message
---
float32 progress
```

- Goal: `target` (TwistStamped: header + twist.linear(x,y,z) + twist.angular(x,y,z))
- Result: `success`, `message`
- Feedback: `progress` (0.0~1.0)

## 发送测试目标 (命令行)
使用 `ros2 action send_goal`：
```bash
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}}}'
```
示例输出（简化）：
```
Accepted
Feedback: progress: 0.1
Feedback: progress: 0.2
...
Result: success: True message: "LegControl done"
```

### 失败/拒绝示例
```bash
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}}'
```
输出：`Goal rejected`（零速度与零角速度被拒绝）。

### 取消示例
另开终端发送：
```bash
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}}}' --feedback
```
在执行中（看到进度后）按 Ctrl+C 触发取消；或使用：
```bash
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}}}' --feedback --cancel-on=feedback:0.5
```

## 在 C++ 客户端中调用示例
```cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "interfaces/action/leg_control.hpp"

using interfaces::action::LegControl;

class LegClient : public rclcpp::Node {
public:
  LegClient(): Node("leg_client") {
    client_ = rclcpp_action::create_client<LegControl>(shared_from_this(), "leg_control");
  }
  void send() {
    if (!client_->wait_for_action_server(std::chrono::seconds(2))) {
      RCLCPP_ERROR(get_logger(), "Server not available");
      return;
    }
    auto goal_msg = LegControl::Goal();
  goal_msg.target.header.frame_id = "base_link";
  goal_msg.target.twist.linear.x = 0.2;
  goal_msg.target.twist.angular.z = 0.3;
    auto send_goal_options = rclcpp_action::Client<LegControl>::SendGoalOptions();
    send_goal_options.feedback_callback = [this](auto, auto feedback){
      RCLCPP_INFO(get_logger(), "Feedback progress=%.2f", feedback->progress);
    };
    send_goal_options.result_callback = [this](auto res){
      if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
        RCLCPP_INFO(get_logger(), "Result: %s", res.result->message.c_str());
      }
    };
    client_->async_send_goal(goal_msg, send_goal_options);
  }
private:
  rclcpp_action::Client<LegControl>::SharedPtr client_;
};
```

## 与 SkillManager 集成（潜在）
若在技能配置 `robot_skills.yaml` 中添加：
```
- name: LegControl
  interfaces:
    - LegControl.action
```
则 `SkillManager` 将可注册客户端并通过行为树调度。

## 扩展建议
- 增加模式枚举常量（使用 `int32` + 常量说明）
- 增加安全检查： effort 上限
- 增加实时关节状态发布（自定义 msg）
- 支持并行多目标（需队列管理）

---
需要我继续补充测试脚本或 Python 客户端示例，请再告诉我。
