1
0
forked from s425077/PotatoPlan
PotatoPlan/route-planning.md

104 lines
4.2 KiB
Markdown
Raw Normal View History

2020-05-04 01:56:37 +02:00
# intelligent tractor route planning implementation report
2020-05-04 01:56:37 +02:00
A* has been implemented. As of now, there are 4 types of fields wtih different costs:
Grass - 1
Dirt - 7
Crops - 15
4th type are obstacles (mountains) which are unpassable.
Apart from costs of each fields also cost or turning the agent has been implemented.
Turning once costs 2
Turning twice costs 9
Cost of turning twice is so high to encourage the agent to try different path rather that just turning back after reaching its destination point.
2020-05-04 01:56:37 +02:00
All costs can be changed and are not final values.
2020-05-04 01:56:37 +02:00
Very rarely agent will make a little bump instead of going forward.
As of now we were not able to detect if it is a problem with A* algorithm or some other part of the program.
2020-05-04 01:56:37 +02:00
Snippets of code:
2020-05-04 01:56:37 +02:00
A* main loop:
2020-05-04 01:56:37 +02:00
while (openList.GetSize() > 0)
{
current = openList.getMin();
closedList.Insert(current);
openList.removeMin();
direction = current.getDirection();
2020-05-04 01:56:37 +02:00
if (current.getCords() == target.getCords())
break;
2020-05-04 01:56:37 +02:00
var adjacentNodes = GetAdjacentNodes(current.getCords());
foreach (var adjacentNode in adjacentNodes)
{
if (closedList.Exists(adjacentNode.getCords())) // check if adjacent node is on closed list, if it is, skip it
continue;
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection()); // calculate g - cost from start point
if (!(openList.Exists(adjacentNode.getCords()))) // if adjacent node is not on open list, add it
{
2020-05-04 01:56:37 +02:00
adjacentNode.setG(g);
adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), target.getCords()));
adjacentNode.calculateF();
adjacentNode.setParent(current);
openList.Insert(adjacentNode);
}
else
{
if (g + adjacentNode.getH() < adjacentNode.getF()) // check if adjacent node is a better path than the current one
{
adjacentNode.setG(g);
adjacentNode.calculateF();
adjacentNode.setParent(current);
}
}
2020-05-04 01:56:37 +02:00
}
}
// backtrack to create path
while (current != null)
{
path.AddNode(current);
current = current.getParent();
}
Successor function:
2020-05-04 01:58:05 +02:00
private List<Nodes> GetAdjacentNodes(Vector2 currentPos)
2020-05-04 01:56:37 +02:00
{
var adjacentNodes = new List<Nodes>()
{
new Nodes(new Vector2(currentPos.X, currentPos.Y+1), 0),
new Nodes(new Vector2(currentPos.X + 1, currentPos.Y), 1),
new Nodes(new Vector2(currentPos.X, currentPos.Y - 1), 2),
new Nodes(new Vector2(currentPos.X - 1, currentPos.Y), -1),
};
//check if out of range
for (int i = 3; i >= 0; i--)
{
if (adjacentNodes[i].getCords().X < 0 || adjacentNodes[i].getCords().Y < 0)
adjacentNodes.Remove(adjacentNodes[i]);
else
{
if (adjacentNodes[i].getCords().X > Size.X - 1 || adjacentNodes[i].getCords().Y > Size.Y - 1)
adjacentNodes.Remove(adjacentNodes[i]);
}
}
// return if not an obstacle
return adjacentNodes.Where(
item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
}
Heuristic function:
public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
{
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
}