//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();
}
