====== Creating process modules ======

**Description:** in this tutorial you will learn about CRAM process modules and write a simple one to move the turtlesim.

**Previous Tutorial:** [[tutorials:beginner:designators|Creating action designators for the turtlesim]]\\
**Next Tutorial:** [[tutorials:beginner:location_designators|Using location designators with the turtlesim]]

===== Process modules: an overview =====

A process module is a program that controls a robot actuator. Different robots will have different kinds of actuators, requiring different kinds of controllers, and we would like to abstract away such specifics at the high level of task specification. When we ask a robot to clean a kitchen, we don't care that it has two arms or one; rather, as long as it has some capacity to manipulate objects, and some ability to move around, we can issue the cleaning task to it. Process modules allow us this flexibility by providing a well-defined, robot-independent interface to high level planning which works as an abstraction layer over the robot controllers. You can read some more about process modules in the [[http://cram-system.org/doc/package/cram_process_modules|documentation of the package]].

For better organization, process modules can also be referenced by aliases that describe what kind of function they can perform. For example, currently in the ''cram_pr2'' stack for the PR2 mobile manipulation platform the following aliases are defined:

  * :navigation
  * :ptu
  * :manipulation
  * :perception

A plan will typically refer to a process module by the alias. The process module would be defined for the particular robot in use and associated with the proper alias when the system is initialized.

Here's an example of invoking a process module:

<code lisp>
(with-designators
    ((my-designator :location '((:close-to :fridge))))
  (pm-execute :navigation my-designator))
</code>

This will run a process module associated with '':navigation'' and use ''my-designator'' as an input parameter. In this case, the designator is a semantic description of a target location and it's up to the process module to ask that this designator be resolved (by some appropriate other module in the system) so as to get an actual location and then control the robot to reach the target. The specifics of the controller may vary enormously; the robot might be differential drive, or legged. But these details are not important for this high level plan. All we want is for the robot to approach the fridge, in whatever way it can carry itself there.

===== Writing a process module for the turtlesim =====

For this tutorial we will use the [[http://wiki.ros.org/actionlib_lisp/Tutorials/actionlibBasicUsage|ActionLib]] turtlesim server for having a turtle draw a shape.

Once again, some new dependencies must be declared in the tutorial files you've been working on. 

In your ''package.xml'' file you need to add build and runtime dependencies on ''actionlib_lisp'', ''actionlib_msgs'', ''turtle_actionlib'' and ''cram_process_modules'':

<code>
  <build_depend>actionlib_lisp</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <build_depend>turtle_actionlib</build_depend>
  <build_depend>cram_process_modules</build_depend>

  <run_depend>actionlib_lisp</run_depend>
  <run_depend>actionlib_msgs</run_depend>
  <run_depend>turtle_actionlib</run_depend>
  <run_depend>cram_process_modules</run_depend>
</code>

Similarly, in your ''.asd'' file you should add ''actionlib'', ''actionlib_msgs-msg'', ''turtle_actionlib-msg'', ''cram-process-modules'' and ''cram-language-designator-support'' to the '':depends-on'' list. Let's also create two new source files in the ''src'' directory of your tutorial, call them ''turtle-action-client.lisp'' and ''process-modules.lisp'', and add them to your ''*.asd'' file, which should now look like this:

<code lisp>
(defsystem cram-beginner-tutorial
  :depends-on (cram-language roslisp turtlesim-msg geometry_msgs-msg cl-transforms
                             cram-designators cram-prolog
                             actionlib actionlib_msgs-msg turtle_actionlib-msg
                             cram-process-modules cram-language-designator-support)
  :components
  ((:module "src"
            :components
            ((:file "package")
             (:file "control-turtlesim" :depends-on ("package"))
             (:file "simple-plans" :depends-on ("package" "control-turtlesim"))
             (:file "action-designators" :depends-on ("package"))
             (:file "turtle-action-client" :depends-on ("package"))
             (:file "process-modules" :depends-on ("package"
                                                   "control-turtlesim"
                                                   "simple-plans"
                                                   "action-designators"
                                                   "turtle-action-client"))))))
</code>

Finally, let's also add '':cram-process-modules'' and ''cram-language-designator-support'' to the use list of our Lisp package for convenience:
<code lisp>
  (:use :cpl :roslisp :cl-transforms :cram-designators :cram-process-modules
        :cram-language-designator-support)
</code>


==== A process module for the turtlesim ====

We first need to connect to the turtlesim action server with an ActionLib client. To do this, append the following to your ''turtle-action-client.lisp'':

<code lisp>
(in-package :tut)

(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*)
</code>

The above code simply declares a variable ''*navp-client*'' which we can then initialize with a pointer to our ROS action client and retrieve this pointer later with the ''init-action-client'' and ''get-action-client functions'', respectively.

We will need an action goal for our action client that should represent a shape (which is specified by a number of edges and a radius), so append this to the ''turtle-action-client.lisp'':

<code lisp>
(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)
      (with-failure-handling
          ((simple-error (e)
             (format t "An error occured!~%~a~%Reinitializing...~%~%" e)
             (setf *navp-client* nil)
             (retry)))
        (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)))
</code>

In the above code we define a simple function to convert an edge and radius pair of values into a goal for the action client, and a function that will call said action client, with some error handling built in (for example, if the function is called without an action client being defined, it will (re)initialize one and retry).

Now that the lower level of controlling the TurtleSim is taken care of, it's finally time to look at process modules and their interface to the higher levels. Append the following to your ''process-modules.lisp'' file:

<code lisp>
(in-package :tut)

(def-process-module actionlib-navigation (action-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "Turtle shape navigation invoked with action designator `~a'."
                    action-designator)
  (destructuring-bind (command action-goal) (reference action-designator)
    (ecase command
      (draw-shape
       (call-shape-action
        :edges (turtle-shape-edges action-goal)
        :radius (turtle-shape-radius action-goal))))))

(defmacro with-turtle-process-modules (&body body)
  `(with-process-modules-running
       (actionlib-navigation)
     ,@body))
</code>

First, we use the ''cram-process-modules:def-process-module'' macro to define ''actionlib-navigation'' as a process module taking one parameter (''action-designator''). The process module then chooses which action to perform depending on the command specified in the designator: ''destructuring-bind'' maps the results from ''(reference action-designator)'' to the variables ''command'' and ''action-goal'' respectively. Note that the inference rules we defined previously provide a name for the kind of action goal we have (currently, all are ''draw-shape''), and a ''turtle-shape'' object. We run an ''ecase'' on the kind of goal (currently, we only have the shape case) and use ''call-shape-action'' to tell the lower level to move the turtle around, given these parameters we infer from designator resolution.

The ''with-turtle-process-modules'' macro is a macro we define for convenience. It allows us to set up a context in which to run commands, knowing that the turtle process modules are all running concurrently. Right now we only have one defined, ''actionlib-navigation''. When we will have several, we will add them to the list we pass to ''cram-process-modules:with-process-modules-running''. Note that the ''with-process-modules-running'' macro (and therefore ''with-turtle-process-modules'' too) needs to be run inside a ''top-level'' form. We will see this below.

Let's try this out. Make sure you have ''roscore'', ''turtlesim'', and ''turtle_actionlib'' running. In a terminal tab for each,

<code>
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun turtle_actionlib shape_server
</code>

For convenience, let's append one more function to ''process-modules.lisp'' that will do all our calls for us:

<code lisp>
(defun draw-hexagon (radius)
  (let ((turtle-name "turtle1"))
    (start-ros-node turtle-name)
    (init-ros-turtle turtle-name)
    (top-level
      (with-turtle-process-modules
        (process-module-alias :navigation 'actionlib-navigation)
        (with-designators
            ((trajectory :action `((:type :shape) (:shape :hexagon) (:radius ,radius))))
          (pm-execute :navigation trajectory))))))
