Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
tutorials:beginner:location_designators [2016/01/25 12:08] – [Using location designators with the turtlesim] gkazhoya | tutorials:beginner:location_designators [2016/03/04 14:25] (current) – old revision restored (2016/01/26 10:38) gkazhoya | ||
---|---|---|---|
Line 8: | Line 8: | ||
===== Location designators: | ===== Location designators: | ||
- | 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: | + | 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: | * location-generator: | ||
Line 15: | Line 15: | ||
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. | 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, | + | Both generator and validator functions need to be written by the programmer for their specific application, |
- | * :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 | + | * '' |
- | * : | + | * '' |
Therefore, a candidate will be accepted if | Therefore, a candidate will be accepted if | ||
* no validator rejected it, AND | * no validator rejected it, AND | ||
- | * at least one validator returned :accept, OR | + | * at least one validator returned |
- | * all validators returned :unknown | + | * all validators returned |
We will now have a look at how to create and resolve location designators. | We will now have a look at how to create and resolve location designators. | ||
- | ===== Creating a location | + | ===== Location |
- | Add a dependency on cl-tf to your *.asd file. Create | + | Let's create |
<code lisp> | <code lisp> | ||
(defsystem cram-beginner-tutorial | (defsystem cram-beginner-tutorial | ||
- | :depends-on (roslisp | + | :depends-on (cram-language |
- | | + | |
+ | actionlib | ||
+ | cram-process-modules | ||
:components | :components | ||
((:module " | ((:module " | ||
:components | :components | ||
((:file " | ((:file " | ||
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
+ | " | ||
+ | " | ||
+ | " | ||
+ | " | ||
</ | </ | ||
+ | Now we'll want some code to generate a location based on a symbolic description found in a designator. There are two interfaces for resolving location designators: | ||
- | Also, we'll want to define new designator properties in package.lisp, so it should now look like this: | + | We would like to specify locations for the turtle to go using spatial relations, e.g.: |
<code lisp> | <code lisp> | ||
- | (in-package | + | TUT> |
+ | | ||
+ | TUT> goal-desig | ||
+ | #< | ||
+ | | ||
+ | </ | ||
- | (desig-props:def-desig-package cram-beginner-tutorial-workthrough | + | This way TurtleSim field will be divided into 9 areas, as shown below: |
- | (:nicknames :tut) | + | |
- | (:use #:cpl | + | |
- | #:roslisp | + | |
- | #: | + | |
- | #: | + | |
- | ) | + | |
- | (:desig-properties | + | {{ :tutorials:beginner:turtle-locations.png? |
- | ;;;action properties | + | |
- | #:shape | + | |
- | #:radius | + | |
- | #: | + | |
- | #:square | + | |
- | #: | + | |
- | #:hexagon | + | |
- | ;;;location Properties | + | |
- | #:vpos | + | |
- | #:hpos | + | |
- | #:left | + | |
- | #:right | + | |
- | #:top | + | |
- | #:bottom | + | |
- | #:center | + | |
- | )) | + | |
- | </ | + | |
- | Now we'll want some code to generate a location based on a description found in a designator. | + | Here is an example of a designator resolving |
<code lisp> | <code lisp> | ||
(in-package :tut) | (in-package :tut) | ||
- | (defun navigation-goal-generator (location-designator) | + | (defun navigation-goal-generator (designator) |
- | (let ((retq | + | (declare |
- | (dotimes | + | (with-desig-props |
- | (let ((x (- 1)) (y (- 1))) | + | (let ((x-offset |
- | (dolist (elem (description location-designator)) | + | (:left 0) |
- | (case | + | (:center (/ 11.0 3.0)) |
- | (car elem) | + | (: |
- | (vpos | + | (y-offset |
- | (case (cadr elem) | + | (:bottom 0) |
- | (center (setq y (+ 6 (random 4.0) -2)) ) | + | (:center (/ 11.0 3.0)) |
- | (top (setq y (+ 11 (random 4.0) -2)) ) | + | (:top (* (/ 11.0 3.0) 2))))) |
- | (bottom (setq y (+ 1 (random 4.0) -2))))) | + | (loop repeat 5 |
- | (hpos | + | |
- | (case (cadr elem) | + | |
- | (center (setq x (+ 6 (random 4.0) -2))) | + | |
- | (right (setq x (+ 11 (random 4.0) -2))) | + | 0))))) |
- | (left (setq x (+ 1 (random 4.0) -2))))))) | + | |
- | (setq retq (append retq | + | |
- | (list (cl-tf:make-pose-stamped | + | |
- | " | + | |
- | | + | |
- | | + | |
- | (cl-transforms: | + | |
- | (return-from navigation-goal-generator retq))) | + | |
</ | </ | ||
- | What this function does is convert a description like, for example, < | + | Let's compile |
- | + | ||
- | We'll also need a validation function, and here is an example | + | |
<code lisp> | <code lisp> | ||
- | (defun turtle-pose-validator (location-designator pose) | + | TUT> |
- | (declare (ignore location-designator)) | + | (#<3D-VECTOR |
- | | + | #<3D-VECTOR |
- | (if | + | #<3D-VECTOR |
- | (or | + | #<3D-VECTOR |
- | (< (cl-transforms: | + | #< |
- | | + | |
- | 0) | + | |
- | (< (cl-transforms: | + | |
- | | + | |
- | 0) | + | |
- | (> (cl-transforms: | + | |
- | | + | |
- | 11.8) | + | |
- | (> (cl-transforms: | + | |
- | (cl-transforms: | + | |
- | 11.8)) | + | |
- | | + | |
- | | + | |
</ | </ | ||
- | This validator simply checks that the required position is inside the screen | + | The function expects a location designator as a parameter and returns a list of solutions (in this case points in the bottom left corner |
- | We also need to register | + | To register |
<code lisp> | <code lisp> | ||
(register-location-generator | (register-location-generator | ||
- | 15 navigation-goal-generator) | + | 5 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 | + | One of the parameters |
- | ===== Resolving a location designator ===== | + | Let's see if it works, don't forget to recompile the file / reload the system: |
+ | <code lisp> | ||
+ | TUT> (reference goal-desig) | ||
+ | #< | ||
+ | TUT> (reference goal-desig) | ||
+ | #< | ||
+ | </ | ||
- | Let's reload | + | So far so good, we get a pose, and it is in fact in the bottom left corner. However, both calls to '' |
<code lisp> | <code lisp> | ||
- | TUT> (defparameter turtle-target (make-designator ' | + | TUT> (next-solution goal-desig) |
- | TURTLE-TARGET | + | #< |
+ | (: | ||
</ | </ | ||
- | So far so good, let's reference it by calling in REPL's command line: | + | Perhaps a little confusingly, '' |
<code lisp> | <code lisp> | ||
- | TUT> (reference | + | TUT> (reference |
- | #<CL-TF: | + | #< |
- | | + | |
- | #< | + | |
- | #< | + | |
- | TUT> (reference turtle-target) | + | |
- | #< | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
</ | </ | ||
- | This also looks ok, we get a pose, and it's plausibly near the center. However, both calls to reference | + | If we called |
<code lisp> | <code lisp> | ||
- | TUT> (next-solution | + | TUT> |
- | #<LOCATION-DESIGNATOR | + | while desig |
+ | do (print (reference desig))) | ||
+ | #<3D-VECTOR (1.142098307609558d0 2.184809684753418d0 0.0d0)> | ||
+ | #<3D-VECTOR | ||
+ | #< | ||
+ | #< | ||
+ | #< | ||
+ | NIL | ||
</ | </ | ||
- | Perhaps | + | |
+ | ===== Location designator validation ===== | ||
+ | |||
+ | Now let's add a validation function. The points we get from '' | ||
<code lisp> | <code lisp> | ||
- | TUT> | + | (defun navigation-goal-validator (designator |
- | #<CL-TF:POSE-STAMPED | + | (declare (type location-designator designator)) |
- | FRAME-ID: " | + | (when (and (desig-prop-value designator |
- | #<3D-VECTOR | + | (desig-prop-value designator |
- | #<QUATERNION | + | (when (typep solution 'cl-transforms: |
+ | | ||
+ | (and | ||
+ | | ||
+ | | ||
+ | (<= (cl-transforms: | ||
+ | | ||
+ | : | ||
+ | |||
+ | (register-location-validation-function | ||
+ | 5 navigation-goal-validator) | ||
</ | </ | ||
- | If we called (reference (next-solution | + | If '' |
- | <code lisp> | + | The last function call registers our validator, validators are prioritized just as the generators. |
- | TUT> (setq turtle-target-copy (copy-designator turtle-target)) | + | |
- | #< | + | |
- | 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: | + | |
- | ; | + | |
- | ; | + | |
- | ; compilation unit finished | + | |
- | ; | + | |
- | ; SOL | + | |
- | ; | + | |
- | #< | + | Let's test if this worked, don't forget to recompile the file / reload the system: |
- | | + | |
- | #< | + | <code lisp> |
- | #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> | + | TUT> (defparameter another-goal |
- | #< | + | (make-designator |
- | | + | ANOTHER-GOAL |
- | #< | + | TUT> (loop for desig = another-goal then (next-solution desig) |
- | #< | + | while desig |
- | #< | + | do (print (reference desig))) |
- | FRAME-ID: " | + | #< |
- | #< | + | #< |
- | #< | + | #< |
- | #<CL-TF:POSE-STAMPED | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
- | #<CL-TF:POSE-STAMPED | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
- | #<CL-TF: | + | |
- | FRAME-ID: " | + | |
- | #< | + | |
- | #< | + | |
- | #<CL-TF:POSE-STAMPED | + | |
- | FRAME-ID: " | + | |
- | #< | + | |
- | #< | + | |
- | #< | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
- | #< | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
- | #< | + | |
- | | + | |
- | #< | + | |
- | #< | + | |
NIL | NIL | ||
</ | </ | ||
- | (Note that at the end of the iteration, turtle-target-copy becomes nil.) | + | Depending on the random number generator we will get some or none of the solutions rejected and, therefore, ''< |
===== Using a location designator ===== | ===== 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: | + | Let's try to create a process module to make use of a location designator as well. Append the following to '' |
<code lisp> | <code lisp> | ||
- | (cram-process-modules: | + | (def-fact-group goal-actions (action-desig) |
+ | (<- (action-desig ?desig (go-to ?point)) | ||
+ | (desig-prop ?desig (:type :goal)) | ||
+ | (desig-prop ?desig (:goal ? | ||
+ | </ | ||
+ | |||
+ | This will resolve any action designator with properties '' | ||
+ | |||
+ | Now for the process | ||
+ | |||
+ | <code lisp> | ||
+ | (in-package :tut) | ||
+ | |||
+ | (def-process-module | ||
(roslisp: | (roslisp: | ||
- | " | + | " |
- | | + | |
- | (let ((target-pose (reference | + | (destructuring-bind |
- | | + | (ecase command |
- | | + | (draw-shape |
+ | | ||
+ | :edges (turtle-shape-edges action-goal) | ||
+ | :radius (turtle-shape-radius action-goal)))))) | ||
+ | |||
+ | (def-process-module simple-navigation | ||
+ | (roslisp:ros-info (turtle-process-modules) | ||
+ | " | ||
+ | action-designator) | ||
+ | (destructuring-bind (command action-goal) (reference action-designator) | ||
+ | (ecase command | ||
+ | (go-to | ||
+ | (when (typep action-goal ' | ||
+ | (let ((target-point (reference action-goal))) | ||
+ | | ||
+ | " | ||
+ | | ||
+ | |||
+ | (defmacro with-turtle-process-modules (&body body) | ||
+ | `(with-process-modules-running | ||
+ | | ||
+ | simple-navigation) | ||
+ | , | ||
- | (defun | + | (defun |
(let ((turtle-name " | (let ((turtle-name " | ||
(start-ros-node turtle-name) | (start-ros-node turtle-name) | ||
(init-ros-turtle turtle-name) | (init-ros-turtle turtle-name) | ||
(top-level | (top-level | ||
- | (cpm:with-process-modules-running | + | (with-turtle-process-modules |
- | (cpm: | + | |
- | (cpm:pm-execute : | + | (with-designators |
+ | ((trajectory | ||
+ | (pm-execute :navigation trajectory)))))) | ||
+ | |||
+ | (defun goto-location (horizontal-position vertical-position) | ||
+ | (let ((turtle-name " | ||
+ | (start-ros-node turtle-name) | ||
+ | (init-ros-turtle turtle-name) | ||
+ | (top-level | ||
+ | (with-turtle-process-modules | ||
+ | (process-module-alias :navigation 'simple-navigation) | ||
+ | (with-designators | ||
+ | ((area :location `((: | ||
+ | | ||
+ | (goal :action `((:type :goal) (:goal ,area)))) | ||
+ | (pm-execute : | ||
</ | </ | ||
- | And let's give it a go. Reload the tutorial | + | And let's give it a go. Reload the tutorial |
<code lisp> | <code lisp> | ||
- | TUT> (defparameter turtle-target (make-designator ' | + | TUT> (goto-location :center |
- | TURTLE-TARGET | + | [(ROSLISP TOP) INFO] 1453758117.881: Node name is /turtle1 |
- | TUT> (goto-location | + | [(ROSLISP TOP) INFO] 1453758117.881: Namespace is / |
- | WARNING: | + | [(ROSLISP TOP) INFO] 1453758117.885: Params are NIL |
- | | + | [(ROSLISP TOP) INFO] 1453758117.885: Remappings are: |
- | [(ROSLISP EVENT-LOOP) INFO] 1414081756.703: | + | [(ROSLISP TOP) INFO] 1453758117.885: master URI is 127.0.0.1: |
- | [(ROSLISP TOP) INFO] 1414081757.022: | + | [(ROSLISP TOP) INFO] 1453758119.036: Node startup complete |
- | [(ROSLISP TOP) INFO] 1414081757.028: Node name is /turtle1 | + | [(TURTLE-PROCESS-MODULES) INFO] 1453758119.377: Turtle |
- | [(ROSLISP TOP) INFO] 1414081757.028: Namespace is / | + | `#<ACTION-DESIGNATOR ((TYPE GOAL) |
- | [(ROSLISP TOP) INFO] 1414081757.028: Params are NIL | + | (GOAL #< |
- | [(ROSLISP TOP) INFO] 1414081757.028: Remappings are: | + | |
- | [(ROSLISP TOP) INFO] 1414081757.028: master URI is 127.0.0.1: | + | [(TURTLE-PROCESS-MODULES) INFO] 1453758119.386: |
- | [(ROSLISP TOP) INFO] 1414081758.033: Node startup complete | + | |
- | [(TURTLE-PROCESS-MODULES) INFO] 1414081758.183: Turtle navigation invoked with location | + | |
- | CENTER) | + | |
- | | + | |
- | | + | |
- | + | ||
- | #<CL-TRANSFORMS: | + | |
T | T | ||
</ | </ | ||
- | The turtle should also have moved to somewhere in the vecinity | + | The turtle should also have moved to somewhere in the vicinity |
== Next == | == Next == | ||
Line 310: | Line 296: | ||
[[tutorials: | [[tutorials: | ||
- |