Location costmaps

This tutorial demonstrates the location costmap library of CRAM, including its API, examples of existing costmaps and how to write your own new costmaps.

If you recall, the resolution of a location designator is divided into two steps - a generation step and a verification step. The generators yield sequences of solution candidates in the form of lazy lists and verification functions accept or reject solutions. The constraints specified with designators normally restrict the solution space but still leave many potentially valid solutions. The process of resolving location designator properties and generating poses in space is split up into three parts in order to deal with the computational complexity of the six-dimensional space of poses and to make a sampling-based approach feasible. Informally, the generation of a pose can be resolved as follows:

1. Generate a two-dimensional grid based on location designator constraints with values greater than zero for all potentially valid solutions. It is a map representing the solution density of a specific cell where greater values represent more solutions for a specific constraint.
2. Use the grid as a probability density function to generate random samples.
3. Use Heuristics to resolve the orientation and Z coordinate of the generated pose.
4. Use the physics-based reasoning system to prove that the solution is valid.

As can be seen, the first three steps generate a solution candidate while the last step verifies it.

Costmaps are density maps of two-dimensional matrices of positive real numbers which allow for representing spacial constraints such as on or in but also represent locations from which objects can be reached to a certain extent. Additionally, they can be merged easily and they can be converted to valid probability density functions to be used for sampling. Each entry corresponds to a grid cell in the x-y-plane of the robot's environment. Values of zero indicate that the corresponding grid cell cannot be a solution for the designator to be resolved and values greater than zero indicate potential solution candidates.

For further reading, refer to section 4 of Lorenz Mösenlechner Ph.D. Thesis.

• launch the map-server in a terminal with

   roslaunch iai_maps map.launch 

• load cram_occupancy_grid_costmap in the REPL

• start a node in REPL with (roslisp-utilities:startup-ros)

• define costmap parameters:

(prolog:def-fact-group costmap-metadata ()
  (prolog:<- (location-costmap:costmap-size 10 10)) ; in meters
  (prolog:<- (location-costmap:costmap-origin -5 -5))
  (prolog:<- (location-costmap:costmap-resolution 0.05))
  (prolog:<- (location-costmap:costmap-padding 0.01))) ; padding to occupancy map obstacles

• create a designator and resolve it:

(defparameter *designator* (desig:a location (visible-for pr2)))
(desig:reference *designator*)

• start rviz and add Map, Marker and MarkerArray
  • as “Topic” for the Map select “/map”
  • as “Marker Topic” select “/cram_location_marker” and “/cram_location_costmap” respectively.

• to see which costmap was used, call this:

(defparameter *costmap*
     (cut:var-value 
          '?cm
          (car (prolog:prolog `(and (location-costmap:desig-costmap ,*designator* ?cm))))))

• to visualize the costmap and see generated values:

(location-costmap:get-cost-map *costmap*)

–> CRAM costmaps calculate values from 0 to 1: 0 – bad sample, 1 – perfect sample

• In rviz you should see something like this:

• to remove visualization markers call

(location-costmap::remove-markers-up-to-index 10000)

To define your own location relations you need to do three things:

1. Define a cost function which resolves the relation you want
2. Register a name for your cost function
3. Define rules for generating a costmap using your cost function associate it with your registered name

Let's define a behind relation.

• create a cost function which returns for each {x, y} in location costmap grid a value between [0, 1]

(defun make-behind-cost-function (ref-x ref-y)
           "`ref-x' and `ref-y' are the coordinates of the reference point according to which the behind-relation is resolved."
           (let* ((supp-tf (cl-transforms:make-transform
                               (cl-transforms:make-3d-vector ref-x ref-y 0)
                               (cl-transforms:make-identity-rotation)))
                  (world->supp-tf (cl-transforms:transform-inv supp-tf)))
             (lambda (x y)
               (let* ((point (cl-transforms:transform-point world->supp-tf
                                                            (cl-transforms:make-3d-vector x y 0)))
                      (vector-length (cl-transforms:dot-product point point)))
                 (if (and (< (cl-transforms:x point) 0.0d0)
                          (> (abs (/ (cl-transforms:x point) vector-length)) 0))
                     (abs (/ (cl-transforms:x point) vector-length))
                     0.0d0)))))

To summarize this, we can see that the individual value in the costmap generator is set to the normalized x value of the sampling point if it lies behind the reference point (when x is negative compared to the reference). Else it's set to 0.

• define order for your costmap function and give it a name:

(defmethod location-costmap:costmap-generator-name->score ((name (eql 'behind-cost-function))) 10)

The name of our costmap function would be behind-cost-function and it's order is 10

• define the prolog rule for generating costmaps:

(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
           (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
             (desig:desig-prop ?designator (:behind ?pose))
             (prolog:lisp-fun cl-transforms:origin ?pose ?pose-origin)
             (prolog:lisp-fun cl-transforms:x ?pose-origin ?ref-x)
             (prolog:lisp-fun cl-transforms:y ?pose-origin ?ref-y)
             (location-costmap:costmap ?costmap)
             (location-costmap:costmap-add-function
              behind-cost-function
              (make-behind-cost-function ?ref-x ?ref-y)
              ?costmap)))

In short, the reference x and y is obtained from the pose we have to calculate the behind relation to. These are then passed on to our previously defined costmap generator, which in turn will generate the costmap we require.

• resolve a designator with your new awesome costmap:

(defparameter *behind-designator*
           (desig:make-designator :location `((:behind ,(cl-transforms:make-identity-pose)))))
(desig:reference *behind-designator*)

• if you were to visualize the costmap like above you would get this:

(location-costmap:get-cost-map
 (cut:var-value
  '?cm
  (car (prolog:prolog `(and (location-costmap:desig-costmap ,*behind-designator* ?cm))))))

In the location-costmap package there are other cost function already available for generating costmaps. Here are two examples:

• using gaussian costmap function:

(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
    (desig:desig-prop ?designator (:behind ?pose))
    (prolog:lisp-fun cl-transforms:origin ?pose ?pose-origin)
    (location-costmap:costmap ?costmap)
    (location-costmap:costmap-add-function
     behind-cost-function
     (location-costmap:make-gauss-cost-function ?pose-origin #2A((0.5 0) (0 0.5)))
     ?costmap)))

• using range costmap function:

(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
    (desig:desig-prop ?designator (:behind ?pose))
    (location-costmap:costmap ?costmap)
    (location-costmap:costmap-add-function
     behind-cost-function
     (location-costmap:make-range-cost-function ?pose 1.0)
     ?costmap)))
(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
    (desig:desig-prop ?designator (:behind ?pose))
    (location-costmap:costmap ?costmap)
    (location-costmap:costmap-add-function
     behind-cost-function
     (location-costmap:make-range-cost-function ?pose 1.0 :invert t)
     ?costmap)))