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: | ||
- |