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.

  • load cram_location_costmap in REPL
  • start a node in REPL with (roslisp-utilities:startup-ros :name “lisp_node” :anonymous nil)
  • start rviz and add Marker and MarkerArray, topic should contain lisp_node
  • 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

  • load cram_occupancy_grid_costmap package in REPL
  • create a designator and resolve it:

      (setf designator (desig:make-designator :location `((:to :see))))
      (desig:reference designator)

  • to see which costmap was used, call this:

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

–> CRAM costmaps need values from 0 to 1: 0 – bad value, 1 – perfect for designator description

  • to visualize the costmap and see generated values:

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

  • to remove visualization call

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

  • 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
   relation is resolved."
  (let* ((translated-supp-pose (cl-transforms:make-transform
                                (cl-transforms:make-3d-vector ref-x ref-y 0)
                                (cl-transforms:make-identity-rotation)))
         (world->supp-trans (cl-transforms:transform-inv translated-supp-pose)))
    (lambda (x y)
      (let* ((point (cl-transforms:transform-point world->supp-trans
                                                   (cl-transforms:make-3d-vector x y 0)))
             (vector-length (sqrt (+ (* (cl-transforms:x point) (cl-transforms:x point))
                                     (* (cl-transforms:y point) (cl-transforms:y point))))))
        (if (< (cl-transforms:x point) 0.0d0)
            (if (> (abs (/ (cl-transforms:x point) vector-length)) 0)
                (abs (/ (cl-transforms:x point) vector-length))
                0.0d0)
            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 left-of-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 your new awesome designator:

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

  • using gaussian costmap function:

    (prolog:def-fact-group left-of-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 left-of-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 left-of-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)))