About | Support | answers.ros.org	Search:

Basic usage of actionlib with actionlib_lisp

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

Tutorial Level: INTERMEDIATE

Next Tutorial: Writing an actionlib server

Working with actionlib

Actionlib is a ros library that you are encouraged to use for any kind of action your robot could perform. Make yourself familiar with the concepts before doing this tutorial.

Writing a client for the tutorial Fibonacci server

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

The 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 tutorials, replace "actionlib_tutorials" with "learning_actionlib" also in all of the following.

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 the following.

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 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:

(defparameter *fibonacci-goal*
  (actionlib:make-action-goal *fibonacci-action-client*
                              order 20))

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

Writing a client for the turtlesim actions

The turtlesim simulator comes with a predefined tutorial action defined in the ROS package turtle_actionlib. 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.