使用Python中的actionlibSimpleActionServer()实现简单动作服务器

actionlib是ROS中用于实现异步通信和执行复杂任务的库。SimpleActionServer是actionlib中的一个类，用于实现简单的动作服务器。

使用Python中的actionlib.SimpleActionServer()需要以下步骤：

1. 导入必要的模块：

   import rospy
   import actionlib
   from actionlib_msgs.msg import *
   from your_package.msg import YourActionFeedback, YourActionResult, YourAction
   

其中，YourActionFeedback和YourActionResult分别是反馈和结果的消息类型，YourAction是动作消息类型。

2. 创建动作服务器回调函数：

   def action_callback(goal):
       # 在此处实现动作服务器的逻辑
       # goal包含客户端传递的目标信息
       # 可以在此处发布反馈信息或计算结果

       # 创建反馈消息对象
       feedback = YourActionFeedback()

       # 创建结果消息对象
       result = YourActionResult()

       # 在此处编写动作服务器的逻辑

       # 判断动作是否已完成
       if condition:
           result = YOUR_RESULT_VALUE
           server.set_succeeded(result)  # 设置动作服务器的结果

       # 发布反馈消息
       server.publish_feedback(feedback)
   

在这个回调函数中，我们可以根据客户端传来的目标信息，编写实际的执行逻辑。

3. 初始化ROS节点并创建动作服务器：

   rospy.init_node('your_action_server')  # 初始化ROS节点

   # 创建动作服务器对象
   server = actionlib.SimpleActionServer('your_action', YourAction, action_callback, False)
   
   # 启动动作服务器
   server.start()
   

这里我们通过SimpleActionServer创建了一个名为your_action的动作服务器，使用的动作类型是YourAction。

4. 运行ROS循环：

   rospy.spin()
   

这个函数用于持续运行ROS节点，并监听来自客户端的目标。

5. 编写代码的其余部分：

   if __name__ == '__main__':
       try:
           # 进行其他初始化操作

           # 运行ROS循环
           rospy.spin()
       except rospy.ROSInterruptException:
           pass
   

通过上述步骤，我们可以实现一个简单的动作服务器。下面是一个完整的例子：

import rospy
import actionlib
from actionlib_msgs.msg import *
from your_package.msg import YourActionFeedback, YourActionResult, YourAction

def action_callback(goal):
    # 在此处实现动作服务器的逻辑
    # goal包含客户端传递的目标信息
    # 可以在此处发布反馈信息或计算结果

    # 创建反馈消息对象
    feedback = YourActionFeedback()

    # 创建结果消息对象
    result = YourActionResult()

    # 在此处编写动作服务器的逻辑

    # 判断动作是否已完成
    if condition:
        result = YOUR_RESULT_VALUE
        server.set_succeeded(result)  # 设置动作服务器的结果

    # 发布反馈消息
    server.publish_feedback(feedback)

if __name__ == '__main__':
    try:
        rospy.init_node('your_action_server')  # 初始化ROS节点

        # 创建动作服务器对象
        server = actionlib.SimpleActionServer('your_action', YourAction, action_callback, False)
        
        # 启动动作服务器
        server.start()

        # 进行其他初始化操作

        # 运行ROS循环
        rospy.spin()

    except rospy.ROSInterruptException:
        pass

在这个例子中，我们实现了一个名为your_action的动作服务器，使用的消息类型是YourActionFeedback和YourActionResult。在回调函数action_callback中，我们可以根据客户端传来的目标信息编写实际的执行逻辑，并发布反馈信息和设置结果。整个代码通过ROS节点的方式进行运行，并监听来自客户端的目标信息。