</code>

What the function does is simply start a ROS node for our turtle and sets up the action client, then activates the turtle process modules (in our case, the sole existing one), creates a designator to describe how the turtle should move, and calls the ''cram-process-modules:pm-execute'' macro to have the process module follow the trajectory specified by the designator. 

Two other things to observe here are the use of the ''top-level'' macro, which sets up a CRAM running context and is needed for ''with-turtle-process-modules''. Also, we declare that '':navigation'' is the alias for our ''actionlib-navigation'' process module. That way, our plan can invoke a generic name like '':navigation'', rather than a robot-specific one like ''actionlib-navigation''.

Reload the tutorial in REPL, and let's try to start the tutorial

<code lisp>
TUT> (draw-hexagon 2)
[(ROSLISP TOP) INFO] 1453721727.445: Node name is /turtle1
[(ROSLISP TOP) INFO] 1453721727.446: Namespace is /
[(ROSLISP TOP) INFO] 1453721727.446: Params are NIL
[(ROSLISP TOP) INFO] 1453721727.446: Remappings are:
[(ROSLISP TOP) INFO] 1453721727.446: master URI is 127.0.0.1:11311
[(ROSLISP TOP) INFO] 1453721728.452: Node startup complete
[(TURTLE-PROCESS-MODULES) INFO] 1453721728.467: Turtle shape navigation invoked with action designator `#<ACTION-DESIGNATOR ((TYPE
                                                                        SHAPE)
                                                                       (SHAPE
                                                                        HEXAGON)
                                                                       (RADIUS
                                                                        2)) {10056DB513}>'.
An error occured!
Client lost connection to server.
Reinitializing...

[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721736.385: Waiting for turtle shape action server...
[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721738.386: Turtle shape action client created.
[(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721761.206: Nav action finished.
[TURTLE_ACTIONLIB-MSG:SHAPERESULT
   INTERIOR_ANGLE:
     2.094395160675049d0
   APOTHEM:
     1.7320507764816284d0]
:SUCCEEDED
</code>

You should also see the turtle move in the TurtleSim window and trace the required trajectory.

== Next ==

Let's have a look at location designators and other ways to move the turtle, as well as have some more practice with designator resolution and process modules ...

[[tutorials:beginner:location_designators|Using location designators with the turtlesim]]