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.

• 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)))))

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

(defmethod location-costmap:costmap-generator-name->score ((name (eql 'behind-cost-function))) 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)))

• 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)))