Implementation of ant colony planner