//function that scans and finds which loads are surrounded by other loads private async System.Threading.Tasks.Task <List <GridPos> > CheckForTrappedLoads(List <GridPos> pos, GridPos endPos) { int listIndex = 0; for (int i = 0; i < pos.Count; i++) { _searchGrid.SetWalkableAt(pos[i], false); _isLoad[pos[i].X, pos[i].Y] = 4; } //if the 1st AGV cannot reach a Load, then that Load is //removed from the loadPos and not considered as available - marked as "4" (temporarily trapped) do { _searchGrid.SetWalkableAt(new GridPos(pos[0].X, pos[0].Y), true); _jumpParam.Reset(pos[0], endPos); if (AStarFinder.FindPath(_jumpParam, nud_weight.Value).Count == 0) { _searchGrid.SetWalkableAt(new GridPos(pos[0].X, pos[0].Y), false); pos.Remove(pos[0]); //load is removed from the List with available Loads } else { _isLoad[pos[0].X, pos[0].Y] = 1; //otherwise, Load is marked as available listIndex = pos.Count; } } while (listIndex < pos.Count); return(pos); }
//function that determines which loads are valid to keep and which are not private void KeepValidLoads(GridPos endPoint) { int listIndex = 0; for (int i = 0; i < _loadPos.Count; i++) { _searchGrid.SetWalkableAt(_loadPos[i], true); //assumes that all loads are walkable } //and only walls are in fact the only obstacles in the grid do { bool removed = false; _jumpParam.Reset(_loadPos[listIndex], endPoint); //tries to find path between each Load and the exit if (AStarFinder.FindPath(_jumpParam, nud_weight.Value).Count == 0) //if no path is found { _isLoad[_loadPos[listIndex].X, _loadPos[listIndex].Y] = 2; //mark the corresponding load as NOT available _loads--; //decrease the counter of total loads in the grid _loadPos.RemoveAt(listIndex); //remove that load from the list removed = true; } if (!removed) { listIndex++; } } while (listIndex < _loadPos.Count); //loop repeats untill all loads are checked if (_loadPos.Count == 0) { _mapHasLoads = false; } }
//function that returns a list that contains the Available for action AGVs private List <GridPos> NotTrappedVehicles(List <GridPos> Vehicles, GridPos End) { //Vehicles is a list with all the AGVs that are inserted in the Grid by the user int list_index = 0; int trapped_index = 0; bool removed; //First, we must assume that ALL the AGVs are trapped and cannot move (trapped means they are prevented from reaching the END block) for (int i = 0; i < trappedStatus.Length; i++) { trappedStatus[i] = true; } do { removed = false; jumpParam.Reset(Vehicles[list_index], End); //we use the A* setting function and pass the //initial start point of every AGV and the final destination (end block) if (AStarFinder.FindPath(jumpParam, nud_weight.Value).Count == 0) //if the number of JumpPoints that is calculated is 0 (zero) { //it means that there was no path found Vehicles.Remove(Vehicles[list_index]); //we removed, from the returning list, the AGV for which there was no path found AGVs.Remove(AGVs[list_index]); //we remove the corresponding AGV from the public list that contains all the AGVs which will participate in the simulation removed = true; } else { trappedStatus[trapped_index] = false; //since it's not trapped, we switch its state to false } if (!removed) { AGVs[list_index].ID = list_index; list_index++; } trapped_index++; }while (list_index < Vehicles.Count); //the above process will be repeated until all elements of the incoming List are parsed. return(Vehicles); //list with NOT TRAPPED AGVs' starting points (trapped AGVs have been removed) //the point of this function is to consider every AGV as trap and then find out which AGVs //eventually, are not trapped and keep ONLY those ones. }
//Path-planner for collecting all the remaining Loads in the Grid private void GetNextLoad(int whichAgv) { aGVIndexToolStripMenuItem.Checked = false; GridPos endPos = new GridPos(); //finds the End point and uses it's coordinates as the starting coords for every AGV for (var widthTrav = 0; widthTrav < Globals.WidthBlocks; widthTrav++) { for (var heightTrav = 0; heightTrav < Globals.HeightBlocks; heightTrav++) { if (_rectangles[widthTrav][heightTrav].BoxType == BoxType.End) { try { _startPos[whichAgv] = new GridPos(widthTrav, heightTrav); _a = _startPos[whichAgv].X; _b = _startPos[whichAgv].Y; } catch { } } } } List <GridPos> loadPos = new List <GridPos>(); for (var i = 0; i < Globals.WidthBlocks; i++) { for (var j = 0; j < Globals.HeightBlocks; j++) { if (_rectangles[i][j].BoxType == BoxType.Load) { _searchGrid.SetWalkableAt(new GridPos(i, j), false); } //places the available AND the temporarily trapped loads in a list if (_isLoad[i, j] == 1 || _isLoad[i, j] == 4) { loadPos.Add(new GridPos(i, j)); } } } loadPos = CheckForTrappedLoads(loadPos, new GridPos(_a, _b), true); //scans the loadPos list to check which loads are available if (loadPos.Count == 0) { _AGVs[whichAgv].HasLoadToPick = false; return; } _isLoad[loadPos[0].X, loadPos[0].Y] = 3; _AGVs[whichAgv].MarkedLoad = new Point(loadPos[0].X, loadPos[0].Y); _loads--; endPos = loadPos[0]; //Mark all loads as unwalkable,except the targetted ones for (var m = 0; m < loadPos.Count; m++) { _searchGrid.SetWalkableAt(loadPos[m], false); } _searchGrid.SetWalkableAt(loadPos[0], true); //creates the path between the AGV (which at the moment is at the exit) and the Load _jumpParam.Reset(_startPos[whichAgv], endPos); List <GridPos> jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight); _AGVs[whichAgv].JumpPoints = jumpPointsList; //adds the result from A* to the AGV's //embedded List //Mark all loads as unwalkable for (var m = 0; m < loadPos.Count; m++) { _searchGrid.SetWalkableAt(loadPos[m], false); } int c = 0; for (short i = 0; i < _startPos.Count; i++) { c += _AGVs[i].JumpPoints.Count; if ((c - 1) > 0) { Array.Resize(ref _AGVs[i].Paths, c - 1); } } for (int j = 0; j < _AGVs[whichAgv].JumpPoints.Count - 1; j++) { GridLine line = new GridLine( _rectangles [_AGVs[whichAgv].JumpPoints[j].X] [_AGVs[whichAgv].JumpPoints[j].Y], _rectangles [_AGVs[whichAgv].JumpPoints[j + 1].X] [_AGVs[whichAgv].JumpPoints[j + 1].Y] ); _AGVs[whichAgv].Paths[j] = line; } //2nd part of route: Go to exit int oldC = c - 1; _jumpParam.Reset(endPos, _startPos[whichAgv]); jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight); _AGVs[whichAgv].JumpPoints.AddRange(jumpPointsList); c = 0; for (int i = 0; i < _startPos.Count; i++) { c += _AGVs[i].JumpPoints.Count; if ((c - 1) > 0) { Array.Resize(ref _AGVs[i].Paths, oldC + (c - 1)); } } for (short i = 0; i < _startPos.Count; i++) { for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++) { GridLine line = new GridLine( _rectangles [_AGVs[i].JumpPoints[j].X] [_AGVs[i].JumpPoints[j].Y], _rectangles [_AGVs[i].JumpPoints[j + 1].X] [_AGVs[i].JumpPoints[j + 1].Y] ); _AGVs[i].Paths[j] = line; } } Invalidate(); }
//Basic path planner function private void Redraw() { bool startFound = false; bool endFound = false; _mapHasLoads = false; GridPos endPos = new GridPos(); _posIndex = 0; _startPos = new List <GridPos>(); //list that will be filled with the starting points of every AGV _AGVs = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle _loadPos = new List <GridPos>(); //list that will be filled with the points of every Load _loads = 0; //Double FOR-loops to scan the whole Grid and perform the needed actions for (var i = 0; i < Globals.WidthBlocks; i++) { for (var j = 0; j < Globals.HeightBlocks; j++) { if (_rectangles[i][j].BoxType == BoxType.Wall) { _searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable } else { _searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now) } if (_rectangles[i][j].BoxType == BoxType.Load) { _mapHasLoads = true; _searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable _isLoad[i, j] = 1; //considers every Load as available _loads++; //counts the number of available Loads in the grid _loadPos.Add(new GridPos(i, j)); //inserts the coordinates of the Load inside a list } if (_rectangles[i][j].BoxType == BoxType.Normal) { _rectangles[i][j].OnHover(_boxDefaultColor); } if (_rectangles[i][j].BoxType == BoxType.Start) { if (_beforeStart) { _searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed } else { _searchGrid.SetWalkableAt(new GridPos(i, j), true); } startFound = true; _AGVs.Add(new Vehicle(this)); _AGVs[_posIndex].ID = _posIndex; _startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list //a & b are used by DrawPoints() as the starting x,y for calculation purposes _a = _startPos[_posIndex].X; _b = _startPos[_posIndex].Y; if (_posIndex < _startPos.Count) { _startPos[_posIndex] = new GridPos(_startPos[_posIndex].X, _startPos[_posIndex].Y); _posIndex++; } } if (_rectangles[i][j].BoxType == BoxType.End) { endFound = true; endPos.X = i; endPos.Y = j; _endPointCoords = new Point(i * Globals.BlockSide, j * Globals.BlockSide + Globals.TopBarOffset); } } } if (!startFound || !endFound) { return; //will return if there are no starting or end points in the Grid } _posIndex = 0; if (_AGVs != null) { for (short i = 0; i < _AGVs.Count(); i++) { if (_AGVs[i] != null) { _AGVs[i].UpdateAGV(); _AGVs[i].Status.Busy = false; //initialize the status of _AGVs, as 'available' } } } _startPos = NotTrappedVehicles(_startPos, endPos); //replaces the List with all the inserted _AGVs //with a new one containing the right ones if (_mapHasLoads) { KeepValidLoads(endPos); //calls a function that checks which Loads are available } //to be picked up by _AGVs and removed the trapped ones. //For-loop to repeat the path-finding process for ALL the _AGVs that participate in the simulation for (short i = 0; i < _startPos.Count; i++) { if (_loadPos.Count != 0) { var task = System.Threading.Tasks.Task.Run(() => CheckForTrappedLoads(_loadPos, endPos)); _loadPos = task.Result; //_loadPos = await CheckForTrappedLoads(_loadPos, endPos); } if (_loadPos.Count == 0) { _mapHasLoads = false; _AGVs[i].HasLoadToPick = false; } else { _mapHasLoads = true; _AGVs[i].HasLoadToPick = true; } if (_AGVs[i].Status.Busy == false) { List <GridPos> jumpPointsList; switch (_mapHasLoads) { case true: //====create the path FROM START TO LOAD, if load exists===== for (int m = 0; m < _loadPos.Count; m++) { _searchGrid.SetWalkableAt(_loadPos[m], false); //Do not allow walk over any other load except the targeted one } _searchGrid.SetWalkableAt(_loadPos[0], true); //use of the A* alorithms to find the path between AGV and its marked Load _jumpParam.Reset(_startPos[_posIndex], _loadPos[0]); jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight); _AGVs[i].JumpPoints = jumpPointsList; _AGVs[i].Status.Busy = true; //====create the path FROM START TO LOAD, if load exists===== //======FROM LOAD TO END====== for (int m = 0; m < _loadPos.Count; m++) { _searchGrid.SetWalkableAt(_loadPos[m], false); } _jumpParam.Reset(_loadPos[0], endPos); jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight); _AGVs[i].JumpPoints.AddRange(jumpPointsList); //marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load _isLoad[_loadPos[0].X, _loadPos[0].Y] = 3; _AGVs[i].MarkedLoad = new Point(_loadPos[0].X, _loadPos[0].Y); _loadPos.Remove(_loadPos[0]); //======FROM LOAD TO END====== break; case false: _jumpParam.Reset(_startPos[_posIndex], endPos); jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight); _AGVs[i].JumpPoints = jumpPointsList; break; } } _posIndex++; } int c = 0; for (short i = 0; i < _startPos.Count; i++) { c += _AGVs[i].JumpPoints.Count; } for (short i = 0; i < _startPos.Count; i++) { for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++) { GridLine line = new GridLine ( _rectangles[_AGVs[i].JumpPoints[j].X][_AGVs[i].JumpPoints[j].Y], _rectangles[_AGVs[i].JumpPoints[j + 1].X][_AGVs[i].JumpPoints[j + 1].Y] ); _AGVs[i].Paths[j] = line; } } for (int i = 0; i < _startPos.Count; i++) { if ((c - 1) > 0) { Array.Resize(ref _AGVs[i].Paths, c - 1); //resize of the _AGVs steps Table } } if (_loads != 0) { tree_stats.Nodes[2].Text = "Remaining loads: " + _loads; } else { tree_stats.Nodes[2].Text = "Remaining loads: "; } Invalidate(); }
//Basic path planner function private void Redraw() { return; bool start_found = false; bool end_found = false; mapHasLoads = false; GridPos endPos = new GridPos(); pos_index = 0; startPos = new List <GridPos>(); //list that will be filled with the starting points of every AGV AGVs = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle loadPos = new List <GridPos>(); //list that will be filled with the points of every Load //Double FOR-loops to scan the whole Grid and perform the needed actions for (int i = 0; i < Globals._WidthBlocks; i++) { for (int j = 0; j < Globals._HeightBlocks; j++) { if (m_rectangles[i][j].boxType == BoxType.Wall) { searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable } else { searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now) } if (m_rectangles[i][j].boxType == BoxType.Load) { mapHasLoads = true; searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable isLoad[i, j] = 1; //considers every Load as available loadPos.Add(new GridPos(i, j)); //inserts the coordinates of the Load inside a list } if (m_rectangles[i][j].boxType == BoxType.Normal) { m_rectangles[i][j].onHover(Globals.boxDefaultColor); } if (m_rectangles[i][j].boxType == BoxType.Start) { if (beforeStart) { searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed } else { searchGrid.SetWalkableAt(new GridPos(i, j), true); } start_found = true; AGVs.Add(new Vehicle(this)); AGVs[pos_index].ID = pos_index; startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list //a & b are used by DrawPoints() as the starting x,y for calculation purposes a = startPos[pos_index].x; b = startPos[pos_index].y; if (pos_index < startPos.Count) { startPos[pos_index] = new GridPos(startPos[pos_index].x, startPos[pos_index].y); pos_index++; } } if (m_rectangles[i][j].boxType == BoxType.End) { end_found = true; endPos.x = i; endPos.y = j; } } } if (!start_found || !end_found) { return; //will return if there are no starting or end points in the Grid } pos_index = 0; if (AGVs != null) { for (int i = 0; i < AGVs.Count(); i++) { if (AGVs[i] != null) { AGVs[i].Status.Busy = false; //initialize the status of AGVs, as 'available' } } } startPos = NotTrappedVehicles(startPos, endPos); //replaces the List with all the inserted AGVs //with a new one containing the right ones if (mapHasLoads) { KeepValidLoads(endPos); //calls a function that checks which Loads are available } //to be picked up by AGVs and removed the trapped ones. //For-loop to repeat the path-finding process for ALL the AGVs that participate in the simulation for (int i = 0; i < startPos.Count; i++) { if (loadPos.Count != 0) { loadPos = CheckForTrappedLoads(loadPos, endPos); } if (loadPos.Count == 0) { mapHasLoads = false; AGVs[i].HasLoadToPick = false; } else { mapHasLoads = true; AGVs[i].HasLoadToPick = true; } if (AGVs[i].Status.Busy == false) { List <GridPos> JumpPointsList; switch (mapHasLoads) { case true: //====create the path FROM START TO LOAD, if load exists===== for (int m = 0; m < loadPos.Count; m++) { searchGrid.SetWalkableAt(loadPos[m], false); //Do not allow walk over any other load except the targeted one } searchGrid.SetWalkableAt(loadPos[0], true); //use of the A* alorithms to find the path between AGV and its marked Load jumpParam.Reset(startPos[pos_index], loadPos[0]); JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value); AGVs[i].JumpPoints = JumpPointsList; AGVs[i].Status.Busy = true; //====create the path FROM START TO LOAD, if load exists===== //======FROM LOAD TO END====== for (int m = 0; m < loadPos.Count; m++) { searchGrid.SetWalkableAt(loadPos[m], false); } jumpParam.Reset(loadPos[0], endPos); JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value); AGVs[i].JumpPoints.AddRange(JumpPointsList); //marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load isLoad[loadPos[0].x, loadPos[0].y] = 3; AGVs[i].MarkedLoad = new Point(loadPos[0].x, loadPos[0].y); loadPos.Remove(loadPos[0]); //======FROM LOAD TO END====== break; case false: jumpParam.Reset(startPos[pos_index], endPos); JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value); AGVs[i].JumpPoints = JumpPointsList; break; } } pos_index++; } int c = 0; for (int i = 0; i < startPos.Count; i++) { c += AGVs[i].JumpPoints.Count; } for (int i = 0; i < startPos.Count; i++) { for (int j = 0; j < AGVs[i].JumpPoints.Count - 1; j++) { GridLine line = new GridLine ( m_rectangles[AGVs[i].JumpPoints[j].x][AGVs[i].JumpPoints[j].y], m_rectangles[AGVs[i].JumpPoints[j + 1].x][AGVs[i].JumpPoints[j + 1].y] ); AGVs[i].Paths[j] = line; } } for (int i = 0; i < startPos.Count; i++) { if ((c - 1) > 0) { Array.Resize(ref AGVs[i].Paths, c - 1); //resize of the AGVs steps Table } } Invalidate(); }