Using location designators with the TurtleSim

Description: in this tutorial you will learn about location designators, how to create and resolve them. You will expand a process module to make use of the location designator.

Previous Tutorial: Automatically choosing a process module for an action
Next Tutorial: Writing plans for the TurtleSim

To run the code in the tutuorial the roscore and the turtlesim need to be started over the terminal. Each in their own tab.

$ roscore

$ rosrun turtlesim turtlesim_node

And in the REPL the following commands should be executed:

CL-USER>(ros-load:load-system "cram_my_beginner_tutorial" :cram-my-beginner-tutorial)
...
CL-USER>(in-package :tut)

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 motion designators comes in how they are resolved. We've seen how motion 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. The resulting cram-my-beginner-tutorial.asd should now look like this:

(defsystem cram-my-beginner-tutorial
  :depends-on (roslisp cram-language turtlesim-msg turtlesim-srv cl-transforms geometry_msgs-msg cram-designators cram-prolog
                       cram-process-modules cram-language-designator-support cram-executive)
  :components
  ((:module "src"
            :components
            ((:file "package")
             (:file "control-turtlesim" :depends-on ("package"))
             (:file "simple-plans" :depends-on ("package" "control-turtlesim"))
             (:file "motion-designators" :depends-on ("package"))
             (:file "location-designators" :depends-on ("package"))
             (:file "process-modules" :depends-on ("package"
                                                   "control-turtlesim"
                                                   "simple-plans"
                                                   "motion-designators"))
             (:file "selecting-process-modules" :depends-on ("package"
                                                             "motion-designators"
                                                             "process-modules"))))))

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
#<A LOCATION
​    (VERTICAL-POSITION BOTTOM)
    (HORIZONTAL-POSITION LEFT)>

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. That means, you might get a different number of solutions than what we got: we got 3 but you might be 4 or 5 or something else.

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

(def-fact-group goal-motions (motion-grounding)
  (<- (motion-grounding ?desig (go-to ?point))
    (desig-prop ?desig (:type :going-to))
    (desig-prop ?desig (:goal ?point))))

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

Now for the process module, let's add a new case to our navigation process-module that handles motion designators with above-mentioned properties and a function goto-location that will invoke the process module with the new designator. Your process-modules.lisp should now look something like this:

(in-package :tut)
 
(def-process-module turtlesim-navigation (motion-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "TurtleSim navigation invoked with motion designator `~a'."
                    motion-designator)
  (destructuring-bind (command motion) (reference motion-designator)
    (ecase command
      (drive
         (send-vel-cmd
          (turtle-motion-speed motion)
          (turtle-motion-angle motion)))
      (move
       (move-to motion))
      (go-to
       (when (typep motion 'location-designator)
         (let ((target-point (reference motion)))
           (roslisp:ros-info (turtle-process-modules)
                             "Going to point ~a." target-point)
           (move-to target-point)))))))
 
(def-process-module turtlesim-pen-control (motion-designator)
  (roslisp:ros-info (turtle-process-modules)
                    "TurtleSim pen control invoked with motion designator `~a'."
                    motion-designator)
  (destructuring-bind (command motion) (reference motion-designator)
    (ecase command
      (set-pen
       (call-set-pen
        (pen-motion-r motion)
        (pen-motion-g motion)
        (pen-motion-b motion)
        (pen-motion-width motion)
        (pen-motion-off motion))))))
 
(defun drive (?speed ?angle)
  (top-level
    (with-process-modules-running (turtlesim-navigation)
      (let ((trajectory (desig:a motion (type driving) (speed ?speed) (angle ?angle))))
        (pm-execute 'turtlesim-navigation trajectory)))))
 
(defun move (?x ?y)
  (top-level
    (with-process-modules-running (turtlesim-navigation)
      (let ((goal (desig:a motion (type moving) (goal (?x ?y 0)))))
        (pm-execute 'turtlesim-navigation goal)))))
 
(defun goto-location (?horizontal-position ?vertical-position)
  (let ((turtle-name "turtle1"))
    (start-ros-node turtle-name)
    (init-ros-turtle turtle-name)
    (top-level
      (with-process-modules-running (turtlesim-navigation)
        (let* ((?area (desig:a location
                               (horizontal-position ?horizontal-position)
                               (vertical-position ?vertical-position)))
               (goal (desig:a motion (type going-to) (goal ?area))))
          (cram-executive:perform goal))))))

In goto-location we use perform, which means we have to make sure the right process module for our going-to designator can be found. For that we add a rule to the fact-group available-turtle-process-modules from the selecting-process-modules.lisp file. The file should now look like this:

(in-package :tut)
 
(def-fact-group available-turtle-process-modules (available-process-module
                                                  matching-process-module)
  (<- (available-process-module turtlesim-navigation))
  (<- (available-process-module turtlesim-pen-control))
 
  (<- (matching-process-module ?desig turtlesim-navigation)
    (desig-prop ?desig (:type :driving)))
  (<- (matching-process-module ?desig turtlesim-navigation)
    (desig-prop ?desig (:type :moving)))
  (<- (matching-process-module ?desig turtlesim-navigation)
    (desig-prop ?desig (:type :going-to)))
  (<- (matching-process-module ?desig turtlesim-pen-control)
    (desig-prop ?desig (:type :setting-pen))))
 
(defun perform-some-motion (motion-desig)
  (top-level
    (with-process-modules-running (turtlesim-navigation turtlesim-pen-control)
      (cram-executive:perform motion-desig))))

It's the same as the other ones. If the type of ?desig is going-to, the rule applies and turtlesim-navigation gets selected.

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

TUT> (goto-location :right :top)
[(ROSLISP TOP) INFO] 1501153969.640: Node name is /turtle1
[(ROSLISP TOP) INFO] 1501153969.640: Namespace is /
[(ROSLISP TOP) INFO] 1501153969.641: Params are NIL
[(ROSLISP TOP) INFO] 1501153969.641: Remappings are:
[(ROSLISP TOP) INFO] 1501153969.641: master URI is 127.0.0.1:11311
[(ROSLISP TOP) INFO] 1501153970.649: Node startup complete
[(TURTLE-PROCESS-MODULES) INFO] 1562698457.619: TurtleSim navigation invoked with motion designator `#<A MOTION
    (TYPE GOING-TO)
    (GOAL #<A LOCATION
    (HORIZONTAL-POSITION RIGHT)
    (VERTICAL-POSITION TOP)>)>'.
[(TURTLE-PROCESS-MODULES) INFO] 1501153970.691: Going to point #<3D-VECTOR (10.131428718566895d0 8.874866485595703d0 0.0d0)>.
T

If you already had a running node in the REPL, you might get the error shown below but that is absolutely normal: ROS simply notifies you that it is going to shut down the previous node and start a new one.

WARNING:
   Before starting node, node-status equalled RUNNING instead of :shutdown.  Shutting the previous node invocation down now.

The turtle should also have moved to somewhere in the vicinity of the top-right corner of its window.

Let's put everything we made so far together to write some high-level plans.

Writing high-level plans