Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:beginner:location_designators [2016/01/25 21:23] – gkazhoyatutorials:beginner:location_designators [2016/03/04 14:25] (current) – old revision restored (2016/01/26 10:38) gkazhoya
Line 56: Line 56:
 </code> </code>
  
-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: one analogous to action designators' ''(action-desig ?desig ?solution)'' Prolog predicate, for locations it's ''(desig-solution ?desig ?solution)'' (predicate names could've been more similar, but, oh well). However, with Prolog rules all solutions have more or less the same priority, and if one constraint can be resolved to multiple solutions there is no way to prioritize one over the other. One use case of priorities is to always just in case check robot's current position when a pose for the robot base is being resolved: if robot's current pose is already sufficient to execute some task (location validator says '':accept'') this mechanism can save some time on re-positioning. In this case robot's current pose will always have higher priority over the rest of location generators. In this tutorial we will implement the same functionality using both interfaces and compare the pros and cons of both.+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: one analogous to action designators' ''(action-desig ?desig ?solution)'' Prolog predicate, for locations it's ''(desig-solution ?desig ?solution)'' (predicate names could've been more similar, but, oh well), the mechanism is exactly the same as for action designators. However, with Prolog rules all solutions have more or less the same priority, and if one constraint can be resolved to multiple solutions there is no way to prioritize one over the other. One use case of priorities is to always just in case check robot's current position when a pose for the robot base is being resolved: if robot's current pose is already sufficient to execute some task (location validator says '':accept'') this mechanism can save some time on re-positioning. In this case robot's current pose will always have higher priority over the rest of location generators. In this tutorial we will implement a custom generator function that can be prioritized. In fact, the solutions coming from Prolog predicate ''(desig-solution ?location-desig ?solution)'' are also implemented as a custom generator with priority 10. We will discuss priorities in more detail below.
  
 We would like to specify locations for the turtle to go using spatial relations, e.g.: We would like to specify locations for the turtle to go using spatial relations, e.g.:
Line 209: Line 209:
 </code> </code>
  
 +This will resolve any action designator with properties ''( (:type :goal) (:goal some-location-designator) )'' into ''(go-to some-location-designator)'' instruction.
  
- ''process-modules.lisp'':+Now for the process module, let's add a new process module called ''simple-navigation'' that accepts action designators with above-mentioned properties and a function ''goto-location'' that will invoke the new process module. Also, let's add ''simple-navigation'' to the running process modules in our ''with-turtle-process-modules'' macro. You ''process-modules.lisp'' should now look something like this:
  
 <code lisp> <code lisp>
 +(in-package :tut)
 +
 +(def-process-module actionlib-navigation (action-designator)
 +  (roslisp:ros-info (turtle-process-modules)
 +                    "Turtle shape navigation invoked with action designator `~a'."
 +                    action-designator)
 +  (destructuring-bind (command action-goal) (reference action-designator)
 +    (ecase command
 +      (draw-shape
 +       (call-shape-action
 +        :edges (turtle-shape-edges action-goal)
 +        :radius (turtle-shape-radius action-goal))))))
 +
 (def-process-module simple-navigation (action-designator) (def-process-module simple-navigation (action-designator)
   (roslisp:ros-info (turtle-process-modules)   (roslisp:ros-info (turtle-process-modules)
-                    "Turtle navigation invoked with location designator `~a'." +                    "Turtle simple navigation invoked with action designator `~a'." 
-                    location-designator) +                    action-designator) 
-  (let ((target-pose (reference location-designator))) +  (destructuring-bind (command action-goal) (reference action-designator) 
-       (print (cl-transforms:origin target-pose)) +    (ecase command 
-       (move-to (cl-transforms:origin target-pose))))+      (go-to 
 +       (when (typep action-goal 'location-designator) 
 +         (let ((target-point (reference action-goal))) 
 +           (roslisp:ros-info (turtle-process-modules) 
 +                             "Going to point ~a." target-point) 
 +           (move-to target-point)))))))
  
-(defun goto-location (location-desig)+(defmacro with-turtle-process-modules (&body body) 
 +  `(with-process-modules-running 
 +       (actionlib-navigation 
 +        simple-navigation) 
 +     ,@body)) 
 + 
 +(defun draw-hexagon (radius)
   (let ((turtle-name "turtle1"))   (let ((turtle-name "turtle1"))
     (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 (turtle-navigation) +      (with-turtle-process-modules 
-        (cpm:process-module-alias :navigation 'turtle-navigation) +        (process-module-alias :navigation 'actionlib-navigation) 
-            (cpm:pm-execute :navigation location-desig)))))+        (with-designators 
 +            ((trajectory :action `((:type :shape) (:shape :hexagon) (:radius ,radius)))) 
 +          (pm-execute :navigation trajectory)))))) 
 + 
 +(defun goto-location (horizontal-position vertical-position) 
 +  (let ((turtle-name "turtle1")) 
 +    (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 `((:horizontal-position ,horizontal-position) 
 +                               (:vertical-position ,vertical-position))) 
 +             (goal :action `((:type :goal) (:goal ,area)))) 
 +          (pm-execute :navigation goal))))))
 </code> </code>
  
-And let's give it a go. Reload the tutorial into roslisp_repl then in the command line of REPL:+And let's give it a go. Reload the tutorial in ''roslisp_repl'' then in the command line of REPL:
  
 <code lisp> <code lisp>
-TUT> (defparameter turtle-target (make-designator 'location `((vpos center) (hpos center)))) +TUT> (goto-location :center :center) 
-TURTLE-TARGET +[(ROSLISP TOP) INFO] 1453758117.881: Node name is /turtle1 
-TUT> (goto-location turtle-target) +[(ROSLISP TOP) INFO] 1453758117.881: Namespace is / 
-WARNING: +[(ROSLISP TOP) INFO] 1453758117.885: Params are NIL 
-   Before starting node, node-status equalled RUNNING instead of :shutdown.  Shutting the previous node invocation down now. +[(ROSLISP TOP) INFO] 1453758117.885: Remappings are: 
-[(ROSLISP EVENT-LOOP) INFO] 1414081756.703: Terminating ROS Node event loop +[(ROSLISP TOP) INFO] 1453758117.885: master URI is 127.0.0.1:11311 
-[(ROSLISP TOP) INFO] 1414081757.022: Shutdown complete +[(ROSLISP TOP) INFO] 1453758119.036: Node startup complete 
-[(ROSLISP TOP) INFO] 1414081757.028: Node name is /turtle1 +[(TURTLE-PROCESS-MODULES) INFO] 1453758119.377: Turtle simple navigation invoked with action designator 
-[(ROSLISP TOP) INFO] 1414081757.028: Namespace is / +`#<ACTION-DESIGNATOR ((TYPE GOAL) 
-[(ROSLISP TOP) INFO] 1414081757.028: Params are NIL +                      (GOAL #<LOCATION-DESIGNATOR ((HORIZONTAL-POSITION CENTER) 
-[(ROSLISP TOP) INFO] 1414081757.028: Remappings are: +                                                   (VERTICAL-POSITION CENTER)) {10095B8283}>)) {10095B87D3}>'. 
-[(ROSLISP TOP) INFO] 1414081757.028: master URI is 127.0.0.1:11311 +[(TURTLE-PROCESS-MODULES) INFO] 1453758119.386: Going to point #<3D-VECTOR (6.038690567016602d0 6.027290344238281d0 0.0d0)>.
-[(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 T
 </code> </code>
  
-The turtle should also have moved to somewhere in the vecinity of the center of its window.+The turtle should also have moved to somewhere in the vicinity of the center of its window.
  
 == Next == == Next ==