Fixed dynamic_reconfigure build bug and refactor
Fixed two compile errors on OS X with clang