Moved functionality into robot_viz_tools
Merged