action话题控制机械臂

要通过ROS中的Action话题来控制机械臂，你需要按照以下步骤进行：

1. **定义Action文件**
2. **编写Action Server（控制机械臂）**
3. **编写Action Client（发送控制命令）**
4. **配置和运行**

### 1. 定义Action文件

首先，你需要定义一个`.action`文件来描述客户端和服务器之间的消息格式。例如，定义一个文件`MoveArm.action`：

```plaintext
# 定义目标
geometry_msgs/PoseStamped target_pose
---
# 定义反馈
float32 progress
---
# 定义结果
bool success
```

### 2. 生成消息文件

在`CMakeLists.txt`中，添加以下内容：

```plaintext
find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib message_generation geometry_msgs)

add_action_files(
  FILES
  MoveArm.action
)

generate_messages(
  DEPENDENCIES
  actionlib_msgs geometry_msgs
)
```

在`package.xml`中，添加依赖：

```xml
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib_msgs</exec_depend>
<build_depend>actionlib</build_depend>
<exec_depend>actionlib</exec_depend>
<build_depend>geometry_msgs</build_depend>
<exec_depend>geometry_msgs</exec_depend>
```

### 3. 编写Action Server（控制机械臂）

你需要一个Action Server来控制机械臂的动作。假设你使用的是MoveIt!，可以编写如下服务器代码：

```python
import rospy
import actionlib
from moveit_commander import MoveGroupCommander
from your_package_name.msg import MoveArmAction, MoveArmFeedback, MoveArmResult

class MoveArmActionServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer('move_arm', MoveArmAction, self.execute, False)
        self.server.start()
        self.arm = MoveGroupCommander("arm")

    def execute(self, goal):
        feedback = MoveArmFeedback()
        result = MoveArmResult()

        # 设置目标位置
        self.arm.set_pose_target(goal.target_pose)

        # 开始规划并移动
        plan = self.arm.go(wait=True)

        # 假设我们通过反馈更新进度
        feedback.progress = 50.0
        self.server.publish_feedback(feedback)

        if plan:
            result.success = True
        else:
            result.success = False
        
        self.server.set_succeeded(result)

if __name__ == '__main__':
    rospy.init_node('move_arm_action_server')
    server = MoveArmActionServer()
    rospy.spin()
```

### 4. 编写Action Client（发送控制命令）

客户端需要发送控制命令给Action Server：

```python
import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from your_package_name.msg import MoveArmAction, MoveArmGoal

def feedback_cb(feedback):
    rospy.loginfo('Progress: %f' % feedback.progress)

def move_arm_client():
    client = actionlib.SimpleActionClient('move_arm', MoveArmAction)
    client.wait_for_server()

    goal = MoveArmGoal()
    goal.target_pose.header.frame_id = "base_link"
    goal.target_pose.pose.position.x = 0.5
    goal.target_pose.pose.position.y = 0.0
    goal.target_pose.pose.position.z = 0.5
    goal.target_pose.pose.orientation.w = 1.0

    client.send_goal(goal, feedback_cb=feedback_cb)
    client.wait_for_result()

    result = client.get_result()
    rospy.loginfo('Success: %s' % result.success)

if __name__ == '__main__':
    rospy.init_node('move_arm_action_client')
    move_arm_client()
```

### 5. 配置和运行

1. 启动ROS master：

   ```sh
   roscore
   ```

2. 启动Action Server：

   ```sh
   rosrun your_package_name move_arm_action_server.py
   ```

3. 启动Action Client：

   ```sh
   rosrun your_package_name move_arm_action_client.py
   ```

### 6. 调试和验证

确保所有节点正常运行，Action Client能够发送目标给Action Server，并且服务器能够控制机械臂移动到指定位置。如果遇到问题，可以通过`rostopic`查看相关的主题和消息进行调试。

这样你就能够通过Action话题控制机械臂了。