public AddCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
m_pointsContainer = pointsContainer;
m_linesContainer = linesContainer;
m_observationsContainer = observationsContainer;
if (slamEvent.Points != null)
{
m_addedPoints = slamEvent.Points.Where(p => p.isNew).ToArray();
}
if (slamEvent.Lines != null)
{
m_addedLines = slamEvent.Lines.Where(l => !m_linesContainer.Exists(l)).ToArray();
for (int i = 0; i < m_addedLines.Length; ++i)
{
m_addedLines[i].isRemoved = true;
m_addedLines[i].vert1 = m_pointsContainer[m_addedLines[i].pointId1].position;
m_addedLines[i].vert2 = m_pointsContainer[m_addedLines[i].pointId2].position;
}
}
if (slamEvent.Observations != null)
{
m_addedObservations = slamEvent.Observations.Where(o => o.Point.isNew && o.Point.id != -1).ToArray();
}
}
public SlamPackage GetPackage()
{
int countOfBytes = -1;
SlamPackage receivedPackage = null;
Debug.LogFormat("[PackagesReceiver.GetPackage] Buffer size {0}", m_packagesBuffer.Count);
Debug.LogFormat("[PackagesReceiver.GetPackage] Buffer check");
if (m_packagesBuffer.Count > 0 && m_packagesBuffer.First().Key == m_packageNum)
{
receivedPackage = m_packagesBuffer[m_packageNum];
Debug.LogFormat("[PackagesReceiver.GetPackage] Buffer removing");
m_packagesBuffer.Remove(m_packageNum);
Debug.LogFormat("[PackagesReceiver.GetPackage] Buffer removed");
Debug.LogFormat("[PackagesReceiver.GetPackage] Buffer checked");
m_packageNum += 1;
return(receivedPackage);
}
Debug.LogFormat("[PackagesReceiver.GetPackage] Socket check");
if (m_network.Available > 8)
{
Debug.LogFormat("[PackagesReceiver.GetPackage] Receive");
byte[] datagram = m_network.Receive(ref endPoint);
Debug.LogFormat("[PackagesReceiver.GetPackage] Received");
uint packageNum = BitConverter.ToUInt32(datagram, 0);
countOfBytes = BitConverter.ToInt32(datagram, sizeof(int));
if (countOfBytes != datagram.Length - 2 * sizeof(int))
{
Debug.LogWarningFormat("Bad package! Read bytes count = {0}; Actual bytes count {1}", countOfBytes, datagram.Length - sizeof(int));
}
else
{
byte[] rawPackage = new byte[countOfBytes];
Array.Copy(datagram, sizeof(int) * 2, rawPackage, 0, countOfBytes);
Debug.LogFormat("[PackagesReceiver.GetPackage] Parse");
SlamPackage.Parse(rawPackage, 0, out receivedPackage);
Debug.LogFormat("[PackagesReceiver.GetPackage] Parsed");
}
if (packageNum == m_packageNum)
{
m_packageNum += 1;
}
else
{
Debug.LogFormat("[PackagesReceiver.GetPackage] Add to buffer");
Debug.LogFormat("[PackagesReceiver.GetPackage] Package Num = {0}, receivedPackage = {1}", packageNum, receivedPackage);
Debug.LogFormat("[PackagesReceiver.GetPackage] Already in list = {0}", m_packagesBuffer.ContainsKey(packageNum));
m_packagesBuffer.Add(packageNum, receivedPackage);
Debug.LogFormat("[PackagesReceiver.GetPackage] Added to buffer");
receivedPackage = null;
}
}
Debug.LogFormat("[PackagesReceiver.GetPackage] Socket checked");
// Wi-Fi pass: 5251166525 :)
return(receivedPackage);
}
public void UpdateInfo(
SlamPackage package,
ICloudObjectsContainer <SlamPoint> pointsMap,
ICloudObjectsContainer <SlamObservation> graph)
{
listBoxWithSpecializedObjects.Clear();
if (package == null)
{
commonIformation.text = "";
return;
}
commonIformation.text = package.ToString();
SlamPoint[] specialPts = package.Points.Where(p => p.id != -1).Where(p => p.message != null).ToArray();
Vector3[] ptsPositionsFromMap = specialPts.Select(p => pointsMap[p].position).ToArray();
SlamObservation[] specialObs = package.Observations
.Where(o => o.Point.id != -1)
.Where(o => o.Point.message != null)
.ToArray();
Vector3[] obsPositionsFromMap = specialObs.Select(o => graph[o].Point.position).ToArray();
for (int i = 0; i < specialPts.Length; ++i)
{
var item = listBoxWithSpecializedObjects.Add() as SpecialInfoListBoxItem;
item.SetObject(specialPts[i].id, specialPts[i].ToString(), ptsPositionsFromMap[i], specialPts[i].message);
}
for (int i = 0; i < specialObs.Length; ++i)
{
var item = listBoxWithSpecializedObjects.Add() as SpecialInfoListBoxItem;
item.SetObject(specialObs[i].Point.id, specialObs[i].ToString(), obsPositionsFromMap[i], specialObs[i].Point.message);
}
}
public RemoveCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
m_pointsContainer = pointsContainer;
m_linesContainer = linesContainer;
m_observationsContainer = observationsContainer;
if (slamEvent.Points != null)
{
m_points2Remove = slamEvent.Points
.Where(p => p.id != -1)
.Where(p => p.isRemoved)
.Select(p => m_pointsContainer[p])
.ToArray();
}
if (slamEvent.Lines != null)
{
m_lines2Remove = slamEvent.Lines
.Where(l => l.isRemoved)
.Select(l => m_linesContainer[l])
.ToArray();
}
if (slamEvent.Observations != null)
{
m_observations2Remove = slamEvent.Observations
.Where(o => o.Point.id != -1)
.Where(o => o.Point.isRemoved)
.Select(o => m_observationsContainer[o])
.ToArray();
}
}
private void PostProcessMaps(SlamPackage pkg)
{
if (pkg.Points != null)
{
SlamPoint[] updatedPoints = pkg.Points
.AsParallel()
.Where(p => p.id != -1)
.Where(p => !p.isRemoved)
.Select(p => { var mp = map.PointsContainer[p]; mp.color = mp.defaultColor; return(mp); })
.ToArray();
lock (map.PointsContainer)
UpdateMap(
updatedPoints,
/*isNew*/ _ => false, /*isRemoved*/ _ => false, /*justColored*/ _ => true, /*isValid*/ p => p.id != -1,
map.PointsContainer);
}
if (pkg.Lines != null)
{
lock (map.LinesContainer)
UpdateMap(
pkg.Lines,
/*isNew*/ _ => false, /*isRemoved*/ _ => true, /*justColored*/ _ => false, /*isValid*/ _ => true,
map.LinesContainer);
}
}
public override void Present(IPackage package)
{
if (package.Type != PackageType.SLAMPackage)
{
m_presenter?.Present(package);
return;
}
SlamPackage slamPkg = package as SlamPackage;
UpdateMaps(slamPkg);
PostProcessMaps(slamPkg);
}
public override int Parse(byte[] data, int startIdx, out IPackage result)
{
result = null;
if ((PackageType)data[startIdx] != PackageType.SLAMPackage)
{
return(m_successor?.Parse(data, startIdx, out result) ?? 0);
}
int readBytes = SlamPackage.Parse(data, startIdx, out SlamPackage slamPkg);
Convert(ref slamPkg);
result = slamPkg;
return(readBytes);
}
public override void Present(IPackage package)
{
if (package.Type != PackageType.SLAMPackage)
{
m_presenter?.SetSuccessor(m_presenter);
return;
}
if (!package.IsKey)
{
return;
}
m_pkg2Present = package as SlamPackage;
}
private void UpdateMaps(SlamPackage pkg)
{
lock (map.PointsContainer)
UpdateMap(pkg.Points, p => p.isNew, p => p.isRemoved, p => p.justColored, p => p.id != -1, map.PointsContainer);
lock (map.LinesContainer)
UpdateMap(
pkg.Lines,
/*isNew*/ _ => true, /*isRemoved*/ _ => false, /*justColored*/ _ => false, /*isValid*/ _ => true,
map.LinesContainer);
lock (map.ObservationsContainer)
UpdateMap(
pkg.Observations,
o => o.Point.isNew, o => o.Point.isRemoved, o => o.Point.justColored, o => o.Point.id != -1,
map.ObservationsContainer);
}
public static void TestExistent(
this SlamPackage package,
Func <SlamPoint, bool> filter,
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph)
{
if (package.Points != null)
{
var existentPoints = SlamObjectQuery(package.Points.Where(p => filter(p)), id => pointsContainer.Exists(id));
Test(package, existentPoints, "Point id {0} already exists. New: '{1}'. Removed: '{2}'");
}
if (package.Observations != null)
{
var existentObs = SlamObjectQuery(package.Observations.Select(o => (SlamPoint)o).Where(o => filter(o)), id => graph.Exists(id));
Test(package, existentObs, "Observation id {0} already exists. New: '{1}'. Removed: '{2}'");
}
}
public static void Test(
SlamPackage package,
IEnumerable <SlamPoint> objects,
string messageTemplate)
{
var objArray = objects.ToArray();
if (objArray.Length != 0)
{
ElektronikLogger.Log(package.ToString(), "", LogType.Error);
foreach (var obj in objArray)
{
string msg = String.Format(messageTemplate, obj.id, obj.isNew, obj.isRemoved);
ElektronikLogger.Log(msg, "", LogType.Error);
}
}
}
public PostProcessingCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
IEnumerable <SlamPoint> points = null;
if (slamEvent.Points != null)
{
points = slamEvent.Points
.Where(p => p.id != -1)
.Select(p => pointsContainer[p])
.Select(p => { p.color = p.defaultColor; return(p); });
m_commands.Add(new UpdateCommand(pointsContainer, observationsContainer, points, null));
m_commands.Add(new RemoveCommand(pointsContainer, linesContainer, observationsContainer, slamEvent));
}
}
public static void TestNonExistent(
this SlamPackage package,
Func <SlamPoint, bool> filter,
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph)
{
#if DEBUG
if (package.Points != null)
{
var existentPoints = SlamObjectQuery(package.Points.Where(p => filter(p)), id => !pointsContainer.Exists(id));
ElektronikLogger.WrapDebug(() => Test(package, existentPoints, "Point id {0} doesn't exists. New: '{1}'. Removed: '{2}'"));
}
if (package.Observations != null)
{
var existentObs = SlamObjectQuery(package.Observations.Select(o => (SlamPoint)o).Where(o => filter(o)), id => !graph.Exists(id));
ElektronikLogger.WrapDebug(() => Test(package, existentObs, "Observation id {0} doesn't exists. New: '{1}'. Removed: '{2}'"));
}
#endif
}
private void Convert(ref SlamPackage pkg)
{
List <SlamObservation> observations = pkg.Observations;
Parallel.For(0, pkg.Observations.Count, (i) =>
{
SlamPoint point = observations[i];
Quaternion rot = observations[i].Orientation;
m_converter.Convert(ref point.position, ref rot);
observations[i].Point = point;
observations[i].Orientation = rot;
});
List <SlamPoint> points = pkg.Points;
Parallel.For(0, pkg.Points.Count, (i) =>
{
SlamPoint pt = points[i];
Quaternion stub = Quaternion.identity;
m_converter.Convert(ref pt.position, ref stub);
points[i] = pt;
});
}
public UpdateCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph,
SlamPackage slamEvent) : this(pointsContainer, graph, slamEvent.Points, slamEvent.Observations)
{
}
public void Convert(ref SlamPackage srcEvent)
{
throw new NotImplementedException();
}
