(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a Simple Action Server using the Execute Callback (Python)

Description: This tutorial covers using the simple_action_server library to create a Fibonacci action server in Python. This example action server generates a Fibonacci sequence, the goal is the order of the sequence, the feedback is the sequence as it is computed, and the result is the final sequence.

Tutorial Level: BEGINNER

Next Tutorial: Writing a Simple Action Client (Python)

Creating the Action Messages

See C++ version tutorial and come back when you've finished creating messages.

Writing a Simple Server

The code and examples used in this tutorial can be found in the actionlib_tutorials package. You may want to read about the actionlib package before starting this tutorial.

The Code

The following code can be found in actionlib_tutorials/simple_action_servers/fibonacci_server.py, and implements a python action server for the fibonacci action.

   1 #! /usr/bin/env python
   2 
   3 import rospy
   4 
   5 import actionlib
   6 
   7 import actionlib_tutorials.msg
   8 
   9 class FibonacciAction(object):
  10     # create messages that are used to publish feedback/result
  11     _feedback = actionlib_tutorials.msg.FibonacciFeedback()
  12     _result = actionlib_tutorials.msg.FibonacciResult()
  13 
  14     def __init__(self, name):
  15         self._action_name = name
  16         self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
  17         self._as.start()
  18       
  19     def execute_cb(self, goal):
  20         # helper variables
  21         r = rospy.Rate(1)
  22         success = True
  23         
  24         # append the seeds for the fibonacci sequence
  25         self._feedback.sequence = []
  26         self._feedback.sequence.append(0)
  27         self._feedback.sequence.append(1)
  28         
  29         # publish info to the console for the user
  30         rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
  31         
  32         # start executing the action
  33         for i in range(1, goal.order):
  34             # check that preempt has not been requested by the client
  35             if self._as.is_preempt_requested():
  36                 rospy.loginfo('%s: Preempted' % self._action_name)
  37                 self._as.set_preempted()
  38                 success = False
  39                 break
  40             self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
  41             # publish the feedback
  42             self._as.publish_feedback(self._feedback)
  43             # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
  44             r.sleep()
  45           
  46         if success:
  47             self._result.sequence = self._feedback.sequence
  48             rospy.loginfo('%s: Succeeded' % self._action_name)
  49             self._as.set_succeeded(self._result)
  50         
  51 if __name__ == '__main__':
  52     rospy.init_node('fibonacci')
  53     server = FibonacciAction(rospy.get_name())
  54     rospy.spin()

The Code, explained

   6 import actionlib

This line imports the actionlib library used for implementing simple actions.

   7 import actionlib_tutorials.msg

The action specification generates several messages for sending goals, receiving feedback, etc... This line imports the generated messages.

  17         self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
  18         self._as.start()

Here, the SimpleActionServer is created, we pass it a name (used as a namespace), an action type, and optionally an execute callback. Since we've specified an execute callback in this example, a thread will be spun for us which allows us to take long running actions in a callback received when a new goal comes in.

Note you should always set auto_start to False explicitly, unless you know what you're doing (ref).

  20     def execute_cb(self, goal):

This is the execute callback function that we'll run everytime a new goal is received.

  22         r = rospy.Rate(1)
  23         success = True
  24         
  25         # append the seeds for the fibonacci sequence
  26         self._feedback.sequence = []
  27         self._feedback.sequence.append(0)
  28         self._feedback.sequence.append(1)
  29         
  30         # publish info to the console for the user
  31         rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))

Here, the internals of the action are created. In this example rospy.loginfo is published to let the user know that the action is executing.

  33         # start executing the action
  34         for i in range(1, goal.order):
  35             # check that preempt has not been requested by the client
  36             if self._as.is_preempt_requested():
  37                 rospy.loginfo('%s: Preempted' % self._action_name)
  38                 self._as.set_preempted()
  39                 success = False

An important component of an action server is the ability to allow an action client to request that the goal under execution be canceled. When a client requests that the current goal be preempted, the action server should cancel the goal, perform any necessary cleanup, and call the set_preempted function, which signals that the action has been preempted by user request. Here, we'll check if we've been preempted every second. We could, alternatively, receive a callback when a preempt request is received.

  41             self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
  42             # publish the feedback
  43             self._as.publish_feedback(self._feedback)

Here, the Fibonacci sequence is put into the feedback variable and then published on the feedback channel provided by the action server. Then, the action continues looping and publishing feedback.

  47         if success:
  48             self._result.sequence = self._feedback.sequence
  49             rospy.loginfo('%s: Succeeded' % self._action_name)
  50             self._as.set_succeeded(self._result)

Once the action has finished computing the Fibonacci sequence, the action server notifies the action client that the goal is complete by calling set_succeeded.

  52 if __name__ == '__main__':
  53     rospy.init_node('fibonacci')
  54     server = FibonacciAction(rospy.get_name())
  55     rospy.spin()

Finally, the main function, creates the action server and spins the node.

Compiling

Only initially when you just created your tutorial package, you need to compile to generate shell config files.

cd %TOPDIR_YOUR_CATKIN_WORKSPACE%
catkin_make
source devel/setup.bash

Running the Action Server

Run:

roscore

Then on a new terminal, the following command will run the action server.

rosrun actionlib_tutorials fibonacci_server.py

Wiki: actionlib_tutorials/Tutorials/Writing a Simple Action Server using the Execute Callback (Python) (last edited 2023-03-08 08:53:20 by Hirotaka Yamada)