Changed PoseStamped_to_TF.cpp to update 10 times a second. Not sure if this is necessary. Also made the goalPose transform to initialize as the robot0 transform. Also added lots of comments.