private PacketPb BuildPacket(Vector3Pb[] cameraPoses, ColorPb[] cameraColors, Quaternion[] cameraOrientations,
int[][] points, int pointsAmount, int firstId)
{
var obsPacket = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Add,
Observations = new PacketPb.Types.Observations(),
};
for (int i = 0; i < cameraPoses.Length; i++)
{
obsPacket.Observations.Data.Add(new ObservationPb
{
Point = new PointPb { Id = -i - firstId, Position = cameraPoses[i], Color = cameraColors[i] },
Orientation = new Vector4Pb
{
X = cameraOrientations[i].X,
Y = cameraOrientations[i].Y,
Z = cameraOrientations[i].Z,
W = cameraOrientations[i].W,
},
ObservedPoints = { points[i].SelectMany(p => Enumerable.Range(p * pointsAmount, pointsAmount)) },
});
}
return obsPacket;
}
public static IEnumerable <SlamObservationDiff> ExtractObservations(this PacketPb packet,
ICSConverter converter,
string imageDir)
{
foreach (var o in packet.Observations.Data)
{
SlamObservationDiff observation = o;
if (observation.Point.Position.HasValue)
{
observation.Point.Position = converter?.Convert(observation.Point.Position.Value);
}
if (observation.Rotation.HasValue)
{
observation.Rotation = converter?.Convert(observation.Rotation.Value);
}
if (!string.IsNullOrEmpty(observation.FileName) &&
!Path.IsPathRooted(observation.FileName))
{
observation.FileName = Path.Combine(imageDir, observation.FileName);
}
yield return(observation);
}
}
public void Present(PacketPb data, ICSConverter converter)
{
MainThreadInvoker.Enqueue(() => _info.Clear());
IEnumerable <ICloudItem> objects = Pkg2Pts(data, converter).ToArray();
MainThreadInvoker.Enqueue(() => _info.Render((data.Message, objects)));
}
public override Task <ErrorStatusPb> Handle(PacketPb request, ServerCallContext context)
{
if (request.DataCase != PacketPb.DataOneofCase.Lines)
{
return(base.Handle(request, context));
}
Timer = Stopwatch.StartNew();
return(Handle(request.Action, request.ExtractLines(_converter).ToList()));
}
public static Frame ParsePacket(PacketPb packetPb, DataParser <PacketPb> parsersChain)
{
return(new Frame
{
Packet = packetPb,
Timestamp = packetPb.Timestamp,
IsSpecial = packetPb.Special,
Command = parsersChain.GetCommand(packetPb),
});
}
protected void SendAndCheck(PacketPb packet, string?filename = null, bool isFirst = false)
{
if (filename != null)
{
using var file = File.Open(filename, isFirst ? FileMode.Create : FileMode.Append);
packet.WriteDelimitedTo(file);
}
var response = MapClient.Handle(packet);
Assert.True(response.ErrType == ErrorStatusPb.Types.ErrorStatusEnum.Succeeded, response.Message);
}
public override Task <ErrorStatusPb> Handle(PacketPb request, ServerCallContext context)
{
if (request.DataCase != PacketPb.DataOneofCase.Observations)
{
return(base.Handle(request, context));
}
Timer = Stopwatch.StartNew();
var obs = request.ExtractObservations(_converter, Directory.GetCurrentDirectory()).ToList();
return(HandleConnections(request, Handle(request.Action, obs)));
}
public void Clear()
{
var packet = new PacketPb()
{
Special = true,
Action = PacketPb.Types.ActionType.Clear,
Points = new PacketPb.Types.Points(),
};
SendAndCheck(packet, Filename);
}
public void Clear()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Clear,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
SendAndCheck(packet, Filename);
}
protected PacketPb CreateRemovedPacket <TCloudItem>(IList <int> args, int timestamp)
where TCloudItem : struct, ICloudItem
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Remove,
Timestamp = timestamp,
};
SetData <TCloudItem>(args, packet);
return(packet);
}
public static IEnumerable <SlamPointDiff> ExtractPoints(this PacketPb packet, ICSConverter converter = null)
{
foreach (var p in packet.Points.Data)
{
SlamPointDiff point = p;
if (point.Position.HasValue)
{
point.Position = converter?.Convert(point.Position.Value);
}
yield return(point);
}
}
protected PacketPb CreateAddedPacket <TCloudItem>(IList <TCloudItem> args, int timestamp, ICSConverter converter)
where TCloudItem : struct, ICloudItem
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Add,
Timestamp = timestamp,
};
SetData(args, packet, converter);
return(packet);
}
public void Remove()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Remove,
Points = new PacketPb.Types.Points(),
};
packet.Points.Data.Add(new[] { _map[1], _map[3] });
SendAndCheck(packet, Filename);
}
public void Create()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Add,
Points = new PacketPb.Types.Points(),
};
packet.Points.Data.Add(_map);
SendAndCheck(packet, Filename);
}
public void Create()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Add,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
packet.Observations.Data.Add(_map);
SendAndCheck(packet, Filename, true);
}
public void Remove()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Remove,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
packet.Observations.Data.Add(new[] { _map[1], _map[3] });
SendAndCheck(packet, Filename);
}
public void Create()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Add,
Points = new PacketPb.Types.Points(),
Special = true,
};
packet.Points.Data.Add(_map);
using var file = File.Open(Filename, FileMode.Create);
packet.WriteDelimitedTo(file);
}
public void SendInfoPackage()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Info,
Points = new PacketPb.Types.Points(),
Message = "This is package with info message",
Special = true,
};
packet.Points.Data.Add(_map);
using var file = File.Open(Filename, FileMode.Append);
packet.WriteDelimitedTo(file);
}
public void RemoveConnections()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Update,
Points = new PacketPb.Types.Points(),
Connections = new PacketPb.Types.Connections
{
Action = PacketPb.Types.Connections.Types.Action.Remove,
},
};
SendAndCheck(packet, Filename);
}
public void Clear()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Clear,
Points = new PacketPb.Types.Points(),
};
var t = new Stopwatch();
t.Start();
SendAndCheck(packet, Filename);
t.Stop();
TestContext.WriteLine($"Handle packet: {t.ElapsedMilliseconds} ms");
}
public void UpdatePositions()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Update,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 0, Position = new Vector3Pb {
Z = 0.5
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 1, Position = new Vector3Pb {
Z = 1
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 2, Position = new Vector3Pb {
Z = 1.5
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 3, Position = new Vector3Pb {
Z = 2
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 4, Position = new Vector3Pb {
Z = 2.5
}
}
});
SendAndCheck(packet, Filename);
}
public void UpdateColors()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Update,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 0, Color = new ColorPb {
R = 255, G = 255, B = 255
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 1, Color = new ColorPb {
R = 255, G = 255, B = 255
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 2, Color = new ColorPb {
R = 255, G = 255, B = 255
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 3, Color = new ColorPb {
R = 255, G = 255, B = 255
}
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 4, Color = new ColorPb {
R = 255, G = 255, B = 255
}
}
});
SendAndCheck(packet, Filename);
}
public void UpdateOrientations()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Update,
Observations = new PacketPb.Types.Observations(),
Timestamp = ++_timestamp,
};
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 0
}, Orientation = new Vector4Pb {
W = 1, X = 1
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 1
}, Orientation = new Vector4Pb {
W = 1, X = 1
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 2
}, Orientation = new Vector4Pb {
W = 1, X = 1
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 3
}, Orientation = new Vector4Pb {
W = 1, X = 1
}
});
packet.Observations.Data.Add(new ObservationPb {
Point = new PointPb {
Id = 4
}, Orientation = new Vector4Pb {
W = 1, X = 1
}
});
SendAndCheck(packet, Filename);
}
public static IEnumerable <SlamInfinitePlaneDiff> ExtractInfinitePlanes(this PacketPb packet,
ICSConverter converter = null)
{
foreach (var p in packet.InfinitePlanes.Data)
{
SlamInfinitePlaneDiff plane = p;
if (plane.Offset.HasValue)
{
plane.Offset = converter?.Convert(plane.Offset.Value);
}
if (plane.Normal.HasValue)
{
plane.Normal = converter?.Convert(plane.Normal.Value);
}
yield return(plane);
}
}
public void UpdateConnections()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Update,
Observations = new PacketPb.Types.Observations(),
Connections = new PacketPb.Types.Connections
{
Action = PacketPb.Types.Connections.Types.Action.Add,
},
Timestamp = ++_timestamp,
};
packet.Connections.Data.Add(_connections);
SendAndCheck(packet, Filename);
}
public void RemoveConnections()
{
var packet = new PacketPb
{
Action = PacketPb.Types.ActionType.Update,
Observations = new PacketPb.Types.Observations(),
Connections = new PacketPb.Types.Connections
{
Action = PacketPb.Types.Connections.Types.Action.Remove,
},
Timestamp = ++_timestamp,
};
packet.Connections.Data.Add(new[] { _connections[0], _connections[1] });
SendAndCheck(packet, Filename);
}
public void Update()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Update,
Points = new PacketPb.Types.Points(),
};
packet.Points.Data.Add(GeneratePoints(20000));
var t = new Stopwatch();
t.Start();
SendAndCheck(packet, Filename);
t.Stop();
}
/// <summary> Handles gRPC request. </summary>
/// <param name="request"> Packet to handle. </param>
/// <param name="context"> Server call context </param>
/// <returns> Async error status </returns>
public override Task <ErrorStatusPb> Handle(PacketPb request, ServerCallContext context)
{
Task <ErrorStatusPb> status;
if (_link != null)
{
status = _link.Handle(request, context);
}
else
{
status = Task.FromResult(new ErrorStatusPb()
{
ErrType = ErrorStatusPb.Types.ErrorStatusEnum.Unknown,
Message = "Valid MapManager not found for this message"
});
}
return(status);
}
public static IEnumerable <SlamLineDiff> ExtractLines(this PacketPb packet, ICSConverter converter = null)
{
foreach (var l in packet.Lines.Data)
{
SlamLineDiff line = l;
if (line.Point1.Position.HasValue)
{
line.Point1.Position = converter?.Convert(line.Point1.Position.Value);
}
if (line.Point2.Position.HasValue)
{
line.Point2.Position = converter?.Convert(line.Point2.Position.Value);
}
yield return(line);
}
}
public void Create()
{
var packet = new PacketPb
{
Special = true,
Action = PacketPb.Types.ActionType.Add,
Points = new PacketPb.Types.Points(),
};
packet.Points.Data.Add(GeneratePoints(20000));
var t = new Stopwatch();
t.Start();
SendAndCheck(packet, Filename, true);
t.Stop();
TestContext.WriteLine($"Handle packet: {t.ElapsedMilliseconds} ms");
}
