add a test-bench to debug controllers

[airballking]

Would have been nice when I coded a buggy pouring controller. I dream about an interactive shell.

[airballking]

First brain-storming:

• make a 2 ROS nodes
• Node 1 generates and runs the controller with an initial joint-states
• Node 1 can be queries through a service for internals
• Node 1 is in roscpp
• Node 2 acts as a front-and with gui
• Node 2 could use rqt

[airballking]

Another thought: A test-bench has to provide the controller with all necessary inputs. Meaning: one set of joint-states, and one set of goals. I do not have a clean story for the latter, yet.

Something like an aggregated topic over which semantically annotated goals and percepts come in would be nice. Also, the controllers should then really define their inputs with names attached. Maybe even adding declaration of input-vector3, input-rotation, and input-frame would be desirable.

[airballking]

@ARoefer has been working on this, it will probably be put into its own repo.