Separated the GP publisher logic into a class. Now using poses from TF instead of trying to synchronize pose and wifi topics