This is an old revision of the document!
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
Location designators: an overview
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.
Creating a location designator
Let's create a new file called location-designators.lisp
in our tutorial's src
directory and add it to the *.asd
file with process-modules
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) :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 symbolic 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 (designator) (declare (type location-designator designator)) (desig:with-desig-props (vertical-position horizontal-position) designator (let ((x-offset (ecase horizontal-position (:left 0) (:center (/ 11.0 3.0)) (:right (* (/ 11.0 3.0) 2)))) (y-offset (ecase vertical-position (:bottom 0) (:center (/ 11.0 3.0)) (:top (* (/ 11.0 3.0) 2))))) (loop repeat 10 collect (cl-transforms:make-pose (cl-transforms:make-3d-vector (+ x-offset (random (/ 11.0 3.0))) (+ y-offset (random (/ 11.0 3.0))) 0) (cl-transforms:make-identity-rotation)))))) (defun make-2d-pose-with-randomness (x y random-range) (cl-transforms:make-pose (cl-transforms:make-3d-vector (+ x (random random-range)) (+ y (random random-range)) 0) (cl-transforms:make-identity-rotation))) (def-fact-group location-designator-generator (desig-solution) (<- (position-offset :bottom 0)) (<- (position-offset :top ?offset) (lisp-fun / 11.0 3.0 0.5 ?offset)) (<- (position-offset :center ?offset) (lisp-fun / 11.0 3.0 ?offset)) (<- (position-offset :left 0)) (<- (position-offset :right ?offset) (position-offset :top ?offset)) (<- (desig-solution ?desig ?solution) (loc-desig? ?desig) (desig-prop ?desig (:vertical-position ?vertical-position)) (desig-prop ?desig (:horizontal-position ?horizontal-position)) (position-offset ?vertical-position ?vertical-offset) (position-offset ?horizontal-position ?horizontal-offset) (lisp-fun / 11.0 3.0 ?random-range) (lisp-fun make-2d-pose-with-randomness ?horizontal-offset ?vertical-offset ?random-range ?solution)))
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.
Resolving a location designator
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.)
Using a location designator
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.
Next
So far we called process modules directly. Sometimes it's better to let the system decide on its own …