changed priority queue to sorted dict
changed priority queue to sorted dict

intel-cornellcup/sim-ai,maZang/ModbotPathPlan,maZang/ModbotPathPlan,intel-cornellcup/sim-ai,intel-cornellcup/sim-ai,maZang/ModbotPathPlan