Using location designators with the TurtleSim

Description: in this tutorial you will learn about location designators, how to create and resolve them. Also, you will write another process module to make use of the location designator.

Previous Tutorial: Creating process modules
Next Tutorial: Automatically choosing a process module for an action

As mentioned previously, location designators are a way to describe a location in symbolic terms, and have actual coordinates for it generated later, when needed. The crucial difference between location and action designators comes in how they are resolved. We've seen how action designators are resolved via an inference engine (Prolog) operating on a set of rules. Instead of that, location designators make use of a pair of [types of] functions:

• location-generator: creates a lazy list of candidate poses
• location-validator: checks that a candidate pose is actually valid (feasible for the robot, not in collision with objects in the environment, or whatever other criteria were relevant when writing the program)

This approach may be familiar to you if you're coming from a sampling-based planning background: use the generator to get a sample (or candidate), then validate it, and repeat if the previously checked candidate was not valid.

Both generator and validator functions need to be written by the programmer for their specific application, and registered as generators (respectively validators) for given designator constraints. It is also possible to define and register several generators and validators for the same designator. Having more generators gives more candidates to check, but the really interesting aspect is how different validators collaborate to check a candidate. To that end, several return values are possible for validators:

• :accept means the candidate is acceptable by this validator
• :reject immediately invalidates the candidate, no further processing needed
• :unknown means this validator cannot decide and leaves the decision to the others
• :maybe-reject means the candidate will be rejected unless some other validator explicitly accepts it

Therefore, a candidate will be accepted if

• no validator rejected it, AND
  • at least one validator returned :accept, OR
  • all validators returned :unknown

We will now have a look at how to create and resolve location designators.

Add a dependency on cl_tf and cl-tf to your package.xml and *.asd files respectively. Create a new file called location-designators.lisp in your tutorial's src directory and add it to the *.asd file with process-moduless depending on it. The resulting cram-beginner-tutorial.asd should now look like this:

(defsystem cram-beginner-tutorial
  :depends-on (cram-language roslisp turtlesim-msg geometry_msgs-msg cl-transforms
                             cram-designators cram-prolog
                             actionlib actionlib_msgs-msg turtle_actionlib-msg
                             cram-process-modules cram-language-designator-support
                             cl-tf)
  :components
  ((:module "src"
            :components
            ((:file "package")
             (:file "control-turtlesim" :depends-on ("package"))
             (:file "simple-plans" :depends-on ("package" "control-turtlesim"))
             (:file "action-designators" :depends-on ("package"))
             (:file "turtle-action-client" :depends-on ("package"))
             (:file "location-designators" :depends-on ("package"))
             (:file "process-modules" :depends-on ("package"
                                                   "control-turtlesim"
                                                   "simple-plans"
                                                   "action-designators"
                                                   "turtle-action-client"))))))

Now we'll want some code to generate a location based on a description found in a designator. Here is an example of such a function, which you can append to your location-designators.lisp file:

(in-package :tut)
 
(defun navigation-goal-generator (location-designator)
  (let ((retq ()))
      (dotimes (k 10)
          (let ((x (- 1)) (y (- 1)))
              (dolist (elem (description location-designator))
                  (case
                      (car elem)
                          (vpos
                              (case (cadr elem)
                                  (center (setq y (+ 6 (random 4.0) -2)) )
                                  (top (setq y (+ 11 (random 4.0) -2)) )
                                  (bottom (setq y (+ 1 (random 4.0) -2)))))
                          (hpos
                              (case (cadr elem)
                                  (center (setq x (+ 6 (random 4.0) -2)))
                                  (right (setq x (+ 11 (random 4.0) -2)))
                                  (left (setq x (+ 1 (random 4.0) -2)))))))
    (setq retq (append retq
         (list (cl-tf:make-pose-stamped
              "turtlesim"
              (roslisp:ros-time)
              (cl-transforms:make-3d-vector x y 0)
              (cl-transforms:make-quaternion 0 0 0 1)))))))
    (return-from navigation-goal-generator retq)))

What this function does is convert a description like, for example, ( (vpos center) (hpos center) ), into a pose for the turtle expressed as numeric coordinates. Note that the function expects a location designator as a parameter and returns a list of candidate positions. This is not accidental; if we want to register this function as a location-generator (which we will do, shortly), then we need to obey this interface. Even if we had wanted to return a single candidate, it should have been returned as a list, containing only one candidate. Also, and this is very important, elements of the result list must be stamped poses made by the cl-tf package. Already-existing software using location designators relies on results being stamped poses, and it's a good idea to use them yourself.

We'll also need a validation function, and here is an example of such, which you can append to your location-designators.lisp:

