Pose toQPose(flatworld::Pose p) { Pose qp; qp.setPosition({p.x, p.y}); qp.setOrientation(p.heading); return qp; }