====== Writing plans for the TurtleSim ======

**Description:** In this tutorial you will learn what action designators are and write high-level plans: you will learn how to define action designators and how to use them.

**Previous Tutorial:** [[tutorials:beginner:location_designators_2|Using location designators with the TurtleSim]]\\
**Next Tutorial:** [[tutorials:beginner:failure_handling|Implementing failure handling for the TurtleSim]]

To run the code in the tutuorial the roscore and the turtlesim need to be started over the terminal. Each in their own tab.
<code bash>
$ roscore
</code>
<code bash>
$ rosrun turtlesim turtlesim_node
</code>

And in the REPL the following commands should be executed:
<code lisp>
CL-USER>(ros-load:load-system "cram_my_beginner_tutorial" :cram-my-beginner-tutorial)
...
CL-USER>(in-package :tut)
...
TUT>(start-ros-node "turtle1")
</code>
===== Designators: Actions vs Motions =====

On a pragmatic level there is not a big difference between action and motion designators since both are a set of key-value pairs. But the difference lies in the semantics. As mentioned in an earlier tutorial about motion designators, they are meant to represent actual motions of the robot. Like moving an arm or driving around. The action designators on the other hand should be used to describe abstract plans (e.g. tidying up a room).

This difference comes to play when designators are ''performed''. The ''perfom'' function handles action designators differently than motion designators. Both are first referenced and then motion designators are executed by passing them to a matching process module. Action designators are executed by looking up a function with the same name as the resolved designator. The rest of the designator is passed as arguments to the function. Let's take a designator ''(desig:an action (type tidying-up) (what the-room))''. This could be resolved to something like ''(tidy-up the-room)''. So there has to be a function ''tidy-up'' which takes one parameter (''the-room''), for ''perform'' to be able to execute the designator.
Action designators can optionally contain a ''goal''-key which would be checked to see if the goal has been already achieved, so the action doesn't have to be performed. But this will not be covered in this tutorial.


===== The plan: Drawing a house =====

The plan we want to implement is for drawing a house using the turtles pen. We already have all the low-level functions we need to achieve this. Now we need to define action designators for this plan. In simple terms a house consists of some rectangles and a triangle for the roof. We can define a plan to simply trace a set of vertices to draw the simple shapes. In our overall plan, we will call this other plan to draw the parts of the house.


==== Defining the action designators ====

Let's create new source files for the code in this tutorial (under the ''src'' directory), call them ''action-designators.lisp'' and ''high-level-plans.lisp''. We will need to add the files to the '':components'' of the ''src'' module in your ''cram-my-beginner-tutorial.asd'', which should now look something like this:

<code lisp>
(defsystem cram-my-beginner-tutorial
  :depends-on (roslisp cram-language turtlesim-msg turtlesim-srv cl-transforms geometry_msgs-msg cram-designators cram-prolog
                       cram-process-modules cram-language-designator-support cram-executive)
  :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 "location-designators" :depends-on ("package"))
             (:file "action-designators" :depends-on ("package"))
             (:file "process-modules" :depends-on ("package"
                                                   "control-turtlesim"
                                                   "simple-plans"
                                                   "motion-designators"))
             (:file "selecting-process-modules" :depends-on ("package"
                                                             "motion-designators"
                                                             "process-modules"))
             (:file "high-level-plans" :depends-on ("package"
                                                    "motion-designators"
                                                    "location-designators"
                                                    "action-designators"
                                                    "process-modules"))))))
</code>

Now, reload the tutorial in ''roslisp_repl'' (which also loads the newly added dependencies).

Since we already covered creating designators in previous tutorials here's just a quick example of what we will be using for the plans.

<code list>
TUT> (desig:an action (type drawing) (shape rectangle) (width 5) (height 4))

#<A ACTION
    (TYPE DRAWING)
    (SHAPE RECTANGLE)
    (WIDTH 5)
    (HEIGHT 4)>
</code>


==== Defining inference rules for designators ====

Append the following to your ''action-designators.lisp'' file:

<code lisp>
(in-package :tut)

(defun get-shape-vertices (type &rest parameters)
  (with-fields (x y)
      (value *turtle-pose*)
    (ecase type
      (:triangle
       (let ((base-width (first parameters))
             (height (second parameters)))
         (list
          (list (+ x base-width) y 0)
          (list (+ x (/ (float base-width) 2)) (+ y height) 0)
          (list x y 0))))
      (:rectangle
       (let ((width (first parameters))
             (height (second parameters)))
         (list
          (list (+ x width) y 0)
          (list (+ x width) (+ y height) 0)
          (list x (+ y height) 0)
          (list x y 0)))))))

(def-fact-group turtle-action-designators (action-grounding)

  (<- (desig:action-grounding ?action-designator (navigate ?action-designator))
     (desig-prop ?action-designator (:type :navigating))
     (desig-prop ?action-designator (:target ?target)))
 
  (<- (desig:action-grounding ?action-designator (draw-house ?action-designator))
    (desig-prop ?action-designator (:type :drawing))
    (desig-prop ?action-designator (:shape :house)))

  (<- (desig:action-grounding ?action-designator (draw-simple-shape ?resolved-action-designator))
    (desig-prop ?action-designator (:type :drawing))
    (desig-prop ?action-designator (:shape :rectangle))
    (desig-prop ?action-designator (:width ?width))
    (desig-prop ?action-designator (:height ?height))
    (lisp-fun get-shape-vertices :rectangle ?width ?height ?vertices)   
    (desig:designator :action ((:type :drawing)
                              (:vertices ?vertices))
                      ?resolved-action-designator))

  (<- (desig:action-grounding ?action-designator (draw-simple-shape ?resolved-action-designator))
    (desig-prop ?action-designator (:type :drawing))
    (desig-prop ?action-designator (:shape :triangle))
    (desig-prop ?action-designator (:base-width ?base-width))
    (desig-prop ?action-designator (:height ?height))    
    (lisp-fun get-shape-vertices :triangle ?base-width ?height ?vertices)
    (desig:designator :action ((:type :drawing)
                              (:vertices ?vertices))
                      ?resolved-action-designator)))

