Changed mt_bfs locking on the frontier_node_q and node_distance_q so that the locking actually happens all at once and then nodes are inserted in batch into the queues.