====== Implementing failure handling for the TurtleSim ======

**Description:** In this tutorial you will learn how to implement failure handling for your plans.

**Previous Tutorial:** [[tutorials:beginner:high_level_plans|Writing plans for the TurtleSim]]\\
**Next Tutorial:** [[tutorials:beginner:testing|Writing tests]]

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")
...
TUT> (init-ros-turtle "turtle1")
</code>
===== Failure Handling in CRAM =====

Failures can occur during almost any motion or action a robot can take. Because the environment of a real-world robot is non-deteministic it's impossible to plan for all possible failures to be avoided.

CRAM offers a failure handling mechanism similar to the condition handling of Common Lisp. The macro ''with-failure-handling'' is CRAM's replacement for ''handler-case''. Let's take a look at the documentation of ''with-failure-handling'':

<code lisp>
"Macro that replaces handler-case in cram-language. This is
necessary because error handling does not work across multiple
threads. When an error is signaled, it is put into an envelope to
avoid invocation of the debugger multiple times. When handling errors,
this envelope must also be taken into account.

We also need a mechanism to retry since errors can be caused by plan
execution and the environment is highly non-deterministic. Therefore,
it is possible to use the function `retry' that is lexically bound
within with-failure-handling and causes a re-execution of the body.

When an error is unhandled, it is passed up to the next failure
handling form (exactly like handler-bind). Errors are handled by
invoking the retry function or by doing a non-local exit. Note that
with-failure-handling implicitly creates an unnamed block,
i.e. `return' can be used."
</code>

As you can see this is very similar to how handling condition in normal Common Lisp works. With the addition of being able to simply call ''retry'' to re-execute the failed body.

Therefore, to implement failure handling in our plan, we have to define a failure condition, signal it at a sensible point and then handle it. Handling a failure often consists of executing a recovery strategy and retrying, so that the robot can continue to pursue it's original goal. For example if a robot failed to grasp something, it could be a good idea to retrace the movements of the arm, move the arm back to it's original position, reposition the robot, so that it might be able to reach its goal better, and retry.

An example of how ''with-failure-handling'' can be used:

<code lisp>
(with-failure-handling
    ((some-error-condition (e)
       (print e)
       (recover-from e)
       (retry)))
  (do-some-failure-prone-stuff))
</code>

The failure occurs in ''do-some-failure-prone-stuff''. The ''some-error-condition'' is signaled and ''with-failure-handling'' handles it. First we print the error, then we do something to recover from it with ''recover-from'' and at last we call ''retry'', which will re-execute ''do-some-failure-prone-stuff''.


===== The code =====

Let's see how to implement this. First we have to extend our system definition again, because we will add a ''conditions.lisp'' file. Don't forget to add the file to the '':depends-on'' of ''high-level-plans''.

<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 std_srvs-srv)
  :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 "conditions" :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"
                                                    "conditions"))))))

</code>

==== Defining and signaling a failure condition ====

Append this to your ''conditions.lisp'':

<code lisp>
(in-package :tut)

(define-condition out-of-bounds-error (cpl:simple-plan-failure)
  ((description :initarg :description
                :initform "Turtle went out of bounds."
                :reader error-description))
  (:documentation "Turtle went out of bounds.")
  (:report (lambda (condition stream)
             (format stream (error-description condition)))))
</code>

This defines the condition ''out-of-bounds-error'' which inherits from ''cpl:simple-plan-failure''. It's a good idea to base your own conditions on the ones provided by CRAM, so it's recognized as a plan-failure by CRAM.

Let's signal this condition when moving outside of the bounds of the world. For this we will expand our ''navigate'' plan in ''high-level-plans.lisp'':

<code lisp>
(defparameter *min-bound* 0.5)
(defparameter *max-bound* 10.5)

