RobotState Interface changes:
   getPoses
   getRoot
JointState Interface changes: reset
Implementation of robot_state_publisher. It will be useful because the URDF/KDL tree will change, and the original robot_state_publisher depend of an initial KDL tree (which it is fixed, it cannot change)