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: Creating motion designators for the turtlesim
Next Tutorial: Automatically choosing a process module for a motion

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 documentation of the package.

Here's an example of invoking a process module:

(with-designators
    ((my-designator :location '((:close-to :fridge))))
  (pm-execute :navigation my-designator))

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

In this tutorial we will have a turtle drive by using a process module to execute a resolved motion designator.

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

  <build_depend>cram_process_modules</build_depend>

  <run_depend>cram_process_modules</run_depend>

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

(defsystem cram-beginner-tutorial
  :depends-on (cram-language roslisp turtlesim-msg geometry_msgs-msg cl-transforms
                             cram-designators cram-prolog
                             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 "motion-designators" :depends-on ("package"))
             (:file "process-modules" :depends-on ("package"
                                                   "control-turtlesim"
                                                   "simple-plans"
                                                   "motion-designators"))))))

Finally, let's also add :cram-process-modules and cram-language-designator-support to the use list of our Lisp package for convenience:

  (:use :cpl :roslisp :cl-transforms :cram-designators :cram-process-modules
        :cram-language-designator-support)

A process module for the turtlesim

In the previous tutorial we defined the function send-vel-command to publish a command for the turtle. We will use this function as the lower level of controlling the TurtleSim. Now it's time to look at process modules and their interface to the higher levels. Append the following to your process-modules.lisp file:

(in-package :tut)
 
(def-process-module turtlesim-navigation (motion-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "TurtleSim navigation invoked with motion designator `~a'."
                    motion-designator)
  (destructuring-bind (command motion) (reference motion-designator)
    (ecase command
      (drive
       (send-vel-cmd
        (turtle-motion-speed motion)
        (turtle-motion-angle motion))))))

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

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

$ roscore
$ rosrun turtlesim turtlesim_node

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

