======DEBUG the TF =====
  * see [[http://wiki.ros.org/tf|ROS TF]]
  * Plot the tf tree: ''rosrun rqt_tf_tree rqt_tf_tree''
  * print tf_echo