/// <summary>
/// Get difference of two occupancy map
/// </summary>
/// <param name="globalMulti">ocGrid1</param>
/// <param name="globalSingle">ocGrid2</param>
/// <param name="currentPose">current position</param>
/// <param name="extentX">x-length of the comparison</param>
/// <param name="extentY">y-length of the comparison</param>
/// <returns>list of index classes - has ocGrid1 value</returns>
public UpdateMapDataMessage Diff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
{
if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
//OccupancyGrid2D globalSingle = globalOcGridByEachRobot[robotID];
//List<Index> diffIndexToSend = new List<Index>();
List<Position> diffPositionToSend = new List<Position>();
//List<float> heightList = new List<float>();
//List<float> covList = new List<float>();
//List<float> pijList = new List<float>();
List<float> pijSum = new List<float>();
List<float> puHat = new List<float>();
List<float> puHatSquare = new List<float>();
List<float> pSigUhateSquare = new List<float>();
int numCellXHalf = (int)(extentX / globalMulti.ResolutionX);
int numCellYHalf = (int)(extentY / globalMulti.ResolutionY);
int currentCellX, currentCellY;
globalMulti.GetIndicies(currentPose.x, currentPose.y, out currentCellX, out currentCellY);
int comparisonCellX, comparisonCellY;
for (int i = 0; i < numCellYHalf * 2; i++) // [i, j] = [column, row]
{
for (int j = 0; j < numCellXHalf * 2; j++)
{
comparisonCellX = currentCellX - numCellXHalf + j;
comparisonCellY = currentCellY - numCellYHalf + i;
if (globalMulti.GetCellByIdx(comparisonCellX, comparisonCellY) != globalSingle.GetCellByIdx(comparisonCellX, comparisonCellY))
{
double x, y; globalMulti.GetReals(comparisonCellX, comparisonCellY, out x, out y);
diffPositionToSend.Add(new Position((float)x, (float)y));
//heightList.Add((float)gaussianMixMapAlgorithm.UhatGM.GetCellByIdx(j, i));
//covList.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCellByIdx(j, i));
//pijList.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCellByIdx(j, i));
pijSum.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCell(x, y));
puHat.Add((float)gaussianMixMapAlgorithm.Pu_hat.GetCell(x, y));
puHatSquare.Add((float)gaussianMixMapAlgorithm.Pu_hat_square.GetCell(x, y));
pSigUhateSquare.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCell(x, y));
}
}
}
return new UpdateMapDataMessage(robotID, diffPositionToSend, pijSum, puHat, puHatSquare, pSigUhateSquare);
}
public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
{
List<Waypoint> path = new List<Waypoint>();
//added by aaron (sort of a hack)
if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
{
path.Add(new Waypoint(start.Coordinate, true, 0));
success = true;
return path;
}
int xIdx, yIdx;
success = true;
Vector2[] NESWVector = new Vector2[4];
Vector2[] diagVector = new Vector2[4];
bool[] NESW = new bool[4];
Vector2 startV = start.Coordinate; // Start Vector2
Vector2 goalV = goal.Coordinate; // Goal Vector2
PriorityQueue open = new PriorityQueue();
closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
opened = new OccupancyGrid2D(resX, resY, extentX, extentY);
GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
startV = new Vector2(xIdx, yIdx);
GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
goalV = new Vector2(xIdx, yIdx);
Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);
Node current = root;
open.Push(current);
// Do the spreading/discovering stuff until we discover a path.
while (current.xy != startV)
{
if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
{
Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
success = false;
break;
}
current = open.Pop();
NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);
diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);
for (int i = 0; i < 4; i++)
{
if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
NESW[i] = true;
if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
+ og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
}
}
}
}
for (int i = 0; i < 4; i++)
{
if (NESW[i % 4] && NESW[(i + 1) % 4])
{
if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
+ og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
}
}
}
}
}
for (int i = 0; i < 4; i++) NESW[i] = false;
closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
}
// Build a path using the discovered path.
double x, y;
Waypoint waypoint;
// First waypoint is a user waypoint
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
// Middle waypoints are path waypoints
while (current != root && current != null)
{
GetReals((int) current.xy.X, (int) current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
}
// Last waypoint is a user waypoint
if (current != null)
{
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
}
return path;
}
