fix heading normalization, using ROS angles lib + sort members constructors