About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Writing a Simple Action Client (Common Lisp)

Description: This tutorial gives a brief intro to writing clients for actionlib servers

Tutorial Level: INTERMEDIATE

Next Tutorial: Using TF with CL_TF

Writing a client for the tutorial Fibonacci server

The actionlib tutorials for c++ and python declare a fibonacci server example. Here is an example for a Lisp action client compatible with the Fibonacci server tutorial example.

The actionlib_tutorial is part of ROS and can be started with the following commands in two terminals

$ roscore
$ rosrun actionlib_tutorials fibonacci_server

Alternatively, to use a server you wrote when following the actionlib_lisp tutorials, replace "actionlib_tutorials" with "learning_actionlib" in all of the following pieces of code.

Consider the contents of the /actionlib_tutorials/Fiobnacci.action file:

#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence 

You will need to know the name of the goal and feedback fields, "order" and "sequence" in this case.

Start a REPL, and load actionlib and the fibonacci action definition:

CL-USER> (ros-load:load-system "actionlib_lisp" "actionlib")
CL-USER> (ros-load:load-system "actionlib_tutorials" "actionlib_tutorials-msg")

Now start a node in the REPL, as we need a running node to create an action client.

CL-USER> (roslisp:start-ros-node "fibonacci_lisp_client")

The name is arbitrary.

next create the action client.

CL-USER> (defparameter *fibonacci-action-client*
           (actionlib:make-action-client 
            "fibonacci" 
            "actionlib_tutorials/FibonacciAction"))
*FIBONACCI-ACTION-CLIENT*
CL-USER> (actionlib:wait-for-server *fibonacci-action-client*)
T

The first call creates the action client. Again we pass the name of the action "fibonacci". And the actiontype, which is the combination of packagename + / + filename of the .action file + "Action".

If you've got an error like "Package actionlib_tutorials-msg not found" then you missed the ros-load:load-system command above.

We can define a callback function for the feedback messages:

CL-USER> (defun fibonacci-feedback-cb (msg)
           (roslisp:with-fields (sequence)
               msg
             (format t "~a~%" sequence)))

We use the name of the sequence field in the Fibonacci.action file to access the part of the feedback message we care about, and the callback just prints the feedback.

This is how to create a message for the action, using the client definition:

CL-USER> (defparameter *fibonacci-goal*
           (actionlib:make-action-goal *fibonacci-action-client*
             order 10))

Note that we use the goal message type here, "FibonacciGoal". This has one field "order", as defined in the Fibonacci.action file. You can also use the rosmsg command to check:

$ rosmsg show actionlib_tutorials/FibonacciGoal 
int32 order

Finally we call the action with the goal:

