Python中actionlibSimpleActionServer()的简单实现

在Python中，使用actionlib库可以实现ROS系统中的简单行为服务器（Simple Action Server）。Action是一个在ROS系统中执行长时间任务的一种方法。

下面是一个使用actionlib库实现的简单行为服务器的例子：

import rospy
import actionlib
from your_package.msg import YourAction

# 行为执行函数
def execute_goal(goal):
    # 在此执行行为的逻辑
    result = YourActionResult()
    # 设置结果状态，通常是成功或失败
    result.status = actionlib.SimpleClientGoalState.DONE
    # 设置行为的结果
    server.set_succeeded(result)

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

    # 创建行动服务器
    server = actionlib.SimpleActionServer('your_action', YourAction, execute_goal, False)
    
    # 启动服务器
    server.start()

    rospy.spin()

上面的代码中，首先需要导入必要的库，并从your_package包中导入所需的消息类型。然后，定义了一个execute_goal函数，作为行为的执行逻辑。在该函数中，你可以编写实际的行为逻辑，并设置行为的结果。在这个例子中，设置了一个YourActionResult对象，并将结果状态设置为DONE。最后，在if __name__ == '__main__':中初始化了节点，并创建了一个简单行为服务器server，并指定处理行为的函数execute_goal。最后，通过调用server.start()方法启动服务器，并通过rospy.spin()函数使程序保持运行。

你还需要相应地定义消息类型your_package.msg.YourAction和your_package.msg.YourActionResult，以及行为的目标和结果类型。

此外，还可以为行为服务器添加一些回调函数来处理不同的行为状态，例如server.set_aborted(result)用于设置行为已中止的结果。

行为服务器可以通过以下方式与行为客户端进行通信：

import rospy
import actionlib
from your_package.msg import YourAction, YourGoal, YourResult

# 创建行动客户端
client = actionlib.SimpleActionClient('your_action', YourAction)
# 等待服务器启动
client.wait_for_server()

# 创建行动目标
goal = YourGoal()
# 设置目标的参数
goal.param = 'value'

# 发送目标到服务器
client.send_goal(goal)

# 等待行动完成
client.wait_for_result()

# 获取行动结果
result = client.get_result()

# 打印结果状态
if client.get_state() == actionlib.SimpleGoalState.SUCCEEDED:
    print('Action succeeded')
elif client.get_state() == actionlib.SimpleGoalState.ABORTED:
    print('Action aborted')
elif client.get_state() == actionlib.SimpleGoalState.PREEMPTED:
    print('Action preempted')

在上面的例子中，首先创建了一个行动客户端client，并调用wait_for_server()方法等待服务器启动。然后，创建了行动的目标对象goal，并设置相应的参数。通过调用client.send_goal(goal)方法将目标发送给服务器。然后，通过调用client.wait_for_result()等待行动完成，并通过client.get_result()来获取行动的结果。最后，通过client.get_state()获取行动的状态，并根据状态来执行相应的操作。

总结来说，使用actionlib库可以在Python中实现ROS系统中的简单行为服务器，并通过行为客户端与服务器进行通信。你可以根据具体的需求编写并执行行为逻辑，并根据需要设置行为的结果和状态。