(defun turtle-pose-validator (location-designator pose)
  (declare (ignore location-designator))
  (when (typep pose 'cl-transforms:pose)
    (if
     (or
      (< (cl-transforms:x
          (cl-transforms:origin pose))
         0)
      (< (cl-transforms:y
          (cl-transforms:origin pose))
         0)
      (> (cl-transforms:x
          (cl-transforms:origin pose))
         11.8)
      (> (cl-transforms:y
          (cl-transforms:origin pose))
         11.8))
     (return-from turtle-pose-validator :reject)
     (return-from turtle-pose-validator :accept))))

This validator simply checks that the required position is inside the screen of the turtlesim. Again, the interface of the function is important. We must declare it as taking two parameters, a designator and a pose, even if, as is the case here, we don't actually use one of the parameters. Also, the return value must be one of the values listed above (:accept :reject :unknown :maybe-reject).

We also need to register these two functions, so append the following to location-designators.lisp:

(register-location-generator
 15 navigation-goal-generator)
 
(register-location-validation-function
 15 turtle-pose-validator)

The register-* functions have self explanatory names, and one of the parameters is a function name that we want to register. The number parameter is a 'priority', and it will order the calls to generators and validators. Lower numbers mean called earlier.

Let's reload the tutorial system in roslisp_repl and play with the new functions a bit. Let's try to create a location designator via this call in the REPL command line:

TUT> (defparameter turtle-target (make-designator 'location `((vpos center) (hpos center))))
TURTLE-TARGET

So far so good, let's reference it by calling in REPL's command line:

TUT> (reference turtle-target)
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.414076190451d9
   #<3D-VECTOR (5.194351673126221d0 6.093849182128906d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
TUT> (reference turtle-target)
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.414076190451d9
   #<3D-VECTOR (5.194351673126221d0 6.093849182128906d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>

This also looks ok, we get a pose, and it's plausibly near the center. However, both calls to reference returned the same solution. Surely the random number generator in navigation-goal-generator didn't create the exact same numbers twice. Indeed, it didn't, but to get other candidates that survived validation we will need another function, next-solution:

TUT> (next-solution turtle-target)
#<LOCATION-DESIGNATOR ((VPOS CENTER) (HPOS CENTER)) {100A1F3393}>

Perhaps a little confusingly, next-solution doesn't actually return the next solution, but rather a new designator, with identical properties to the first, that is associated to the next solution. If no other solution exists, then next-solution returns nil. If we actually want to see this solution, assuming it exists, we need to call:

TUT> (reference (next-solution turtle-target))
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.414076190451d9
   #<3D-VECTOR (6.060066223144531d0 5.841248512268066d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>

If we called (reference (next-solution turtle-target)) again, we will get this same solution again. If turtle-target stayed the same, then the next-solution designator also stayed the same, and so did the solution associated with it. One way to iterate over the solutions would be:

TUT> (setq turtle-target-copy (copy-designator turtle-target))
#<LOCATION-DESIGNATOR ((HPOS CENTER) (VPOS CENTER)) {100A079C03}>
TUT> (loop do (setq sol (reference turtle-target-copy)) (print sol) (setq turtle-target-copy (next-solution turtle-target-copy)) while (not (equal turtle-target-copy nil)))
; 
; caught WARNING:
;   undefined variable: SOL
; 
; compilation unit finished
;   Undefined variable:
;     SOL
;   caught 1 WARNING condition
 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.414076190451d9
   #<3D-VECTOR (5.194351673126221d0 6.093849182128906d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.414076190451d9
   #<3D-VECTOR (6.060066223144531d0 5.841248512268066d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (7.604331970214844d0 7.907034873962402d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (4.6705708503723145d0 5.763947486877441d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (5.831815719604492d0 7.300619125366211d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (7.306698799133301d0 4.04398775100708d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (5.219023704528809d0 4.466149806976318d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (6.606534957885742d0 4.025519847869873d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (4.306739330291748d0 7.699792861938477d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
#<CL-TF:POSE-STAMPED 
   FRAME-ID: "turtlesim", STAMP: 1.4140771269d9
   #<3D-VECTOR (7.969078063964844d0 4.372735500335693d0 0.0d0)>
   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> 
NIL

(Note that at the end of the iteration, turtle-target-copy becomes nil.)

Let's try to create a process module to make use of a location designator as well. Append the following to process-modules.lisp:

(cram-process-modules:def-process-module turtle-navigation (location-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "Turtle navigation invoked with location designator `~a'."
                    location-designator)
  (let ((target-pose (reference location-designator)))
       (print (cl-transforms:origin target-pose))
       (move-to (cl-transforms:origin target-pose))))
 
(defun goto-location (location-desig)
  (let ((turtle-name "turtle1"))
    (start-ros-node turtle-name)
    (init-ros-turtle turtle-name)
    (top-level
      (cpm:with-process-modules-running (turtle-navigation)
        (cpm:process-module-alias :navigation 'turtle-navigation)
            (cpm:pm-execute :navigation location-desig)))))

And let's give it a go. Reload the tutorial into roslisp_repl then in the command line of REPL:

TUT> (defparameter turtle-target (make-designator 'location `((vpos center) (hpos center))))
TURTLE-TARGET
TUT> (goto-location turtle-target)
WARNING:
   Before starting node, node-status equalled RUNNING instead of :shutdown.  Shutting the previous node invocation down now.
[(ROSLISP EVENT-LOOP) INFO] 1414081756.703: Terminating ROS Node event loop
[(ROSLISP TOP) INFO] 1414081757.022: Shutdown complete
[(ROSLISP TOP) INFO] 1414081757.028: Node name is /turtle1
[(ROSLISP TOP) INFO] 1414081757.028: Namespace is /
[(ROSLISP TOP) INFO] 1414081757.028: Params are NIL
[(ROSLISP TOP) INFO] 1414081757.028: Remappings are:
[(ROSLISP TOP) INFO] 1414081757.028: master URI is 127.0.0.1:11311
[(ROSLISP TOP) INFO] 1414081758.033: Node startup complete
[(TURTLE-PROCESS-MODULES) INFO] 1414081758.183: Turtle navigation invoked with location designator `#<LOCATION-DESIGNATOR ((VPOS
                                                                            CENTER)
                                                                           (HPOS
                                                                            CENTER)) {10078A2C53}>'.
 
#<CL-TRANSFORMS:3D-VECTOR (6.383428573608398d0 5.245925426483154d0 0.0d0)> 
T

The turtle should also have moved to somewhere in the vecinity of the center of its window.

So far we called process modules directly. Sometimes it's better to let the system decide on its own …

Automatically choosing a process module for an action