Fix add message PoseNormStamped here instead of visuServer