Moved functionality into robot_viz_tools