Added macro and adjacent infrastructure for defining policies during plans

In order to define a policy for use while plan performance, three code blocks and a list of parameters must be supplied:

(define-policy object-moved (object)
  (:check (format t "Testing for movement of object ~a~%" object)
          (do-object-checking-here)
  (:clean-up (format t "Cleaning up~%")
  	     (do-clean-up-here))
  (:recover (format t "Recovering~%")
  	    (do-recovery-here))))

During a plan, call it like this:

(with-policy object-moved (?obj)
  (do-plan-actions-with-policy-awareness))

The parameter `object' defined during definition of the policy is replaced by `?obj' during the call of `with-policy' and is available as `object' to all code blocks of the policy (currently `check', `clean-uo', `recover').

`check' is called whenever the policy is checked for triggering.
`clean-up' is called during the clean-up-phase of unwind-protect (so it is called either way)
`recover' is called when the policy gets triggered before the main body of `with-policy' is completed.

If the main body is completed without the policy getting triggered, only `clean-up' is called and should be treated like that. `with-policy' returns the return value of the main body. If a policy is referred to that was not defined before (i.e. does not exist), a plan failure of type `policy-not-found' is signalled.
Separated the object-in-hand plan into two encapsulated plans

Easier to develop and debug. Also, the functionality is now split up into two bits:
a) Moving around, perceiving objects from far distance, moving close to them
b) Being close to the objects, perceiving them from near again and doing actual grasping
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))
Merge branch 'master' of github.com:fairlight1337/cram_highlevel