Added edit of theta if publish nav_msgs to /wall_brain/pose_correction