(defun navigate (&key ((:target ?target))
                 &allow-other-keys)
  (declare (type (or list null) ?target))
  (flet ((out-of-bounds (pose)
           (with-fields (x y)
               (value pose)
             (not (and (< *min-bound* x *max-bound*)
                       (< *min-bound* y *max-bound*))))))
    (pursue
      (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
        (error 'out-of-bounds-error))
      (exe:perform (a motion (type moving) (goal ?target))))))
</code>

At first this looks more complicated than it is. We have a ''flet'' to define a local function ''out-of-bounds'' to check if a poses position is out of bounds of the world. Normally you would react to a collision or something like that, but checking if the turtle has collided with the walls is outside of the scope of this tutorial.
We use this function to build a fluent network which is recalculated every time ''*turtle-pose*'' changes. It's value will be T, as long as ''*turtle-pose*'' is outside the bounds. We pass this fluent into a ''whenever'' macro. As the name suggests this macro executes it's body whenever the value of the passed fluent is non-NIL. This happens in a loop, so we don't have to worry about it returning after signaling one error. In the body we use ''error'' to signal our defined condition.
Because ''whenever'' blocks it's thread until it returns, we have to perform our motion in a parallel thread. For this we use ''pursue'' instead of ''par'' because we want the parallel form to return after the plan is executed. ''par'' would never return, because not all of it's children forms return. ''pursue'' returns as soon as ''perform'' returns.

You can test the error signaling with this:

<code lisp>
TUT> (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (navigate-without-pen '(4 8 0))
      (exe:perform (an action (type drawing) (shape house)))))
[(TURTLE-PROCESS-MODULES) INFO] 1503583551.243: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (OFF
                                                                            1)) {10047EB603}>'.
                                                                            
                                                                            [ ... ]
                                                                            
[(TURTLE-PROCESS-MODULES) INFO] 1503583557.555: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (9.08701467514038d0
                                                                            12.503347396850586d0
                                                                            0))) {1009740303}>'.
; Evaluation aborted on #<CRAM-BEGINNER-TUTORIAL::OUT-OF-BOUNDS-ERROR {10089BA023}>.
TUT> 
</code>


==== Recovering from the failure ====

Now that we can run into an error, we better handle it. Again, we have to expand our ''navigate'' plan and also define a function to implement some form of recovery strategy.

The strategy we are going to implement is as follows:

<code lisp>
0. When getting out of bounds:
1. Rotate towards the center of the world.
2. Drive a bit forward.
3. Calculate a new point inside the bounds to move to instead of the original target.
4. Rotate towards this target.
5. Move to the new target.
</code>


So first we add a new function to ''simple-plans.lisp'':

