void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e) { if (e.message == null) { return; } UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, gaussianMixMapAlgorithm.UhatGM, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); if (mapToSend != null) { int segmentLength = 2000; int numIteration = mapToSend.CellData.Count / segmentLength; int numLeftOver = mapToSend.CellData.Count - (numIteration * segmentLength); int lastIndex = 0; for (int i = 0; i < numIteration + 1; i++) { if (i == numIteration) { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, numLeftOver))); } else { localMapResponseServer.SendUnreliably(new UpdateMapDataMessage(mapToSend.CellData.GetRange(lastIndex, segmentLength))); lastIndex += segmentLength; } } } }
void localMapRequestClient_MsgReceived(object sender, MsgReceivedEventArgs <LocalMapRequestMessage> e) { if (e.message == null) { return; } UpdateMapDataMessage mapToSend = Diff(e.message.RobotID, globalOcGrid, e.message.CurrentPose, e.message.ExtentX, e.message.ExtentY); localMapReponseServer.SendUnreliably(mapToSend); }
//void localMapUpdateClient_MsgReceived(object sender, MsgReceivedEventArgs<UpdateMapDataMessage> e) //{ // lock (locker) // { // //UpdateCurrentLocalMap(ref heightMap, ref covMap, ref pijMap, e.message); // UpdateCurrentLocalMap(ref heightMap, e.message); // } //} void UpdateCurrentLocalMap(ref OccupancyGrid2D currentLocalHeightMap, ref OccupancyGrid2D currentLocalCovMap, ref OccupancyGrid2D currentLocalPijMap, UpdateMapDataMessage updateMessage) { try { //check if the size matches foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData) { if (cell.X == cell.Y && cell.Y == 0) continue; // the first packet double x = cell.X; double y = cell.Y; //currentLocalHeightMap.SetCell(x, y, cell.Height); //currentLocalCovMap.SetCell(x, y, cell.Cov); //currentLocalPijMap.SetCell(x, y, cell.Pij); } } catch { throw new IndexOutOfRangeException("The update message and current map are not in the same size"); } }
public void UpdateCurrentLocalMap(ref GaussianMixtureMapping.GaussianMixMappingQ gaussMapper, UpdateMapDataMessage updateMessage) { try { //check if the size matches foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData) { double x = cell.X; double y = cell.Y; gaussMapper.SetPijSum(x, y, cell.PijSum); gaussMapper.SetPuHat(x, y, cell.PuHat); gaussMapper.SetPuHatSquare(x, y, cell.PuHatSquare); gaussMapper.SetPsigUhatSquare(x, y, cell.PSigUhatSquare); gaussMapper.UpdateHeight(x, y); } } catch { throw new IndexOutOfRangeException("The update message and current map are not in the same size"); } }
//void localMapUpdateClient_MsgReceived(object sender, MsgReceivedEventArgs<UpdateMapDataMessage> e) //{ // lock (locker) // { // //UpdateCurrentLocalMap(ref heightMap, ref covMap, ref pijMap, e.message); // UpdateCurrentLocalMap(ref heightMap, e.message); // } //} void UpdateCurrentLocalMap(ref OccupancyGrid2D currentLocalHeightMap, ref OccupancyGrid2D currentLocalCovMap, ref OccupancyGrid2D currentLocalPijMap, UpdateMapDataMessage updateMessage) { try { //check if the size matches foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData) { if (cell.X == cell.Y && cell.Y == 0) { continue; // the first packet } double x = cell.X; double y = cell.Y; //currentLocalHeightMap.SetCell(x, y, cell.Height); //currentLocalCovMap.SetCell(x, y, cell.Cov); //currentLocalPijMap.SetCell(x, y, cell.Pij); } } catch { throw new IndexOutOfRangeException("The update message and current map are not in the same size"); } }
public void UpdateCurrentLocalMap(ref GaussianMixtureMapping.GaussianMixMappingQ gaussMapper, UpdateMapDataMessage updateMessage) { try { //check if the size matches foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData) { double x = cell.X; double y = cell.Y; gaussMapper.SetPijSum(x, y, cell.PijSum); gaussMapper.SetPuHat(x, y, cell.PuHat); gaussMapper.SetPuHatSquare(x, y, cell.PuHatSquare); gaussMapper.SetPsigUhatSquare(x, y, cell.PSigUhatSquare); gaussMapper.UpdateHeight(x, y); } } catch { throw new IndexOutOfRangeException("The update message and current map are not in the same size"); } }