使用SimpleActionServer()在Python中实现简单的行为控制逻辑

在Python中，可以使用SimpleActionServer类来实现简单的行为控制逻辑。SimpleActionServer是ROS中提供的一个用于处理和管理行为服务器（Action Server）的类，它提供了一组方法来处理和响应行为请求。

下面是一个使用SimpleActionServer实现简单行为控制逻辑的示例：

import rospy
import actionlib
from my_robot_msgs.msg import MyAction, MyResult

def action_callback(goal):
    # 获取目标值
    target_value = goal.target

    # 执行行为逻辑
    result = MyResult()

    if target_value > 0:
        # 如果目标值大于0，表示执行成功
        result.success = True
        result.message = "Action completed successfully."
    else:
        # 如果目标值小于等于0，表示执行失败
        result.success = False
        result.message = "Action failed."

    # 发布结果
    server.set_succeeded(result)

if __name__ == "__main__":
    # 初始化ROS节点
    rospy.init_node("action_server")

    # 创建行为服务器
    server = actionlib.SimpleActionServer("my_action", MyAction, action_callback, auto_start=False)

    # 启动行为服务器
    server.start()

    # 进入主循环
    rospy.spin()

在下面的例子中，我们创建一个名为action_server的ROS节点。首先，我们初始化了ROS节点。然后，我们创建了一个SimpleActionServer对象，并传递了以下参数：

- "my_action"：用于指定行为服务器的名称。

- MyAction：用于指定行为消息类型。

- action_callback：用于指定行为回调函数，该函数将在接收到行为请求时执行。

在action_callback函数中，我们首先从行为目标（goal）中获取要执行的目标值（target_value）。然后，我们执行了一个简单的行为逻辑。如果目标值大于0，表示行为执行成功，我们将设置result.success为True，并将"Action completed successfully."赋给result.message。否则，我们将设置result.success为False，并将"Action failed."赋给result.message。

最后，我们通过调用set_succeeded()方法向行为客户端发送执行结果。

在main函数中，我们创建了行为服务器，并调用start()方法启动行为服务器。然后，我们通过调用rospy.spin()进入了主循环，以保持节点运行。

使用这个例子，你可以创建一个简单的行为服务器，接收来自行为客户端的请求，并根据请求执行相应的行为逻辑，并将结果发送回给行为客户端。