Fix compiler warnings

Similar to ros-planning/moveit_ros#451, but targeting moveit_core.

Most fixes are on enforcing compiler strictness and minor performance improvements. However, 4130e2b is behavior-changing. I believe it's a bugfix.