Implement a Generic MoveToStaging, and perhaps a service to return the robot arm state

[amalnanavati]

See #113 's failed test case (in the main PR description) for motivation.

This PR is to implement a MoveToStagingGeneric, that first check's the robot end effector pose. If it is closer to the user than the staging pose, it servos. Else it uses a kinematic motion.

It may also be beneficial to implement a ROS service that returns what state the robot arm is currently in (e.g., above plate, at mouth, etc.) so the app can use that info.