Have added code that SHOULD reinitialize amcl to where the april tags are saying the robot is