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