利用SimpleActionServer()在Python中实现基本行为控制

在Python中，可以使用SimpleActionServer()来实现基本的行为控制。SimpleActionServer()是ROS中的一个类，用于创建一个简单的行为服务器。

下面是一个使用SimpleActionServer()的示例，以控制一个机器人移动的行为为例：

#!/usr/bin/env python
import rospy
import actionlib
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from my_robot_msgs.msg import MoveAction, MoveGoal, MoveResult, MoveFeedback

class MoveActionServer:
    def __init__(self):
        self.action_server = actionlib.SimpleActionServer(
            'move_action', MoveAction, execute_cb=self.execute_callback, auto_start=False)
        self.action_server.start()
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)

    def execute_callback(self, goal):
        # 初始化反馈和结果
        feedback = MoveFeedback()
        result = MoveResult()

        # 获取目标位置和移动速度
        target_position = goal.target_position
        speed = goal.speed

        # 发布移动指令
        twist = Twist()
        twist.linear.x = speed
        self.cmd_vel_pub.publish(twist)

        # 进行移动，并发送反馈
        rospy.loginfo("Moving...")
        while True:
            # 判断是否需要取消行为
            if self.action_server.is_preempt_requested():
                # 取消移动，发送结果和反馈
                rospy.loginfo("Move action is preempted.")
                self.cmd_vel_pub.publish(Twist())  # 停止机器人
                self.action_server.set_preempted(result, "Move action is preempted.")
                return

            # 判断是否到达目标位置
            if abs(distance_to_target - target_position) < 0.1:
                break

            # 更新反馈信息
            feedback.current_position = current_position
            self.action_server.publish_feedback(feedback)

            # 假设机器人每秒移动1米，更新当前位置
            current_position += speed * 0.1

        # 发送移动结束的结果和反馈
        rospy.loginfo("Move action completed.")
        self.cmd_vel_pub.publish(Twist())  # 停止机器人
        result.final_position = current_position
        self.action_server.set_succeeded(result, "Move action completed.")

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

上述示例中创建了一个MoveActionServer类，该类用于实现机器人移动的行为。在__init__函数中，首先创建了一个名为move_action的action服务器，并设置了执行回调函数execute_callback。然后通过self.cmd_vel_pub创建了一个发布速度控制指令的ROS话题。

在execute_callback函数中，首先对目标位置和移动速度进行读取。然后根据移动速度发布移动指令。接着进入一个循环中，判断机器人是否到达目标位置或是否有取消行为的请求。如果满足取消行为的条件，将停止机器人的移动，并发送取消行为的结果和反馈。如果机器人到达目标位置，则停止机器人的移动，并发送行为完成的结果和反馈。

最后，在主程序中初始化ROS节点，并实例化MoveActionServer类。然后通过rospy.spin()进入消息循环。

需要注意的是，上述示例中涉及的消息类型MoveAction、MoveGoal、MoveResult和MoveFeedback需要根据实际情况进行定义和导入。

这是一个简单的基本行为控制的示例，可以根据实际需求进行修改和扩展。在实际使用中，可以根据任务的不同，自定义各种行为并实现相应的控制逻辑。