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 17:54] – gkazhoya | tutorials:beginner:location_designators [2016/03/04 14:25] (current) – old revision restored (2016/01/26 10:38) gkazhoya | ||
|---|---|---|---|
| Line 30: | Line 30: | ||
| 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 |
| Let's create a new file called '' | Let's create a new file called '' | ||
| Line 56: | Line 56: | ||
| </ | </ | ||
| - | Now we'll want some code to generate a location based on a symbolic description found in a designator. | + | 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: |
| + | We would like to specify locations for the turtle to go using spatial relations, e.g.: | ||
| + | <code lisp> | ||
| + | TUT> (defparameter goal-desig | ||
| + | | ||
| + | TUT> goal-desig | ||
| + | #< | ||
| + | | ||
| + | </ | ||
| + | This way TurtleSim field will be divided into 9 areas, as shown below: | ||
| - | Here is an example of such a function, which you can append to your location-designators.lisp file: | + | {{ : |
| - | {{ : | + | Here is an example of a designator resolving function which you can append to your '' |
| <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 289: | Line 296: | ||
| [[tutorials: | [[tutorials: | ||
| - | |||

