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

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.

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

TUT> (defparameter goal-desig
       (make-designator :location '((:vertical-position :bottom) (:horizontal-position :left))))
TUT> goal-desig
#<LOCATION-DESIGNATOR ((:VERTICAL-POSITION :BOTTOM)
                       (:HORIZONTAL-POSITION :LEFT)) {1008548CD3}>

This way TurtleSim field will be divided into 9 areas, as shown below:

Here is an example of a designator resolving function which you can append to your location-designators.lisp file:

(in-package :tut)
 
(defun navigation-goal-generator (designator)
  (declare (type location-designator designator))
  (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 5
            collect (cl-transforms:make-3d-vector
                     (+ x-offset (random (/ 11.0 3.0)))
                     (+ y-offset (random (/ 11.0 3.0)))
                     0)))))

Let's compile the new function and test it in the REPL on the example designator:

TUT> (navigation-goal-generator goal-desig)
(#<3D-VECTOR (1.0404995679855347d0 0.03507864475250244d0 0.0d0)>
 #<3D-VECTOR (3.0929160118103027d0 0.8247395753860474d0 0.0d0)>
 #<3D-VECTOR (3.048727035522461d0 1.0249313116073608d0 0.0d0)>
 #<3D-VECTOR (2.1428534984588623d0 2.775157928466797d0 0.0d0)>
 #<3D-VECTOR (3.3696107864379883d0 0.026859402656555176d0 0.0d0)>)

The function expects a location designator as a parameter and returns a list of solutions (in this case points in the bottom left corner of the TurtleSim field: points between 0 and 3.667 in x and y). This is not accidental; if we want to register this function as a location generator (which we will do next), 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.

To register the function as a location generator we simply append the following to our location-designators.lisp:

(register-location-generator
 5 navigation-goal-generator)

One of the parameters of cram-designators:register-location-generator is a function name that we want to register. The number parameter is the priority, and it will order the calls to the generators. Lower numbers mean called earlier.

Let's see if it works, don't forget to recompile the file / reload the system:

TUT> (reference goal-desig)
#<3D-VECTOR (1.142098307609558d0 2.184809684753418d0 0.0d0)>
TUT> (reference goal-desig)
#<3D-VECTOR (1.142098307609558d0 2.184809684753418d0 0.0d0)>

So far so good, we get a pose, and it is in fact in the bottom left corner. 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 goal-desig)
#<LOCATION-DESIGNATOR ((:VERTICAL-POSITION :BOTTOM)
                       (:HORIZONTAL-POSITION :LEFT)) {10086B5A23}>

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 goal-desig))
#<3D-VECTOR (0.261885404586792d0 2.649489164352417d0 0.0d0)>

If we called (reference (next-solution goal-desig)) again, we would get this same solution again. If goal-desig 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> (loop for desig = goal-desig then (next-solution desig)
           while desig
           do (print (reference desig)))
#<3D-VECTOR (1.142098307609558d0 2.184809684753418d0 0.0d0)> 
#<3D-VECTOR (0.261885404586792d0 2.649489164352417d0 0.0d0)> 
#<3D-VECTOR (2.560281276702881d0 1.5541117191314697d0 0.0d0)> 
#<3D-VECTOR (3.182616949081421d0 1.329969882965088d0 0.0d0)> 
#<3D-VECTOR (1.3107243776321411d0 2.9240825176239014d0 0.0d0)> 
NIL

Now let's add a validation function. The points we get from navigation-goal-generator range from 0 to 11 but whenever the turtle has a coordinate above 10.5 part of it disappears from the screen. Let's improve our location designator resolution by rejecting the points that lie outside of the [0.5, 10.5] range. For that append the following to your location-designators.lisp file:

