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; }