CL-USER> (actionlib:call-goal 
          *fibonacci-action-client* 
          *fibonacci-goal*  
          :feedback-cb 'fibonacci-feedback-cb)
#(0 1 1)
#(0 1 1 2)
#(0 1 1 2 3)
#(0 1 1 2 3 5)
#(0 1 1 2 3 5 8)
#(0 1 1 2 3 5 8 13)
#(0 1 1 2 3 5 8 13 21)
#(0 1 1 2 3 5 8 13 21 34)
#(0 1 1 2 3 5 8 13 21 34 55)
#(0 1 1 2 3 5 8 13 21 34 55 89)
[ACTIONLIB_TUTORIALS-MSG:<FIBONACCIRESULT>
   SEQUENCE:
     #(0 1 1 2 3 5 8 13 21 34 55 89)]
:SUCCEEDED

Sometimes, users might want to cancel a goal from client-side based on the content of the feedback. Actionlib_lisp supports such behavior. As an example, we will abort the action from client-side once the feedback sequence has more than 5 elements.

Basically, call-goal always raises the signal feedback-signal every time it receives a feedback message. This is independent of whether a feedback callback was provided when calling call-goal. feedback-signal is connect to a restart-case abort-goal which does exactly that, aborting the goal.

We first define a second callback which process a signal of type feedback-signal and aborts the action once the feedback sequence is longer than 5:

CL-USER> (defun aborting-feedback-cb (signal)
           (with-slots ((goal-handle actionlib::goal) 
                        (feedback-msg actionlib::feedback))
               signal
             (declare (ignore goal-handle))
             (roslisp:with-fields (sequence) 
                 feedback-msg
               (when (< 5 (length sequence))
                 (invoke-restart 'actionlib:abort-goal)))))

As you can see, feedback-signal itself has two slots: The goal-handle which we do not use here, and a copy of the feedback message the action-client just received. When the desired abort condition is fulfilled we invoke a restart on abort-goal which cancels the goal.

Finally, we wrap our call to call-goal into a handler-bind which associates aborting-feedback-cb with feedback-signal:

CL-USER> (handler-bind ((actionlib:feedback-signal #'aborting-feedback-cb))
           (actionlib:call-goal 
            *fibonacci-action-client* 
            *fibonacci-goal*  
            :feedback-cb 'fibonacci-feedback-cb))
#(0 1 1)
#(0 1 1 2)
#(0 1 1 2 3)
#(0 1 1 2 3 5)
NIL
:ABORTED

Our new callback cancelled the goal once the feedback sequence reached 6 elements. Interestingly, we still see the printout of the intermediate feedback messages. Our original feedback callback fibonacci-feedback-cb generated those. Obviously, we have both of our feedback callbacks operating on every message. The callback provided as parameter to call-goal will always preceed the handler-bind callback.

Writing a client for the turtlesim actions

The turtlesim simulator comes with a predefined tutorial action defined in the ROS package turtle_actionlib (you can find a link to the code in the wiki of the package). We can write a client against this action. Check the action first:

$ rosed turtle_actionlib Shape.action
#goal definition
int32 edges
float32 radius
---
#result definition
float32 interior_angle
float32 apothem
---
#feedback

The action has as inputs the number of edges and radius of a polygon shape.

Start a roscore, a turtlesim, and the actionsserver in 3 terminals:

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun turtle_actionlib shape_server

The only other thing that we need to know is that the turtle actionlib package uses "turtle_shape" as namespace for its topics, which we can find out by starting the action server and calling

$ rostopic list
/rosout
/rosout_agg
/turtle1/color_sensor
/turtle1/command_velocity
/turtle1/pose
/turtle_shape/cancel
/turtle_shape/feedback
/turtle_shape/goal
/turtle_shape/result
/turtle_shape/status

You can see here that the action uses "turtle_shape" as namespace for the action.

The code

Here is a set of functions calling the action:

(defvar *navp-client* nil)

(defun init-action-client ()
  (setf *navp-client* (actionlib:make-action-client
                       "turtle_shape"
                       "turtle_actionlib/ShapeAction"))
  (roslisp:ros-info (turtle-shape-action-client) 
                    "Waiting for turtle shape action server...")
  ;; workaround for race condition in actionlib wait-for server
  (loop until
    (actionlib:wait-for-server *navp-client*))
  (roslisp:ros-info (turtle-shape-action-client) 
                    "Turtle shape action client created."))

(defun get-action-client ()
  (when (null *navp-client*)
    (init-action-client))
  *navp-client*)

(defun make-shape-action-goal (in-edges in-radius)
  (actionlib:make-action-goal (get-action-client)
                        edges in-edges
                        radius in-radius))

(defun call-shape-action (&key edges radius)
  (multiple-value-bind (result status)
      (let ((actionlib:*action-server-timeout* 10.0))
        (actionlib:call-goal
         (get-action-client)
         (make-shape-action-goal edges radius)))
    (roslisp:ros-info (turtle-shape-action-client) "Nav action finished.")
    (values result status)))

The code explained

To run this example, you will need to load the code in a REPL, or define an executable as explained in the roslisp tutorial. functions init-action-client and get-action-client make sure an action client is started when a call gets through.

Function call-shape-action takes a shape structure as argument, creates a ROS actionlib goal message, calls the goal using the action client, and returns the result and status.

When you have loaded the code, you can run it by calling it like this:

CL-USER> (ros-load:load-system "actionlib_lisp" "actionlib")
CL-USER> (ros-load:load-system "turtle_actionlib" "turtle_actionlib-msg")
CL-USER> (roslisp:start-ros-node "turtle-lisp-client")
...
CL-USER> (call-shape-action :edges 3 :radius 1.0)
[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1298126829.929: Waiting for turtle shape action server...
[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1298126832.055: Turtle shape action client created.
[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1298126843.610: Nav action finished.
[TURTLE_ACTIONLIB-MSG:<SHAPERESULT>
   INTERIOR_ANGLE:
     1.0471975803375244d0
   APOTHEM:
     0.5d0]
:SUCCEEDED

Remember a ROS node needs to be started in order to create an action client.

Controlling the torso of PR2

In preparation of the calls in this example, we need to load actionlib and message definitions using

CL-USER> (ros-load:load-system "actionlib_lisp" "actionlib")
CL-USER> (ros-load:load-system "pr2_controllers_msgs" "pr2_controllers_msgs-msg")

or work in a lisp package which depends on actionlib and pr2_controllers_msgs-msg.

Starting up and preparing Gazebo

If you cannot work on a real PR2, you will have to start gazebo and the required controllers.

This is using the PR2 torso controller actions. We will use these for the next few steps, so you may want to apply those commands on a PR2 or in gazebo. We follow the tutorial on moving the torso at pr2_controllers/Tutorials/Moving the torso. For gazebo, e.g. run:

$ roslaunch pr2_gazebo pr2_empty_world.launch

Before continuing, check that the torso-controller is properly running using:

$ rosrun pr2_controller_manager pr2_controller_manager list

Look for "torso_controller ( running )"

Calling the action from Lisp

To work with actions in roslisp, you need an actionlib client. You can create one using (actionlib:make-action-client action-name action-type)

Note that a ros node must have been started for this to work, so if you did not start one before, do so now

CL-USER> (roslisp:start-ros-node "spine-lisp-client")

Let's create such a client:

CL-USER> (defparameter *spine-action-client*
   (actionlib:make-action-client 
     "/torso_controller/position_joint_action"          
     "pr2_controllers_msgs/SingleJointPositionAction"))
(actionlib:wait-for-server *spine-action-client*)

The first function call creates an action client, the second one is recommended before using a client.

The first parameter "/torso_controller/position_joint_action" is the action namespace, as defined by the action server when the action was created. When the action server is running, you can find out the namespace by running rostopic list. The list should contain several topics for the action, prefixed by the namespace.

The second parameter is the type of the action, i.e. the name of the ros package + '/' + filename of the .action file + "Action".

To send goal to the action server, we use  (actionlib:call-goal client goal &key feedback-cb timeout) 

We need to build a goal-message for that, those are plain roslisp messages, so we for the action above, we can create a goal like this:

CL-USER> (defparameter *spine-goal*
  (actionlib:make-action-goal *spine-action-client*
                              position 0.18
                              max_velocity 1.0))

The spine action returns feedback to any client, we can use a callback function to keep ourselves updated:

CL-USER> (defun spine-feedback-cb (msg)
  (roslisp:with-fields (velocity)
      msg
    (format t "~a~%" velocity)))

Putting those together we can now call the action:

CL-USER> (actionlib:call-goal *spine-action-client* *spine-goal* :feedback-cb 'spine-feedback-cb)

You should now see the PR2 moving its torso all the way up, and see outputs in the REPL giving the current velocity feedback.