Change init mapping

Now the mapping starts only after the robot has initialize