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 [2019/07/10 15:03] gkazhoyatutorials: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 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 190: 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 196: 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 211: 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 221: 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 246: Line 269:
 <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 278: 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 293: 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 310: 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 393: 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. 
  
 ---- ----
Line 402: Line 433:
 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 ... 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]]+[[tutorials:beginner:testing|Writing tests]]