Feature/goalset tsr

This should be merged after the or_ompl pull request for the branch of the same name.