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

在Python中，可以使用SimpleActionServer类来实现远程控制机器人动作。SimpleActionServer是ROS（Robot Operating System）中的一个类，用于实现动作服务器，与动作客户端进行通信。

下面是一个使用SimpleActionServer的例子，其中机器人被控制执行一个简单的移动动作。我们将使用actionlib库和rospy库来实现。

首先，在Python代码中导入所需的库：

import rospy
import actionlib
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from actionlib_msgs.msg import GoalStatusArray
from my_project.msg import MoveAction, MoveFeedback, MoveResult

接下来，我们需要创建一个动作服务器节点，并初始化动作服务器：

rospy.init_node('move_action_server')

然后，我们需要定义一个回调函数，用于处理来自动作客户端的请求：

def move_callback(goal):
    # 获取客户端传来的目标位置
    target_position = goal.target_position

    # 创建一个发布器，发布器将发布移动命令到机器人上
    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    # 创建一个订阅器，订阅机器人的状态
    status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, status_callback)

    # 创建一个反馈对象，用于将反馈信息发送回客户端
    feedback = MoveFeedback()

    # 创建一个结果对象，用于将结果信息发送回客户端
    result = MoveResult()

    # 设置移动速度
    move_speed = 0.2

    # 设置移动方向
    if target_position == 'forward':
        twist_msg = Twist()
        twist_msg.linear.x = move_speed
    elif target_position == 'backward':
        twist_msg = Twist()
        twist_msg.linear.x = -move_speed
    elif target_position == 'left':
        twist_msg = Twist()
        twist_msg.angular.z = move_speed
    elif target_position == 'right':
        twist_msg = Twist()
        twist_msg.angular.z = -move_speed

    # 发布移动命令
    cmd_vel_pub.publish(twist_msg)

    # 发布反馈信息
    feedback.current_position = current_position
    move_server.publish_feedback(feedback)

    # 等待移动完成（例如，根据机器人当前位置和目标位置计算距离，等待距离小于某个阈值）
    while not move_finished:
        rospy.sleep(0.1)

    # 发布结果信息
    if move_successful:
        move_server.set_succeeded(result)
    else:
        move_server.set_aborted(result)

在回调函数中，我们首先从参数中获取目标位置，然后根据目标位置设置机器人的移动命令，之后使用发布器发布移动命令。接着，我们等待移动完成，通过检测机器人的状态来确定是否移动完成，最后根据移动是否成功，发布相应的结果信息。

最后，我们创建一个动作服务器实例，并指定动作类型、回调函数和动作服务器名称：

move_server = actionlib.SimpleActionServer('move_robot', MoveAction, move_callback, False)
move_server.start()

在上面的代码中，我们创建了一个名为'move_robot'的动作服务器，它接受的动作类型为MoveAction，并在收到请求时调用move_callback函数进行处理。最后，我们调用start()方法启动动作服务器。

整个代码如下所示：

import rospy
import actionlib
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from actionlib_msgs.msg import GoalStatusArray
from my_project.msg import MoveAction, MoveFeedback, MoveResult

rospy.init_node('move_action_server')

def move_callback(goal):
    # 获取客户端传来的目标位置
    target_position = goal.target_position

    # 创建一个发布器，发布器将发布移动命令到机器人上
    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    # 创建一个订阅器，订阅机器人的状态
    status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, status_callback)

    # 创建一个反馈对象，用于将反馈信息发送回客户端
    feedback = MoveFeedback()

    # 创建一个结果对象，用于将结果信息发送回客户端
    result = MoveResult()

    # 设置移动速度
    move_speed = 0.2

    # 设置移动方向
    if target_position == 'forward':
        twist_msg = Twist()
        twist_msg.linear.x = move_speed
    elif target_position == 'backward':
        twist_msg = Twist()
        twist_msg.linear.x = -move_speed
    elif target_position == 'left':
        twist_msg = Twist()
        twist_msg.angular.z = move_speed
    elif target_position == 'right':
        twist_msg = Twist()
        twist_msg.angular.z = -move_speed

    # 发布移动命令
    cmd_vel_pub.publish(twist_msg)

    # 发布反馈信息
    feedback.current_position = current_position
    move_server.publish_feedback(feedback)

    # 等待移动完成（例如，根据机器人当前位置和目标位置计算距离，等待距离小于某个阈值）
    while not move_finished:
        rospy.sleep(0.1)

    # 发布结果信息
    if move_successful:
        move_server.set_succeeded(result)
    else:
        move_server.set_aborted(result)

move_server = actionlib.SimpleActionServer('move_robot', MoveAction, move_callback, False)
move_server.start()

rospy.spin()

以上是一个使用SimpleActionServer实现远程控制机器人动作的例子。在这个例子中，我们创建了一个动作服务器节点，定义了一个回调函数来处理机器人移动的请求，并使用SimpleActionServer类启动了动作服务器。