Port [PR 2402](https://github.com/ros-planning/moveit/pull/2402), this would enhance [PR 347](https://github.com/ros-planning/moveit2/pull/347).
Port PR 2402, this would enhance PR 347.