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

在Python中，使用SimpleActionServer()可以实现简单的行为控制。Action是一个特殊的消息类型，它允许您定义一个目标和一个与之相关的结果。

首先，需要在工程中安装actionlib包。在终端上输入以下命令进行安装：

pip install actionlib

下面是一个使用SimpleActionServer()的例子，其中实现了一个简单的行为控制，控制机器人移动到指定位置的任务。

1. 请确保您的ROS环境已经设置好，并且保持运行状态。

2. 创建一个名为move_action_server.py的文件，并将以下代码复制到其中：

#!/usr/bin/env python

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Point

class MoveActionServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer('move_action', MoveBaseAction, self.execute, False)
        self.server.start()

    def execute(self, goal):
        rospy.loginfo("Received goal: {}".format(goal))
        
        # 解析目标位置
        target = goal.target_pose.pose.position
        rospy.loginfo("Moving to target: ({}, {}, {})".format(target.x, target.y, target.z))
        
        # 执行移动操作，这里只是一个示例，实际操作需要根据机器人平台来实现
        # 这里只是打印出目标位置和运动完成提示
        rospy.sleep(3)
        rospy.loginfo("Reached target: ({}, {}, {})".format(target.x, target.y, target.z))
        
        # 返回任务结果
        self.server.set_succeeded()

if __name__ == '__main__':
    rospy.init_node('move_action_server')
    server = MoveActionServer()
    rospy.spin()

在上述代码中，我们首先导入了所需的ROS和actionlib包，以及与移动控制相关的消息类型。

然后创建了一个名为MoveActionServer的类，在其中进行了初始化的操作，并创建了一个名为move_action的SimpleActionServer。

在execute方法中，我们首先打印出接收到的目标位置，并执行移动操作，这里只是一个示例，实际操作需要根据机器人平台来实现。在移动操作完成后，我们打印出目标位置以及一个运动完成的提示。

最后，在main函数中，我们初始化了ROS节点并启动了MoveActionServer，并让程序保持运行状态。

3. 现在，在另一个终端中，运行以下命令启动ROS节点：

roscore

4. 接下来，在一个新的终端中，运行以下命令来启动行为服务器：

rosrun <package_name> move_action_server.py

请将<package_name>替换为包的名称。

5. 最后，在另一个新的终端中，可以使用rostopic pub命令发送一个目标位置到行为服务器。运行以下命令：

rostopic pub /move_action/goal move_base_msgs/MoveBaseActionGoal '{header: {stamp: now, frame_id: "map"}, goal_id: {stamp: now, id: "goal"}, goal: {target_pose: {header: {stamp: now, frame_id: "map"}, pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}}'

您可以根据需要修改目标位置(x, y, z)的值。

在运行该命令后，行为服务器将接收到目标位置，并在控制台上打印出相关的信息。然后执行移动操作，并在完成后打印出目标位置和运动完成提示。

这就是使用SimpleActionServer()实现简单的行为控制的方法。您可以根据需要扩展和修改该代码，以满足您的具体需求。