public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
{
OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);
if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
{
throw new Exception("OccupancyGrid size does not match");
}
else
{
for (int i = 0; i < a.Width; i++)
{
for (int j = 0; j < a.Height; j++)
{
toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
}
}
}
return(toReturn);
}
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;
}
/// <summary>
/// Return only height map difference
/// </summary>
/// <param name="robotID">Robot ID</param>
/// <param name="globalMulti">global Map to compare</param>
/// <param name="currentPose">robot's current position</param>
/// <param name="extentX">extent x</param>
/// <param name="extentY">extent y</param>
/// <returns>UpdateMapDataMessage with only height information</returns>
//public UpdateMapDataMessage HeightDiff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
//{
// if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
// OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
// //List<Index> diffIndexToSend = new List<Index>();
// List<Position> diffPositionToSend = new List<Position>();
// List<float> heightList = 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 (comparisonCellX < 0 || comparisonCellX > globalSingle.NumCellX || comparisonCellY < 0 || comparisonCellY > globalSingle.NumCellY)
// continue;
// 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(comparisonCellX, comparisonCellY));
// }
// }
// }
// return new UpdateMapDataMessage(robotID, diffPositionToSend, heightList);
//}
/// <summary>
/// Converts GlobalMapCell data into a occupancy grid for global map
/// </summary>
/// <param name="globalHeightMap">returned global height map</param>
/// <param name="globalSigMap">returned global covariance map</param>
/// <param name="globalPijMap">returned laser hitting (kinda) map</param>
/// <param name="globalMapDataList">incoming message from network</param>
public static void ConvertToOcGrid(ref OccupancyGrid2D globalHeightMap, ref OccupancyGrid2D globalSigMap, ref OccupancyGrid2D globalPijMap, ref OccupancyGrid2D laserHitMap, List<GlobalMapCell.GlobalMapCellData> globalMapDataList)
{
foreach (GlobalMapCell.GlobalMapCellData cell in globalMapDataList)
{
if (cell.Equals(globalMapDataList[0]))
continue;
int col = cell.colIdx; int row = cell.rowIdx;
globalHeightMap.SetCellByIdx(col, row, cell.largeU);
globalSigMap.SetCellByIdx(col, row, cell.largeSig);
globalPijMap.SetCellByIdx(col, row, cell.pij);
laserHitMap.SetCellByIdx(col, row, cell.laserHit);
}
}
public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
{
OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);
if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
{
throw new Exception("OccupancyGrid size does not match");
}
else
{
for (int i = 0; i < a.Width; i++)
{
for (int j = 0; j < a.Height; j++)
{
toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
}
}
}
return toReturn;
}