</code>

Let's see what this code does. The function ''get-shape-vertices'' simply returns a list of vertices for the draw-simple-shape plan to trace. It uses the current position of the turtle to calculate those points.

The inference is simple again: it maps the shape value to the right call to ''get-shape-vertices'' to get the right list of vertices. For the ''navigate'' plan it simply extracts the target point from the designator.

Reload the tutorial package in ''roslisp_repl''. This will also load the newly defined inference rules.

Let's try to create and reference designators in the REPL command line (don't forget to initialize the turtle first):

<code lisp>
TUT> (init-ros-turtle "turtle1")
"/turtle1/set_pen"
TUT> (reference (desig:an action (type drawing) (shape rectangle) (width 5) (height 4)))
(DRAW-SIMPLE-SHAPE
 #<A ACTION
    (TYPE DRAWING)
    (VERTICES ((9.255966663360596d0 10.373946189880371d0 0)
               (9.255966663360596d0 14.373946189880371d0 0)
               (4.255966663360596d0 14.373946189880371d0 0)
               (4.255966663360596d0 10.373946189880371d0 0)))>)
TUT> (reference (desig:an action (type drawing) (shape house)))
(DRAW-HOUSE
 #<A ACTION
    (TYPE DRAWING)
    (SHAPE HOUSE)>)
</code>


==== Writing the plans ====

> Disclaimer: In this tutorial we omit the package (''desig:'') before the ''a'' and ''an'' macro for better readability.

Append the following to your ''high-level-plans.lisp'' file:

<code lisp>
(in-package :tut)

(defun draw-house (&key ((:shape ?shape))
                   &allow-other-keys)
  (declare (type (or keyword) ?shape))
  (with-fields (x y)
      (value *turtle-pose*)
    (exe:perform (an action (type drawing) (shape rectangle) (width 5) (height 4.5)))
    (navigate-without-pen (list (+ x 3) y 0))
    (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 2.5)))
    (navigate-without-pen (list (+ x 0.5) (+ y 2) 0))
    (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 1)))
    (navigate-without-pen (list x (+ y 4.5) 0))
    (exe:perform (an action (type drawing) (shape triangle) (base-width 5) (height 4)))))

(defun draw-simple-shape (&key
                            ((:vertices ?vertices))
                          &allow-other-keys)
  (declare (type (or list null) ?vertices)) 
  (mapcar
   (lambda (?v)
     (exe:perform (an action (type navigating) (target ?v))))
   ?vertices))


(defun navigate-without-pen (?target)
  (exe:perform (a motion (type setting-pen) (off 1)))
  (exe:perform (an action (type navigating) (target ?target)))
  (exe:perform (a motion (type setting-pen) (off 0))))

(defun navigate (&key ((:target ?target))
                 &allow-other-keys)
  (declare (type (or list null) ?target))
  (exe:perform (a motion (type moving) (goal ?target))))
</code>

Here we define three plans and two "helper plans". ''navigate-without-pen'' is a helper plan for moving the turtle between the pieces of the house. It calls the ''navigate'' plan to achieve this. In ''draw-house'' we use the turtle-pose to move the turtle to the position of the next piece by calling ''navigate-without-pen''. To draw the house we call ''perform'' with the action designators we defined above. These get resolved and ''draw-simple-shape'' is called. There we use the ''mapcar'' function to move the turtle by performing a navigating action for every vertex in ''vertices''. For now the ''navigate'' plan is pretty barebones, but we define it as it's own plan because it will be the one to get the failure handling in the next tutorial. Otherwise we could just perform the moving motion.

Now we can test the plan.

<code lisp>
TUT> (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (navigate-without-pen '(1 1 0))
      (exe:perform (an action (type drawing) (shape house)))))
[(TURTLE-PROCESS-MODULES) INFO] 1503577044.541: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (OFF
                                                                            1)) {1008B23133}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503577044.559: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (1 1
                                                                            0))) {1009A325A3}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503577047.556: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (OFF
                                                                            0)) {10088E73D3}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503577047.573: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (6.001096606254578d0
                                                                            1.0863173007965088d0
                                                                            0))) {1008E02D63}>'.
                                                                            
[ ... ]
                                                                            
[(TURTLE-PROCESS-MODULES) INFO] 1503577079.916: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (1.0146702527999878d0
                                                                            5.501473426818848d0
                                                                            0))) {1003C1D383}>'.
(T T T)
TUT> 
</code>

The turtle should move to the lower-left corner (this is not part of the plan itself, but otherwise the house might not fit in the frame). Then it should start by drawing a big rectangle, two smaller ones and finally a triangle. In the end the drawing should resemble a house, albeit an abtract one.

== Next ==


Now that we know how to write plans, let's find out how to handle failures in the plans.

[[tutorials:beginner:failure_handling|Handling Failures]]