DEBUG the TF

see ROS TF

Plot the tf tree: rosrun rqt_tf_tree rqt_tf_tree

print tf_echo