private void ReadCallback(IAsyncResult ar)
{
try
{
ClientPlayer player = (ClientPlayer)ar.AsyncState;
Socket client = player.socket;
// Read data from the remote device.
int bytesRead = client.EndReceive(ar);
if (bytesRead > 0)
{
var currentBytes = new byte[bytesRead];
Buffer.BlockCopy(player.buffer, 0, currentBytes, 0, bytesRead);
player.accumulatedBytes.AddRange(currentBytes);
if (player.accumulatedBytes.Count >= Packet.packetHeaderSize)
{
//If we're not at the start of a packet, increment our position until we are, or we run out of bytes
var accumulatedBytes = player.accumulatedBytes.ToArray();
while (accumulatedBytes.Length >= Packet.packetHeaderSize && !Packet.StreamIsAtPacket(accumulatedBytes))
{
player.accumulatedBytes.RemoveAt(0);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
while (accumulatedBytes.Length >= Packet.packetHeaderSize && Packet.PotentiallyValidPacket(accumulatedBytes))
{
Packet readPacket = null;
try
{
readPacket = Packet.FromBytes(accumulatedBytes);
PacketRecieved?.Invoke(readPacket);
}
catch (Exception) { }
//Remove the bytes which we've already used from the accumulated List
//If the packet failed to parse, skip the header so that the rest of the packet is consumed by the above vailidity check on the next run
player.accumulatedBytes.RemoveRange(0, readPacket?.Size ?? Packet.packetHeaderSize);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
}
// Get the rest of the data.
client.BeginReceive(player.buffer, 0, ClientPlayer.BufferSize, 0, new AsyncCallback(ReadCallback), player);
}
}
catch (Exception e)
{
Logger.Debug(e.ToString());
ServerDisconnected_Internal();
}
}
private void SlipRecvMessage(byte[] data, int len)
{
Log.Debug("Slip Recv:" + BitConverter.ToString(data, 0, len));
if (len < 7)
{
Log.Error("Packet too small");
return;
}
if (!CheckCrc(data, len - 2))
{
Log.Warning("Wrong CRC");
return;
}
var command = (Commands)data[0];
byte?flags = null;
if (command == Commands.DEVICE_STATE || command == Commands.DEVICE_STATE_CHANGED)
{
flags = data[5];
}
if (command == Commands.APS_DATA_INDICATION ||
command == Commands.APS_DATA_REQUEST ||
command == Commands.APS_DATA_CONFIRM)
{
flags = data[7];
}
if (flags.HasValue)
{
NetworkState = (NetworkStates)(flags & 0x3);
ApsConfirmFlag = (flags & 0x04) > 0;
ApsIndicationFlag = (flags & 0x08) > 0;
ConfigurationFlag = (flags & 0x10) > 0;
ApsRequestFreeSlotsFlag = (flags & 0x20) > 0;
}
if (Commands.DEVICE_STATE_CHANGED != command)
{
if (responses.TryRemove(data[1], out var taskSource))
{
var status = (StatusCodes)data[2];
if (status == StatusCodes.SUCCESS)
{
taskSource.SetResult(data.Take(len).ToArray());
}
else
{
taskSource.SetException(new Exception(status.ToString()));
}
}
}
PacketRecieved?.Invoke();
}
private void ReadCallback(IAsyncResult ar)
{
try
{
ClientPlayer player = (ClientPlayer)ar.AsyncState;
Socket client = player.workSocket;
// Read data from the remote device.
int bytesRead = client.EndReceive(ar);
if (bytesRead > 0)
{
var currentBytes = new byte[bytesRead];
Buffer.BlockCopy(player.buffer, 0, currentBytes, 0, bytesRead);
player.accumulatedBytes.AddRange(currentBytes);
if (player.accumulatedBytes.Count >= Packet.packetHeaderSize)
{
//If we're not at the start of a packet, increment our position until we are, or we run out of bytes
var accumulatedBytes = player.accumulatedBytes.ToArray();
while (!Packet.StreamIsAtPacket(accumulatedBytes) && accumulatedBytes.Length >= Packet.packetHeaderSize)
{
player.accumulatedBytes.RemoveAt(0);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
if (Packet.PotentiallyValidPacket(accumulatedBytes))
{
PacketRecieved?.Invoke(Packet.FromBytes(accumulatedBytes));
player.accumulatedBytes.Clear();
}
}
// Get the rest of the data.
client.BeginReceive(player.buffer, 0, ClientPlayer.BufferSize, 0, new AsyncCallback(ReadCallback), player);
}
}
catch (Exception e)
{
Logger.Debug(e.ToString());
ServerDisconnected_Internal();
}
}
public void ReadCallback(IAsyncResult ar)
{
try
{
NetworkPlayer player = (NetworkPlayer)ar.AsyncState;
Socket handler = player.workSocket;
// Read data from the client socket.
int bytesRead = handler.EndReceive(ar);
if (bytesRead > 0)
{
var currentBytes = new byte[bytesRead];
Buffer.BlockCopy(player.buffer, 0, currentBytes, 0, bytesRead);
player.accumulatedBytes.AddRange(currentBytes);
if (player.accumulatedBytes.Count >= Packet.packetHeaderSize)
{
//If we're not at the start of a packet, increment our position until we are, or we run out of bytes
var accumulatedBytes = player.accumulatedBytes.ToArray();
while (!Packet.StreamIsAtPacket(accumulatedBytes) && accumulatedBytes.Length >= Packet.packetHeaderSize)
{
player.accumulatedBytes.RemoveAt(0);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
if (Packet.PotentiallyValidPacket(accumulatedBytes))
{
PacketRecieved?.Invoke(player, Packet.FromBytes(accumulatedBytes));
player.accumulatedBytes.Clear();
}
}
// Not all data received. Get more.
handler.BeginReceive(player.buffer, 0, NetworkPlayer.BufferSize, 0, new AsyncCallback(ReadCallback), player);
}
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
}
private void Recieve()
{
while (recieve)
{
try
{
NetworkStream stream = tcp.GetStream();
byte[] length = new byte[4];
stream.Read(length, 0, 4);
int size = BitConverter.ToInt32(length, 0);
byte[] data = new byte[size];
stream.Read(data, 0, size);
PacketRecieved?.Invoke(this, new Packet(data));
}
catch (Exception)
{
break;
}
}
}
/// <summary>
/// This method won't return untill listener is done (canceled manually, or error).
/// It returns the raw data packets sent by the VLP-16
/// Throws a socket exception only on initialization. Once everything is up and running exceptions are handled internally.
/// </summary>
/// <param name="endpoint">Where to listen for data</param>
/// <param name="packet_recieved_callback">New data gets parsed, sent to this method.</param>
/// <param name="should_cancel_callback">Hearbeat function, called at 1 Hz, can stop the velodyne listener</param>
public static void Listen(
IPEndPoint endpoint,
PacketRecieved packet_recieved_callback = null,
ShouldCancel should_cancel_callback = null)
{
UdpClient cl = null;
try
{
cl = new UdpClient(new IPEndPoint(IPAddress.Any, endpoint.Port));
cl.Client.ReceiveTimeout = 250; // .25 Seconds
cl.Client.ReceiveBufferSize = 4096;
Listen_(cl, packet_recieved_callback, should_cancel_callback); // This will only end when canceled
}
finally
{
if (cl != null)
{
cl.Dispose();
}
}
}
/// <summary>
/// This constructor won't return untill listener is done (or error).
/// It returns the raw data packets sent by the VLP-16
/// Throws a socket exception only on initialization. Once everything is up and running exceptions are handled internally.
/// </summary>
/// <param name="d"></param>
/// <param name="packet_recieved_sync"></param>
/// <param name="should_cancel_async">Called at 1 Hz</param>
public VLP_16(
IPEndPoint d,
PacketRecieved packet_recieved_sync = null,
ShouldCancel should_cancel_async = null)
{
UdpClient cl = null;
try
{
cl = new UdpClient(new IPEndPoint(d.Address, d.Port));
cl.Client.ReceiveTimeout = 250; // .25 Seconds
cl.Client.ReceiveBufferSize = 4096;
this.Listen(cl, packet_recieved_sync, should_cancel_async); // This will only end when canceled
}
finally
{
if (cl != null)
{
cl.Dispose();
}
}
}
/// <summary>
/// Starts listening thread, raising PacketRecieved events whenever a new valid Packet comes in
/// </summary>
public void StartListen()
{
if (isListening)
{
return;
}
isListening = true;
Task.Run(() =>
{
while (true)
{
IPEndPoint sender = new IPEndPoint(IPAddress.Any, 0);
byte[] recieved;
try
{
recieved = Receive(ref sender); // blocks, recieves bytes and fills sender object with sender of packet
}
catch (SocketException e)
{
continue; // this shouldnt f*****g happen but sometimes it does :)
}
Packet p;
try
{
p = Packet.FromBytes(recieved);
}
catch (Exception e)
{
Console.WriteLine("Recieved an invalid packet");
continue;
}
p.Sender = sender;
PacketRecieved?.Invoke(this, p);
}
}, cts.Token);
}
public void ReadCallback(IAsyncResult ar)
{
ConnectedClient player = (ConnectedClient)ar.AsyncState;
try
{
Socket handler = player.workSocket;
// Read data from the client socket.
int bytesRead = handler.EndReceive(ar);
//Logger.Debug($"READ {bytesRead} BYTES");
if (bytesRead > 0)
{
var currentBytes = new byte[bytesRead];
Buffer.BlockCopy(player.buffer, 0, currentBytes, 0, bytesRead);
player.accumulatedBytes.AddRange(currentBytes);
if (player.accumulatedBytes.Count >= Packet.packetHeaderSize)
{
//If we're not at the start of a packet, increment our position until we are, or we run out of bytes
var accumulatedBytes = player.accumulatedBytes.ToArray();
while (!Packet.StreamIsAtPacket(accumulatedBytes) && accumulatedBytes.Length >= Packet.packetHeaderSize)
{
player.accumulatedBytes.RemoveAt(0);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
while (accumulatedBytes.Length >= Packet.packetHeaderSize && Packet.PotentiallyValidPacket(accumulatedBytes))
{
Packet readPacket = null;
try
{
readPacket = Packet.FromBytes(accumulatedBytes);
PacketRecieved?.Invoke(player, readPacket);
}
catch (Exception) { }
//Remove the bytes which we've already used from the accumulated List
//If the packet failed to parse, skip the header so that the rest of the packet is consumed by the above vailidity check on the next run
player.accumulatedBytes.RemoveRange(0, readPacket?.Size ?? Packet.packetHeaderSize);
accumulatedBytes = player.accumulatedBytes.ToArray();
}
}
// Not all data received. Get more.
handler.BeginReceive(player.buffer, 0, ConnectedClient.BufferSize, 0, new AsyncCallback(ReadCallback), player);
}
else
{
//Reading zero bytes is a sign of disconnect
ClientDisconnected_Internal(player);
}
}
catch (Exception e)
{
Logger.Debug(e.ToString());
ClientDisconnected_Internal(player);
}
}
private static unsafe void Listen_(
UdpClient cl,
PacketRecieved packet_recieved_callback,
ShouldCancel should_cancel_callback)
{
DateTime start = DateTime.Now;
int elapsed_seconds = 0;
int corrects = 0;
int incorrects = 0;
int timeouts = 0;
int socketerrors = 0;
while (true)
{
try
{
IPEndPoint iep = null;
var data = cl.Receive(ref iep);
if (data.Length == Packet.Raw._Size)
{
Packet p;
bool valid;
fixed(byte *b = data)
p = new Packet(b, out valid);
if (valid)
{
corrects++;
packet_recieved_callback?.Invoke(p, iep);
}
else
{
incorrects--;
}
}
else
{
incorrects++;
}
}
catch (TimeoutException)
{
timeouts++;
}
catch (SocketException)
{
socketerrors++;
}
catch (Exception e)
{
Logger.WriteException(typeof(VLP_16), "Listen", e);
incorrects++;
}
var now = DateTime.Now;
if ((now - start).TotalSeconds > elapsed_seconds)
{
elapsed_seconds++;
var st = new UpdateArgs();
st.Timeouts = timeouts;
st.PacketsReceivedCorrectly = corrects;
st.PacketsReceivedIncorrectly = incorrects;
st.SocketErrors = socketerrors;
timeouts = 0;
corrects = 0;
incorrects = 0;
socketerrors = 0;
if (should_cancel_callback != null)
{
if (should_cancel_callback(st))
{
return;
}
}
}
}
}
private unsafe void Listen(
UdpClient cl,
PacketRecieved packet_recieved_sync,
ShouldCancel should_cancel_async)
{
DateTime start = DateTime.Now;
int elapsed_seconds = 0;
int corrects = 0;
int incorrects = 0;
int timeouts = 0;
int socketerrors = 0;
while (true)
{
try
{
IPEndPoint iep = null;
var data = cl.Receive(ref iep);
if (data.Length == Packet.Raw._Size)
{
Packet p;
bool valid;
fixed(byte *b = data)
p = new Packet(b, out valid);
if (valid)
{
corrects++;
if (packet_recieved_sync != null)
{
packet_recieved_sync(p, iep);
}
}
else
{
incorrects--;
}
}
else
{
incorrects++;
}
}
catch (TimeoutException)
{
timeouts++;
}
catch (SocketException)
{
socketerrors++;
}
catch (Exception e)
{
System.Diagnostics.Debug.WriteLine(this.GetType().FullName + ": " + e.ToString());
incorrects++;
}
var now = DateTime.Now;
if ((now - start).TotalSeconds > elapsed_seconds)
{
elapsed_seconds++;
var st = new UpdateArgs();
st.SocketErrors = timeouts;
st.PacketsReceivedCorrectly = corrects;
st.PacketsReceivedIncorrectly = incorrects;
st.SocketErrors = socketerrors;
timeouts = 0;
corrects = 0;
incorrects = 0;
socketerrors = 0;
if (should_cancel_async != null)
{
if (should_cancel_async(st))
{
return;
}
}
}
}
}
private void PacketRecievedRaise(Packet msg)
{
PacketRecieved?.Invoke(msg);
}
/// <summary>
/// Raised only in RecievePackets() method
/// </summary>
/// <param name="packet"></param>
private void PacketRecievedRaise(Packet packet)
{
PacketRecieved?.Invoke(packet);
}
