Verify that the robot's actual tcp position matches with ROS

[smith-doug]

Being able to set Tools can be dangerous. If you forget to set a tool then try to move to a Cartesian position it could crash. Add a way to verify that the robot's reported position matches the one from tf.