Merge pull request #3 from gaya-/master

little bugfix for cl_json_pl_server
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.
Merge branch 'master' into groovy
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))
Fixed merge conflicts
Added examine high-level plan stub
Several changes
Refactored and split apart different phases of picking up and putting down objects
Symbol exports and initial implementation of an `examine' plan
Merge branch 'oldstash'
Making sure that the correct header files are included for Hydro
Merge pull request #6 from fairlight1337/master

Mainly changes to high-level plans, including WITH-POLICY macro, also smaller fixes.
ARM_NAVIGATION_MSGS -> SHAPE_MSGS in CRAM_MANIPULATION_KNOWLEDGE
fixed the deprecated warning in LOCATION-COSTMAP

i guess some functionality from CRAM_ROSLISP_COMMON is in ROSLISP_UTILITIES now.
finishing up the port from ARM_NAV_MSGS to MOVEIT_MSGS and SHAPE_MSGS in SEMANTIC_MAP_COLL_ENV.
got rid of deprecated functions warning from CRAM_ROSLISP_COMMON in SEM_MAP_COLL_ENV