(defun navigation-goal-validator (designator solution)
  (declare (type location-designator designator))
  (when (and (desig-prop-value designator :vertical-position)
             (desig-prop-value designator :horizontal-position))
    (when (typep solution 'cl-transforms:3d-vector)
      (when
          (and
           (>= (cl-transforms:x solution) 0.5)
           (>= (cl-transforms:y solution) 0.5)
           (<= (cl-transforms:x solution) 10.5)
           (<= (cl-transforms:y solution) 10.5))
        :accept))))
 
(register-location-validation-function
 5 navigation-goal-validator)

If designator contains the properties for which we are checking the generated solution and if solution is of type 3d-vector, the function will accept all the solutions that lie in the above-mentioned limits and reject everything else (NIL is equivalent to :reject for location validators).

The last function call registers our validator, validators are prioritized just as the generators.

Let's test if this worked, don't forget to recompile the file / reload the system:

TUT> (defparameter another-goal
       (make-designator :location '((:vertical-position :bottom) (:horizontal-position :left))))
ANOTHER-GOAL
TUT> (loop for desig = another-goal then (next-solution desig)
           while desig
           do (print (reference desig)))
#<3D-VECTOR (1.5110043287277222d0 2.1800525188446045d0 0.0d0)> 
#<3D-VECTOR (2.030768394470215d0 2.731144428253174d0 0.0d0)> 
#<3D-VECTOR (0.7122993469238281d0 3.623168706893921d0 0.0d0)> 
NIL

Depending on the random number generator we will get some or none of the solutions rejected and, therefore, ⇐ number of valid solutions for our designator another-goal.

Let's try to create a process module to make use of a location designator as well. Append the following to action-designators.lisp:

(def-fact-group goal-actions (action-desig)
  (<- (action-desig ?desig (go-to ?point))
    (desig-prop ?desig (:type :goal))
    (desig-prop ?desig (:goal ?point))))

This will resolve any action designator with properties ( (:type :goal) (:goal some-location-designator) ) into (go-to some-location-designator) instruction.

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:

(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)
  (roslisp:ros-info (turtle-process-modules)
                    "Turtle simple navigation invoked with action designator `~a'."
                    action-designator)
  (destructuring-bind (command action-goal) (reference action-designator)
    (ecase command
      (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)))))))
 
(defmacro with-turtle-process-modules (&body body)
  `(with-process-modules-running
       (actionlib-navigation
        simple-navigation)
     ,@body))
 
(defun draw-hexagon (radius)
  (let ((turtle-name "turtle1"))
    (start-ros-node turtle-name)
    (init-ros-turtle turtle-name)
    (top-level
      (with-turtle-process-modules
        (process-module-alias :navigation 'actionlib-navigation)
        (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))))))

And let's give it a go. Reload the tutorial in roslisp_repl then in the command line of REPL:

TUT> (goto-location :center :center)
[(ROSLISP TOP) INFO] 1453758117.881: Node name is /turtle1
[(ROSLISP TOP) INFO] 1453758117.881: Namespace is /
[(ROSLISP TOP) INFO] 1453758117.885: Params are NIL
[(ROSLISP TOP) INFO] 1453758117.885: Remappings are:
[(ROSLISP TOP) INFO] 1453758117.885: master URI is 127.0.0.1:11311
[(ROSLISP TOP) INFO] 1453758119.036: Node startup complete
[(TURTLE-PROCESS-MODULES) INFO] 1453758119.377: Turtle simple navigation invoked with action designator
`#<ACTION-DESIGNATOR ((TYPE GOAL)
                      (GOAL #<LOCATION-DESIGNATOR ((HORIZONTAL-POSITION CENTER)
                                                   (VERTICAL-POSITION CENTER)) {10095B8283}>)) {10095B87D3}>'.
[(TURTLE-PROCESS-MODULES) INFO] 1453758119.386: Going to point #<3D-VECTOR (6.038690567016602d0 6.027290344238281d0 0.0d0)>.
T

The turtle should also have moved to somewhere in the vicinity of the center of its window.

So far we called process modules directly. Sometimes it's better to let the system decide on its own …

Automatically choosing a process module for an action