Table of Contents
Implementing failure handling for the TurtleSim
Description: In this tutorial you will learn how to implement failure handling for your plans.
Previous Tutorial: Writing plans for the TurtleSim
Next Tutorial: 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.
$ roscore
$ rosrun turtlesim turtlesim_node
And in the REPL the following commands should be executed:
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")
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:
"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."
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:
(with-failure-handling ((some-error-condition (e) (print e) (recover-from e) (retry))) (do-some-failure-prone-stuff))
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.
(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"))))))
Defining and signaling a failure condition
Append this to your conditions.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)))))
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:
(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))))))
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:
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>
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:
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.
So first we add a new function to simple-plans.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))))
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).
(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))))
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:
((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)))
This is the case that handles out-of-bounds-errors 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:
(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))))
Testing the failure handling
Now to our final test. You can test the failure handling just like before.
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)
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:
"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>\)."
Like at the top here's an example of how to use it, expanding the example from above:
(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)))
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 …
