Ported to new moveit_visual_tools API