Replaced moveit_msgs/CollisionMap usage for visualizing location costmaps

Instead of the (now unavailable) moveit_msgs/CollisionMap message type, the visualization_msgs/MarkerArray message is now used for visualizing location costmaps.
Added options for color coding visualized location costmaps
Added a flag for not elevating the height map
Added comprehensive mechanism for using policies during plan execution

The `with-policy' macro allows for letting plans (and other program constructs) be monitored by customly defined (parameterizable) policies (i.e. watching certain conditions, cleaning up when certain situations apply).

Policies are defined in this way:
(define-policy
  policy-1 (parameter-1 parameter-2)
  (:check (format t "Check condition, using parameters ~a and ~a~%"
                  parameter-1 parameter-2))
  (:recover (format t "Recover from detected condition and quit using parameters ~a and ~a~%" parameter-1 parameter-2))
  (:clean-up (format t "Clean-up using parameters ~a and ~a~%" parameter-1 parameter-2)))

Plans can then be monitored using a policy like this:
(with-policy 'policy-1 (1 2) ;; These values are now assigned to parameter-1 and parameter-2
  (plan-commands))