Added some comments to the action and msg
Temporal fix for making it work (@bmagyar will really fix it at some point).
fix functions with no return statement and other
cppcheck errors fix
Remove self assignment
Improved and updated the test file
Tuned parameters to work on real robot

Without the longer time in the grasp, and without going a bit away of joint limits (this still closes the hand fully, grasps a can) the moveit pick plans fail.