Refactored for new moveit_visual_tools API