Sync Hydro to Indigo Branch - Try 2

Retry via cherry-pick method. Original: https://github.com/ros-planning/moveit_planners/pull/40