Get subgroups helper

Already merged in hydro
https://github.com/ros-planning/moveit_core/pull/178#event-137762008

Is it ok if I merge things like this myself, after the build farm passes, to simplify this dual branch process? It has already been reviewed for hydro.