robot_on_heightmap is working