Pose2D( const Pose &p) : position(Vector2d(p.position.x(), p.position.y())), orientation(p.getYaw()) {}