Use diagnostic updater rather than publish diagnostic_msgs directly