<code lisp>
(defun rotate-to (goal &optional (threshold 0.05))
  (let ((reached-fl (< (fl-funcall #'abs
                                   (fl-funcall #'relative-angle-to goal *turtle-pose*))
                       threshold)))
    (unwind-protect
         (pursue
           (wait-for reached-fl)
           (loop do
             (send-vel-cmd
              0
              (calculate-angular-cmd goal))
             (wait-duration 0.01)))
      (send-vel-cmd 0 0))))
</code>

This is very similar to ''move-to'' but just rotates the turtle.

Now we can implement the failure handling itself.
Let us first look at the code (we will add it to the ''high-level-plans.lisp'' in the next step).

<code lisp>
(defparameter *min-bound* 0.5)
(defparameter *max-bound* 10.5)

(defun navigate (&key ((:target ?target))
                 &allow-other-keys)
  (declare (type (or list null) ?target))
  (flet ((out-of-bounds (pose)
           (with-fields (x y)
               (value pose)
             (not (and (< *min-bound* x *max-bound*)
                       (< *min-bound* y *max-bound*))))))
        (with-failure-handling
        ((out-of-bounds-error (e)
           (ros-warn (draw-simple-simple) "Moving went-wrong: ~a" e)
           (exe:perform (a motion (type setting-pen) (r 204) (g 0) (b 0) (width 2)))
           (let ((?corr-v (list
                           (max 0.6 (min 10.4 (car ?target)))
                           (max 0.6 (min 10.4 (cadr ?target)))
                           0)))
             (recover-from-oob ?corr-v)
             (exe:perform (a motion (type moving) (goal ?corr-v))))
           (exe:perform (a motion (type setting-pen) (off 0)))
           (return)))
      (pursue
        (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
          (error 'out-of-bounds-error))
        (exe:perform (a motion (type moving) (goal ?target)))))))

(defun recover-from-oob (&optional goal)
  (rotate-to (make-3d-vector 5.5 5.5 0))
  (send-vel-cmd 1 0)
  (wait-duration 0.2)
  (when goal
    (rotate-to (apply #'make-3d-vector goal))))
</code>

Again, we expanded the ''navigate'' plan. And we added a helper plan to for recovering from being out of bounds.
''recover-from-oob'' rotates to the center, drives forward and when there's a goal, it rotates towards that. This is just to get inside the bounds again and to place the turtle in a position to be able to move again.

The interesting part of our ''with-failure-handling'' form is this:

<code lisp>
        ((out-of-bounds-error (e)
           (ros-warn (draw-simple-simple) "Moving went-wrong: ~a" e)
           (exe:perform (a motion (type setting-pen) (r 204) (g 0) (b 0) (width 2)))
           (let ((?corr-v (list
                           (max 0.6 (min 10.4 (car ?v)))
                           (max 0.6 (min 10.4 (cadr ?v)))
                           0)))
             (recover-from-oob ?corr-v)
             (exe:perform (a motion (type moving) (goal ?corr-v))))
           (exe:perform (a motion (type setting-pen) (off 0)))
           (return)))
</code>

This is the case that handles ''out-of-bounds-error''s that are signaled inside the ''with-failure-handling'' body. First we just let ROS print out a warning, that something went wrong. Then we perform the recovery. We calculate a new position to drive the turtle to. This position is just the old target clamped to be inside the bounds. The recovery itself consists of calling ''recover-from-oob'' to bring our turtle back to inside the world and then performing a motion to move to the new target. Before and after this recovery we set the pen, so that the turtle draws in red whenever it is correcting a failure. At then end we call ''return'' because otherwise the condition would not count as handled (as stated in the documentation at the top).

Update your ''high-level-plans.lisp'' such that it looks something like this:

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

(defparameter *min-bound* 0.5)
(defparameter *max-bound* 10.5)
 
(defun navigate (&key ((:target ?target))
                 &allow-other-keys)
  (declare (type (or list null) ?target))
  (flet ((out-of-bounds (pose)
           (with-fields (x y)
               (value pose)
             (not (and (< *min-bound* x *max-bound*)
                       (< *min-bound* y *max-bound*))))))
        (with-failure-handling
        ((out-of-bounds-error (e)
           (ros-warn (draw-simple-simple) "Moving went-wrong: ~a" e)
           (exe:perform (a motion (type setting-pen) (r 204) (g 0) (b 0) (width 2)))
           (let ((?corr-v (list
                           (max 0.6 (min 10.4 (car ?target)))
                           (max 0.6 (min 10.4 (cadr ?target)))
                           0)))
             (recover-from-oob ?corr-v)
             (exe:perform (a motion (type moving) (goal ?corr-v))))
           (exe:perform (a motion (type setting-pen) (off 0)))
           (return)))
      (pursue
        (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
          (error 'out-of-bounds-error))
        (exe:perform (a motion (type moving) (goal ?target)))))))

(defun recover-from-oob (&optional goal)
  (rotate-to (make-3d-vector 5.5 5.5 0))
  (send-vel-cmd 1 0)
  (wait-duration 0.2)
  (when goal
    (rotate-to (apply #'make-3d-vector goal))))
</code>

==== Testing the failure handling ====

Now to our final test. You can test the failure handling just like before.

<code lisp>
TUT> (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (navigate-without-pen '(4 8 0))
      (exe:perform (an action (type drawing) (shape house)))))
[(TURTLE-PROCESS-MODULES) INFO] 1503587291.932: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (OFF
                                                                            1)) {1003E82F23}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503587291.968: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (4 8
                                                                            0))) {1005CA82F3}>'.
                                                                            
                                                                            [ ... ]
                                                                            
[(DRAW-SIMPLE-SIMPLE) WARN] 1503587299.619: Moving went-wrong: Turtle went out of bounds.
[(TURTLE-PROCESS-MODULES) INFO] 1503587299.635: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (R
                                                                            204)
                                                                           (G
                                                                            0)
                                                                           (B
                                                                            0)
                                                                           (WIDTH
                                                                            2)) {1004453D23}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503587301.784: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (8.934876441955566d0
                                                                            10.4
                                                                            0))) {10044CC7E3}>'.
                                                                            
                                                                            [ ... ]
                                                                            
[(TURTLE-PROCESS-MODULES) INFO] 1503587335.567: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                            SETTING-PEN)
                                                                           (OFF
                                                                            0)) {1008D59033}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1503587335.594: TurtleSim navigation invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
                                                                           MOVING)
                                                                          (GOAL
                                                                           (4.031672477722168d0
                                                                            10.384994506835938d0
                                                                            0))) {1009088303}>'.
(NIL NIL T)
</code>

As you can see in this transcript of the REPL's output and also should see in the TurtleSim, the turtle didn't stop moving when getting close to the wall. Rather it did exactly what we wanted as our recovery strategy. At the end you should see the lower half of a house with red lines at the top where the turtle couldn't reach the vertices of the shapes.


===== Appendix: Retry counters =====

Sometimes it might be sensible to retry more than once. For example to test different positions or just to retry more often, when a problem is non-deterministic.

This can be achieved by using the ''with-retry-counters'' macro. It enables us to define an arbitrary number of retry counters. We then can use ''do-retry'' with one of these counters to decrease the remaining retries for that counter. ''do-retry'' only executes it's body when there are retries left for the given retry-counter.

Again let's take a look at the documentation of ''with-retry-counters'':

<code lisp>
  "Lexically binds all counters in `counter-definitions' to the intial
  values specified in `counter-definitions'. `counter-definitions' is
  similar to `let' forms with the difference that the counters will
  not be available under the specified names in the lexical
  environment established by this macro. In addition, the macro
  defines the local macro \(DO-RETRY <counter> <body-form>*\) to
  decrement the counter and execute code when the maximal retry count
  hasn't been reached yet and the function `\(RESET-COUNTER
  <counter>\)."
</code>

Like at the top here's an example of how to use it, expanding the example from above:

<code lisp>
(with-retry-counters ((some-error-counter 3))
  (with-failure-handling
      ((some-error-condition (e)
         (print e)
         (do-retry some-error-counter
           (recover-from e)
           (retry))))
    (do-some-failure-prone-stuff)))
</code>

The ''with-failure-handling'' is enclosed in the ''with-retry-counters'', which binds ''some-error-counter'' with a value of ''3''. Inside the handling case the recovery is wrapped by ''do-retry'' which decreases the ''some-error-counter'' each time a ''some-error-condition'' is handled. So ''do-some-failure-prone-stuff'' can fail up to three times, but after a fourth fail the do-retry won't execute it's body and the ''some-error-condition'' won't be handled. 

----

Congratulations! You reached the end of the beginner tutorials!!! The next step would be to look at the intermediate tutorials.

However, you cannot go beyond being a beginner in CRAM, unless you know how to write tests for your CRAM code.
If you are planning to contribute code to the main CRAM base, make sure to go over the next tutorial, which teaches you to write unit (and not only) tests ...

[[tutorials:beginner:testing|Writing tests]]