Python中使用SimpleActionServer()实现机器人动作控制

在Python中，可以使用SimpleActionServer类来实现机器人动作控制。SimpleActionServer是ROS（Robot Operation System）中提供的一个动作服务器类，用于接收机器人动作请求并执行相应的动作。

下面是一个简单的例子，展示了如何使用SimpleActionServer类实现一个机器人的动作控制。

首先，我们需要导入相关的ROS和动作相关的类和模块：

#!/usr/bin/env python

import rospy
import actionlib
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from robot_msgs.msg import MoveRobotAction, MoveRobotFeedback, MoveRobotResult

然后，我们创建一个名为RobotActionServer的类，用于实例化动作服务器：

class RobotActionServer:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('robot_action_server')

        # 创建动作服务器
        self.server = actionlib.SimpleActionServer('move_robot', MoveRobotAction, self.execute_callback, False)

        # 注册动作服务器的回调函数
        self.server.start()

        # 创建一个控制机器人运动的发布者
        self.velocity_pub = rospy.Publisher('robot_velocity', Twist, queue_size=1)

在初始化函数中，我们传递了三个参数给SimpleActionServer()函数，分别是动作服务器的名称、动作类型和回调函数。此外，我们还创建了一个Twist类型的消息发布者velocity_pub，用于发布机器人的速度控制命令。

接着，我们实现了回调函数execute_callback：

    def execute_callback(self, goal):
        # 创建一个反馈消息对象
        feedback = MoveRobotFeedback()

        # 创建一个结果消息对象
        result = MoveRobotResult()

        # 执行机器人动作
        for i in range(goal.distance):
            # 更新反馈消息的进度
            feedback.progress = i

            # 发布机器人的速度控制命令
            velocity_cmd = Twist()
            velocity_cmd.linear.x = 0.1  # 设置机器人前进的线速度
            self.velocity_pub.publish(velocity_cmd)

            # 模拟机器人动作执行的时间
            rospy.sleep(0.1)

            # 检查动作是否被取消
            if self.server.is_preempt_requested():
                # 如果动作被取消，停止机器人运动
                self.velocity_pub.publish(Twist())

                # 设置动作的结果状态为被取消
                result.status = "Action preempted"

                # 通过动作服务器发送结果消息
                self.server.set_preempted(result)
                return

        # 如果动作执行完成，停止机器人运动
        self.velocity_pub.publish(Twist())

        # 设置动作的结果状态为成功完成
        result.status = "Action succeeded"

        # 通过动作服务器发送结果消息
        self.server.set_succeeded(result)

在回调函数中，我们首先创建了一个反馈消息对象和一个结果消息对象。然后，通过一个循环来执行机器人的动作。在循环中，我们更新了反馈消息的进度，发布机器人的速度控制命令，并模拟了机器人动作执行的时间。在每次循环之前，我们检查动作是否被取消，如果是，则停止机器人运动，设置结果消息的状态为被取消，并通过动作服务器发送取消结果。如果动作执行完成，停止机器人运动，设置结果消息的状态为成功，最后通过动作服务器发送成功结果。

最后，我们创建一个名为main的函数来实例化RobotActionServer类并保持运行状态：

def main():
    rospy.loginfo("Robot Action Server is running...")
    robot_action_server = RobotActionServer()
    rospy.spin()

if __name__ == '__main__':
    main()

在main函数中，我们首先使用rospy.loginfo()函数打印一条日志消息以确认动作服务器正在运行。然后，实例化RobotActionServer类，并调用rospy.spin()函数来保持节点的运行状态。

需要注意的是，上述代码中的消息类型MoveRobotAction、MoveRobotFeedback和MoveRobotResult是自定义的消息类型，需要先定义好并在代码中导入相应的模块。

以上就是一个使用SimpleActionServer类实现机器人动作控制的例子，通过这个例子，我们可以了解如何创建一个动作服务器，实现动作的执行和反馈，并处理动作的取消和完成结果。