用ROS 的 action程序实现倒计时

好的，为您介绍一个用ROS actionlib实现倒计时的程序：

#!/usr/bin/env python

import rospy
import time
import actionlib
from std_msgs.msg import Int32
from actionlib_msgs.msg import *
from beginner_tutorials.msg import CountdownAction, CountdownFeedback, CountdownResult

class CountdownServer(object):
    # create messages that are used to publish feedback/result
    _feedback = CountdownFeedback()
    _result = CountdownResult()

    def __init__(self):
        # creates the action server
        self._as = actionlib.SimpleActionServer("countdown_action", CountdownAction, self.execute, False)
        self._as.start()
        self._countdown_time = 0

    def execute(self, goal):
        # helper variables
        success = True
        self._countdown_time = goal.countdown_time
        # publish info to the console for the user
        rospy.loginfo('Starting countdown for %d seconds' % self._countdown_time)

        # start executing the action
        for i in range(self._countdown_time, 0, -1):
            # check that preempt has not been requested by the client
            if self._as.is_preempt_requested():
                rospy.loginfo('Countdown preempted')
                self._as.set_preempted()
                success = False
                break
            rospy.loginfo('Countdown: %d' % i)
            self._feedback.countdown_progress = i
            self._as.publish_feedback(self._feedback)
            time.sleep(1)

        if success:
            self._result.countdown_result = "Countdown finished"
            rospy.loginfo('Countdown finished')
            self._as.set_succeeded(self._result)

if __name__ == '__main__':
    rospy.init_node('countdown_server')
    server = CountdownServer()
    rospy.spin()

这个程序使用了ROS的actionlib库，创建了一个名为 `countdown_action` 的action server，接收一个整数类型的倒计时时间作为goal，然后执行倒计时操作。在倒计时过程中，程序会不断地发送feedback，告诉客户端当前的倒计时进度。

您可以在客户端使用ROS的action client来调用这个服务。比如，可以这样调用：

#!/usr/bin/env python

import rospy
import actionlib
from beginner_tutorials.msg import CountdownAction, CountdownGoal, CountdownFeedback, CountdownResult

def feedback_cb(feedback):
    rospy.loginfo('Countdown progress: %d' % feedback.countdown_progress)

def countdown_client():
    # create the action client
    client = actionlib.SimpleActionClient('countdown_action', CountdownAction)

    # wait for the action server to start up
    client.wait_for_server()

    # create a goal
    goal = CountdownGoal()
    goal.countdown_time = 10

    # send the goal to the action server
    client.send_goal(goal, feedback_cb=feedback_cb)

    # wait for the server to finish performing the action
    client.wait_for_result()

    # print the result
    rospy.loginfo('Countdown result: %s' % client.get_result())

if __name__ == '__main__':
    rospy.init_node('countdown_client')
    countdown_client()

这个客户端会向名为 `countdown_action` 的action server 发送一个10秒的倒计时goal，并且注册了一个feedback回调函数，在倒计时过程中输出进度信息。