My pathfinding code crashes the editor

Hello,

I am making a taxi for my game. What I made so far is a waypoint system and pathfinding for that system.

Today I found out a bug that crashes my entire editor while picking a goal for my taxi. But since my waypoint picking system not working properly too, I am not entirely sure when it actually happens. I can't fix none of these because of this crash. What I suspect is an endless loop in pathfinding code because when I inactive that part, it doesn't crash.

Edit: It crashes at random times whenever I call a_star_search method.

Here's my pathfinding code

 PriorityQueue<CarWaypoint> TheQueue; //For priorities
         Dictionary<CarWaypoint, CarWaypoint> CameFrom; //For where we came from to the waypoint
         Dictionary<CarWaypoint, float> CostSoFar; //Cost so far to this waypoint
     
         private void Start() //Default values
         {
             TheQueue = new PriorityQueue<CarWaypoint>();
             CameFrom = new Dictionary<CarWaypoint, CarWaypoint>();
             CostSoFar = new Dictionary<CarWaypoint, float>();
         }
     
         private float heuristic(CarWaypoint a, CarWaypoint b, float CostUntil)
         { //Heuristic function to prioritize waypoints
             return (a.transform.position - b.transform.position).magnitude + (CostUntil + 1) * 2;
         }
     
         public Dictionary<CarWaypoint, CarWaypoint> a_star_search(CarWaypoint start, CarWaypoint goal)
         {
             TheQueue.Clear(); //Clear the entire queue and clear the variables
             CameFrom = new Dictionary<CarWaypoint, CarWaypoint>();
             CostSoFar = new Dictionary<CarWaypoint, float>();
     
             TheQueue.Enqueue(start, 1); //Enqueue starting waypoint
             CameFrom.Add(start, null);
             CostSoFar.Add(start, 0);
             CarWaypoint Frontier;
     
             bool Found = false;
     
             do
             {
                 //Get the first value in our prioritized queue
                 Frontier = TheQueue.Dequeue();
     
                 //My waypoint structure includes three different direction types
                 //1. NextWaypoint, which is directly next one.
                 //2. Branches, where there is no NextWaypoint and vehicle is entering or exiting a junction
                 //3. LaneChanges where it's just a lane change on straight roads
     
                 //For NextWaypoint
                 if (Frontier.NextWaypoint != null)
                 {
                     CheckFor(Frontier.NextWaypoint, Frontier, goal);
                     if (Frontier.NextWaypoint == goal) { Found = true; break; }
                 }
                 
                 //For Branches
                 if (Frontier.Branches != null)
                 {
                     foreach (CarWaypoint obj in Frontier.Branches)
                     {
                         CheckFor(obj, Frontier, goal);
                         if (obj == goal) { Found = true; break; }
                     }
                 }
     
                 //For LaneChanges
                 if (Frontier.LaneChanges != null)
                 {
                     foreach (CarWaypoint obj in Frontier.LaneChanges)
                     {
                         CheckFor(obj, Frontier, goal);
                         if (obj == goal) { Found = true; break; }
                     }
                 }
             } while (Frontier != null && Frontier != goal && !Found);
     
             //Loop broken. Either it's been found or not found and finished the entire waypoint system..
             if (Frontier == goal || Found) //Did we found it?
             {
                 //Go back from the goal and find the start
                 Dictionary<CarWaypoint, CarWaypoint> Path = new Dictionary<CarWaypoint, CarWaypoint>();
                 CarWaypoint Last = goal;
                 while (Last != start)
                 {
                     Path.Add(CameFrom[Last], Last);
                     Last = CameFrom[Last];
                 }
                 Debug.Log("Path found");
                 return Path;
             }
             else
             {
                 Debug.Log("Path not found");
                 return null;
             }
         }
     
         //This function is to check a waypoint, how close it is to the goal and prioritize it accordingly.
         private void CheckFor(CarWaypoint Checking, CarWaypoint Current, CarWaypoint Goal)
         {
             TheQueue.Enqueue(Checking, heuristic(Checking, Goal, CostSoFar[Current]));
             //Save cost so far and where it came from.
             //Did we arrive at this waypoint before?
             if (CameFrom.ContainsKey(Checking)) //Yes?
             {
                 //Check at the cost
                 if (CostSoFar[Checking] > CostSoFar[Current] + 1)
                 {
                     //If current cost is better, replace old data with current one
                     CameFrom[Checking] = Current;
                     CostSoFar[Checking] = CostSoFar[Current] + 1;
                 }
             }
             else //If not, just write the values down
             {
                 CameFrom.Add(Checking, Current);
                 CostSoFar.Add(Checking, CostSoFar[Current] + 1);
             }
         }

A few images to help you understand the structure Waypoints structure over 3D world

Waypoint class structure