Simplify logic in PSM

[v4hn]

I rebased the patches from #2563 onto master. Assuming CI succeeds, this can be merged (it was reviewed before) and directly backported to melodic-devel as the original author proposed the PR for melodic-devel.

[codecov]

Codecov Report

Merging #2632 (f8ba13f) into master (27c8a13) will decrease coverage by 0.11%.
The diff coverage is 75.00%.

@@            Coverage Diff             @@
##           master    #2632      +/-   ##
==========================================
- Coverage   60.52%   60.42%   -0.10%     
==========================================
  Files         402      402              
  Lines       29619    29241     -378     
==========================================
- Hits        17923    17665     -258     
+ Misses      11696    11576     -120     

Impacted Files	Coverage Δ
...sion_plugin_loader/src/collision_plugin_loader.cpp	40.00% <50.00%> (+4.87%)	⬆️
...nning_scene_monitor/src/planning_scene_monitor.cpp	67.21% <100.00%> (-1.46%)	⬇️
.../ompl_interface/src/detail/constrained_sampler.cpp	43.25% <0.00%> (-16.21%)	⬇️
...ove_group_interface/src/wrap_python_move_group.cpp	35.11% <0.00%> (-2.93%)	⬇️
...meterization/work_space/pose_model_state_space.cpp	83.34% <0.00%> (-0.64%)	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	83.89% <0.00%> (-0.32%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	50.68% <0.00%> (-0.09%)	⬇️
...e/collision_detection_fcl/src/collision_common.cpp	78.61% <0.00%> (+0.26%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 27c8a13...f8ba13f. Read the comment docs.