Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
tutorials:beginner:failure_handling [2017/09/07 13:14] – Beautified out of bounds check cpotutorials:beginner:failure_handling [2022/03/15 13:43] (current) – [Implementing failure handling for the TurtleSim] schimpf
Line 3: Line 3:
 **Description:** In this tutorial you will learn how to implement failure handling for your plans. **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]]+**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 ===== ===== Failure Handling in CRAM =====
  
Line 53: Line 72:
  
 <code lisp> <code lisp>
-(defsystem cram-beginner-tutorial+(defsystem cram-my-beginner-tutorial
   :depends-on (roslisp cram-language turtlesim-msg turtlesim-srv cl-transforms geometry_msgs-msg cram-designators cram-prolog   :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)                        cram-process-modules cram-language-designator-support cram-executive std_srvs-srv)
Line 100: Line 119:
 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. 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 ''move'' plan in ''high-level-plans.lisp'':+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> <code lisp>
Line 106: Line 125:
 (defparameter *max-bound* 10.5) (defparameter *max-bound* 10.5)
  
-(defun move (?v)+(defun navigate (&key ((:target ?target)) 
 +                 &allow-other-keys) 
 +  (declare (type (or list null) ?target))
   (flet ((out-of-bounds (pose)   (flet ((out-of-bounds (pose)
            (with-fields (x y)            (with-fields (x y)
Line 115: Line 136:
       (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))       (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
         (error 'out-of-bounds-error))         (error 'out-of-bounds-error))
-      (exe:perform (a motion (type moving) (goal ?v))))))+      (exe:perform (a motion (type moving) (goal ?target))))))
 </code> </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. 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 everytime ''*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.+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. 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.
  
Line 127: Line 148:
 TUT> (top-level TUT> (top-level
     (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)     (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
-      (move-without-pen '(4 8 0))+      (navigate-without-pen '(4 8 0))
       (exe:perform (an action (type drawing) (shape house)))))       (exe:perform (an action (type drawing) (shape house)))))
 [(TURTLE-PROCESS-MODULES) INFO] 1503583551.243: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE [(TURTLE-PROCESS-MODULES) INFO] 1503583551.243: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
Line 149: Line 170:
 ==== Recovering from the failure ==== ==== Recovering from the failure ====
  
-Now that we can run into an error, we better handle it. Again, we have to expand our ''move'' plan and also define a function to implement some form of recovery strategy.+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: The strategy we are going to implement is as follows:
Line 184: Line 205:
  
 Now we can implement the failure handling itself. 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> <code lisp>
Line 189: Line 211:
 (defparameter *max-bound* 10.5) (defparameter *max-bound* 10.5)
  
-(defun move (?v)+(defun navigate (&key ((:target ?target)) 
 +                 &allow-other-keys) 
 +  (declare (type (or list null) ?target))
   (flet ((out-of-bounds (pose)   (flet ((out-of-bounds (pose)
            (with-fields (x y)            (with-fields (x y)
Line 195: Line 219:
              (not (and (< *min-bound* x *max-bound*)              (not (and (< *min-bound* x *max-bound*)
                        (< *min-bound* y *max-bound*))))))                        (< *min-bound* y *max-bound*))))))
-    (with-failure-handling+        (with-failure-handling
         ((out-of-bounds-error (e)         ((out-of-bounds-error (e)
            (ros-warn (draw-simple-simple) "Moving went-wrong: ~a" 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)))            (exe:perform (a motion (type setting-pen) (r 204) (g 0) (b 0) (width 2)))
            (let ((?corr-v (list            (let ((?corr-v (list
-                           (max 0.6 (min 10.4 (car ?v))) +                           (max 0.6 (min 10.4 (car ?target))) 
-                           (max 0.6 (min 10.4 (cadr ?v)))+                           (max 0.6 (min 10.4 (cadr ?target)))
                            0)))                            0)))
              (recover-from-oob ?corr-v)              (recover-from-oob ?corr-v)
Line 210: Line 234:
         (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))         (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
           (error 'out-of-bounds-error))           (error 'out-of-bounds-error))
-        (exe:perform (a motion (type moving) (goal ?v)))))))+        (exe:perform (a motion (type moving) (goal ?target)))))))
  
 (defun recover-from-oob (&optional goal) (defun recover-from-oob (&optional goal)
Line 220: Line 244:
 </code> </code>
  
-Again, we expanded the ''move'' plan. And we added a helper plan to for recovering from being out of bounds.+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. ''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.
  
Line 241: Line 265:
 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). 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).
  
-Your ''high-level-plans.lisp'' should now look something like this:+Update your ''high-level-plans.lisp'' such that it looks something like this:
  
 <code lisp> <code lisp>
 (in-package :tut) (in-package :tut)
- +  
-(defun draw-house ()+(defun draw-house (&key ((:shape ?shape)) 
 +                   &allow-other-keys) 
 +  (declare (type (or keyword) ?shape))
   (with-fields (x y)   (with-fields (x y)
       (value *turtle-pose*)       (value *turtle-pose*)
     (exe:perform (an action (type drawing) (shape rectangle) (width 5) (height 4.5)))     (exe:perform (an action (type drawing) (shape rectangle) (width 5) (height 4.5)))
-    (move-without-pen (list (+ x 3) y 0))+    (navigate-without-pen (list (+ x 3) y 0))
     (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 2.5)))     (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 2.5)))
-    (move-without-pen (list (+ x 0.5) (+ y 2) 0))+    (navigate-without-pen (list (+ x 0.5) (+ y 2) 0))
     (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 1)))     (exe:perform (an action (type drawing) (shape rectangle) (width 1) (height 1)))
-    (move-without-pen (list x (+ y 4.5) 0))+    (navigate-without-pen (list x (+ y 4.5) 0))
     (exe:perform (an action (type drawing) (shape triangle) (base-width 5) (height 4)))))     (exe:perform (an action (type drawing) (shape triangle) (base-width 5) (height 4)))))
  
