Python中利用SimpleActionServer()实现自定义机器人动作

在Python中，可以使用SimpleActionServer()类来实现自定义机器人动作。SimpleActionServer()类是actionlib包中的一部分，用于处理动作服务器端。

下面是一个使用SimpleActionServer()实现一个机器人接收并执行动作的示例：

import rospy
import actionlib
from my_robot_msgs.msg import MoveRobotAction, MoveRobotFeedback, MoveRobotResult

class RobotActionServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer('move_robot', MoveRobotAction, execute_cb=self.execute_callback, auto_start=False)
        self.server.start()

    def execute_callback(self, goal):
        rate = rospy.Rate(1)  # 控制循环频率为1Hz
        success = False
        
        # 执行动作的逻辑
        for i in range(goal.distance):
            # 在这里执行机器人移动的代码
            # ...
            
            # 更新反馈信息
            feedback = MoveRobotFeedback()
            feedback.current_distance = i
            self.server.publish_feedback(feedback)
            
            # 检查动作是否被取消
            if self.server.is_preempt_requested():
                rospy.loginfo('Action preempted')
                self.server.set_preempted()
                success = False
                break
            
            rate.sleep()
        
        # 完成动作并返回结果
        result = MoveRobotResult()
        if success:
            rospy.loginfo('Action completed successfully')
            self.server.set_succeeded(result)
        else:
            rospy.loginfo('Action failed')
            self.server.set_aborted(result)


if __name__ == '__main__':
    rospy.init_node('robot_action_server')
    
    action_server = RobotActionServer()
    
    rospy.spin()

在上面的示例中，机器人动作使用了自定义的MoveRobotAction消息类型。execute_callback函数是动作执行的回调函数，在其中编写机器人执行动作的逻辑。通过self.server.publish_feedback()函数可以发布反馈信息。

该示例中的动作逻辑是简单的循环移动机器人。循环中，我们在每次迭代时更新反馈信息，并检查动作是否被取消。如果动作被取消，则调用self.server.set_preempted()函数以取消动作，并在适当的地方设置success变量为False。否则，继续执行动作逻辑。

完成动作后，根据成功与否调用self.server.set_succeeded()或self.server.set_aborted()函数设置动作的完成状态。需要注意的是，这些函数必须在所有的return语句之后调用，否则动作服务器将无法正常工作。

为了测试动作服务器，可以编写一个简单的客户端进行调用：

import rospy
import actionlib
from my_robot_msgs.msg import MoveRobotAction, MoveRobotGoal

if __name__ == '__main__':
    rospy.init_node('robot_action_client')
    
    client = actionlib.SimpleActionClient('move_robot', MoveRobotAction)
    client.wait_for_server()
    
    goal = MoveRobotGoal()
    goal.distance = 10
    
    client.send_goal(goal)
    client.wait_for_result()
    
    result = client.get_result()
    
    rospy.loginfo('Action finished with result %s', result)

在上面的示例中，客户端创建了一个SimpleActionClient()实例，并使用send_goal()函数发送动作目标。然后，客户端等待动作完成并获取结果。

以上就是使用SimpleActionServer()类实现自定义机器人动作的示例。在实际应用中，可以根据自己的需求来编写动作逻辑，并根据动作的执行情况设置适当的状态和反馈信息。