Address [cppcheck: duplicateIf] error.

The same condition was being checked twice, and the same action was being taken.
Address [-Wsign-compare] warning.
Address [cppcheck: postfixOperator] warning.
Address [-Wreturn-type] warning.
Address [cppcheck: duplicateExpression] error.

The existing check for NaNs is in fact correct for IEEE-compliant floating
numbers, i.e., if (a == a) then a is not a NaN, but confuses static code
analyzers. This fix instead uses the isnan(a) macro from <cmath>.
Fix broken log & output statements.

- Address [cppcheck: coutCerrMisusage] and [-Werror=format-extra-args] errors.
- ROS_ERROR -> ROS_ERROR_NAMED.
- Print size_t values portably.
Fix [-Wreorder] warning.