-(defun draw-simple-shape (vertices)+(defun draw-simple-shape (&key 
 +                            ((:vertices ?vertices)) 
 +                          &allow-other-keys) 
 +  (declare (type (or list null) ?vertices)
   (mapcar   (mapcar
    (lambda (?v)    (lambda (?v)
-     (exe:perform (an action (type moving) (target ?v)))) +     (exe:perform (an action (type navigating) (target ?v)))) 
-   vertices)) +   ?vertices)) 
- +  
-(defun move-without-pen (?target)+(defun navigate-without-pen (?target)
   (exe:perform (a motion (type setting-pen) (off 1)))   (exe:perform (a motion (type setting-pen) (off 1)))
-  (exe:perform (an action (type moving) (target ?target)))+  (exe:perform (an action (type navigating) (target ?target)))
   (exe:perform (a motion (type setting-pen) (off 0))))   (exe:perform (a motion (type setting-pen) (off 0))))
 + 
  
 (defparameter *min-bound* 0.5) (defparameter *min-bound* 0.5)
 (defparameter *max-bound* 10.5) (defparameter *max-bound* 10.5)
- +  
-(defun move (?v)+(defun navigate (&key ((:target ?target)) 
 +                 &allow-other-keys) 
 +  (declare (type (or list null) ?target))
   (flet ((out-of-bounds (pose)   (flet ((out-of-bounds (pose)
            (with-fields (x y)            (with-fields (x y)
Line 277: Line 309:
              (not (and (< *min-bound* x *max-bound*)              (not (and (< *min-bound* x *max-bound*)
                        (< *min-bound* y *max-bound*))))))                        (< *min-bound* y *max-bound*))))))
-    (with-failure-handling+        (with-failure-handling
         ((out-of-bounds-error (e)         ((out-of-bounds-error (e)
            (ros-warn (draw-simple-simple) "Moving went-wrong: ~a" 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)))            (exe:perform (a motion (type setting-pen) (r 204) (g 0) (b 0) (width 2)))
            (let ((?corr-v (list            (let ((?corr-v (list
-                           (max 0.6 (min 10.4 (car ?v))) +                           (max 0.6 (min 10.4 (car ?target))) 
-                           (max 0.6 (min 10.4 (cadr ?v)))+                           (max 0.6 (min 10.4 (cadr ?target)))
                            0)))                            0)))
              (recover-from-oob ?corr-v)              (recover-from-oob ?corr-v)
Line 292: Line 324:
         (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))         (whenever ((fl-funcall #'out-of-bounds *turtle-pose*))
           (error 'out-of-bounds-error))           (error 'out-of-bounds-error))
-        (exe:perform (a motion (type moving) (goal ?v)))))))+        (exe:perform (a motion (type moving) (goal ?target)))))))
  
 (defun recover-from-oob (&optional goal) (defun recover-from-oob (&optional goal)
Line 309: Line 341:
 TUT> (top-level TUT> (top-level
     (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)     (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
-      (move-without-pen '(4 8 0))+      (navigate-without-pen '(4 8 0))
       (exe:perform (an action (type drawing) (shape house)))))       (exe:perform (an action (type drawing) (shape house)))))
 [(TURTLE-PROCESS-MODULES) INFO] 1503587291.932: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE [(TURTLE-PROCESS-MODULES) INFO] 1503587291.932: TurtleSim pen control invoked with motion designator `#<MOTION-DESIGNATOR ((TYPE
Line 392: Line 424:
 </code> </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 if ''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. +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]]