Interactive marker grasp

* Grasp object when hand marker is clicked
* Publish diagnostics