Fixed issue with conpensating for orientation error
Still need a way to pass initial orientation argument to robot node