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: Using location designators with the TurtleSim
Next Tutorial: 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.
$ 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")
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 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.
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:
(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"))))))
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.
TUT> (desig:an action (type drawing) (shape rectangle) (width 5) (height 4))
#<A ACTION
(TYPE DRAWING)
(SHAPE RECTANGLE)
(WIDTH 5)
(HEIGHT 4)>
Append the following to your action-designators.lisp file:
(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)))
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):
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)>)
Disclaimer: In this tutorial we omit the package (desig:) before theaandanmacro for better readability.
Append the following to your high-level-plans.lisp file:
(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))))
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.
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>
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.
Now that we know how to write plans, let's find out how to handle failures in the plans.