(defun drive (?speed ?angle)
  (top-level
    (with-process-modules-running (turtlesim-navigation)
      (let ((trajectory (desig:a motion (type driving) (speed ?speed) (angle ?angle))))
        (pm-execute 'turtlesim-navigation trajectory)))))

What the function does is simply activate 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.

The with-process-modules-running macro allows us to set up a context in which to run commands, knowing that the defined process modules are all running concurrently. Right now we only have one defined, turtlesim-navigation. When we have several, we can add them to the list we pass to cram-process-modules:with-process-modules-running. Note that the with-process-modules-running macro needs to be run inside a top-level form.

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

First, we need to start a node and initialize our parameters with init-ros-turtle from the tutorial about controlling the TurtleSim.

(start-ros-node "turtle1")
(init-ros-turtle "turtle1")

Then, we can call drive.

TUT> (drive 5 2)
[(TURTLE-PROCESS-MODULES) INFO] 1499958119.336: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           DRIVING)
                                                                          (SPEED
                                                                           5)
                                                                          (ANGLE
                                                                           2)) {1006979703}>'.
1

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

Adding more process modules for the TurtleSim

When adding new motions for a robot, we can either add them to a existing process module or write a new one. A process module only ever executes one designator at a time. This is to prevent unwanted behaviour when executing multiple designators in parallel or quick succession. But when a process module is called while still executing, the incoming call will be queued and executed as soon as the current execution is finished. For example, if we wanted the robot to grasp something and then stack it onto something else. If these motions were executed in parallel the arm would do neither because the commands would interfere with each other. Through the use of process modules, we don't have to worry about this. So to make this decision, we need to think about resources on the robot (eg. arms or the base of the robot).

Our turtlesim-navigation is for the (abstract) resource 'driving' of the turtle. We now want to add the motions for moving and for setting the pen. Former uses the same resource as the navigation process module, so we should add it to the existing process module. The latter however doesn't, so we should add it as a new process module.

To add the move motion to the existing process module, we add a new case to the ecase. It should look like this:

(def-process-module turtlesim-navigation (motion-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "TurtleSim navigation invoked with motion designator `~a'."
                    motion-designator)
  (destructuring-bind (command motion) (reference motion-designator)
    (ecase command
      (drive
         (send-vel-cmd
          (turtle-motion-speed motion)
          (turtle-motion-angle motion)))
      (move
       (move-to motion)))))

Since move-to takes a 3d-vector as a parameter we only have to pass motion to it.

To add the new process module, append this to your process-modules.lisp file.

(def-process-module turtlesim-pen-control (motion-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "TurtleSim pen control invoked with motion designator `~a'."
                    motion-designator)
  (destructuring-bind (command motion) (reference motion-designator)
    (ecase command
      (set-pen
       (call-set-pen
        (pen-motion-r motion)
        (pen-motion-g motion)
        (pen-motion-b motion)
        (pen-motion-width motion)
        (pen-motion-off motion))))))

The code should look very familiar, because it's pretty close to our other process modul

Executing process modules in parallel

To demonstrate how process modules work when called in parallel, we will call the same process module twice. You can execute the following in the REPL:

TUT> (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (let ((goal (desig:a motion (type moving) (goal (9 1 0))))
            (trajectory (desig:a motion (type driving) (speed 3) (angle 8))))
        (cpl:par
          (pm-execute 'turtlesim-navigation goal)
          (pm-execute 'turtlesim-navigation trajectory)))))
[(TURTLE-PROCESS-MODULES) INFO] 1500997686.711: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (9 1
                                                                            0))) {1006DBC893}>'.
WARNING:
   Process module #<TURTLESIM-NAVIGATION
                    {10074C0293}> already processing input. Waiting for it to become free.
[(TURTLE-PROCESS-MODULES) INFO] 1500997690.065: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           DRIVING)
                                                                          (SPEED
                                                                           3)
                                                                          (ANGLE
                                                                           8)) {1006DBCC73}>'.
T

Here we use the par macro to call the same process module twice in parallel. When looking at the turtle we see, that it first moves to the bottom-right corner and then drives in a circle for a second. So the two designators were not executed in parallel because they were executed with the same process module. Also a warning gets printed stating, that the program waits for the process module to be free before executing the next designator.

If we use two different process modules, they can in fact be called in parallel:

TUT> (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (let ((goal (desig:a motion (type moving) (goal (9 9 0)))))
        (cpl:par
          (pm-execute 'turtlesim-navigation goal)
          (dotimes (i 10)
            (pm-execute 'turtlesim-pen-control
                      (let ((?r (random 255))
                            (?g (random 255))
                            (?b (random 255))
                            (?width (+ 3 (random 5))))
                      (desig:a motion (type setting-pen) (r ?r) (g ?g) (b ?b) (width ?width))))
            (sleep 0.5))))))
[(TURTLE-PROCESS-MODULES) INFO] 1500997786.329: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (9 9
                                                                            0))) {1005164AC3}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1500997786.347: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (R
                                                                            220)
                                                                           (G
                                                                            115)
                                                                           (B
                                                                            14)
                                                                           (WIDTH
                                                                            7)) {10072183E3}>'.
 
[ ... ]
 
[(TURTLE-PROCESS-MODULES) INFO] 1500997791.126: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (R
                                                                            225)
                                                                           (G
                                                                            228)
                                                                           (B
                                                                            156)
                                                                           (WIDTH
                                                                            3)) {1002B903C3}>'.
T

Here we call the navigation process module to move the turtle to the upper right corner and our other process module to change the pen color and width twice every second. When looking at the turtle we can see it moving to the upper right corner while randomly changing its pen.

As stated above this behaviour is to ensure that a single resource on a robot isn't used by multiple functions at once, while not hindering the parallel execution of independent resources. But keep in mind, that this is only true for the low-level motions. If a robots arms should grasp something and its base is moved, the grasping might fail, although these motions are not necessarily controlled by the same process module.

Next

So far we called process modules directly. Sometimes it's better to let the system decide on its own …

Automatically choosing a process module for a motion