## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note = 
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0=[[actionlib_lisp/Tutorials/actionLibServer|Writing a Simple Action Server using the Execute Callback (Common Lisp)]]
## descriptive title for the tutorial
## title = Writing a Simple Action Client (Common Lisp)
## multi-line description to be displayed in search 
## description = This tutorial gives a brief intro to writing clients for actionlib servers
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[cl_tf/Tutorials/clTfBasicUsage|Using TF with CL_TF]]
## what level user is this tutorial for 
## level = IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>



== Writing a client for the tutorial Fibonacci server ==
The actionlib tutorials for c++ and python declare a [[actionlib/Tutorials|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.


## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE