public Command HandleFrame(Frame f)
{
switch (m_state)
{
case AssemblyState.ExpectingMethod:
{
if (f.Type != Constants.FrameMethod)
{
throw new UnexpectedFrameException(f);
}
m_command.Method = m_protocol.DecodeMethodFrom(f.GetReader());
m_state = m_command.Method.HasContent
? AssemblyState.ExpectingContentHeader
: AssemblyState.Complete;
return(CompletedCommand());
}
case AssemblyState.ExpectingContentHeader:
{
if (f.Type != Constants.FrameHeader)
{
throw new UnexpectedFrameException(f);
}
NetworkBinaryReader reader = f.GetReader();
m_command.Header = m_protocol.DecodeContentHeaderFrom(reader);
m_remainingBodyBytes = m_command.Header.ReadFrom(reader);
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.ExpectingContentBody:
{
if (f.Type != Constants.FrameBody)
{
throw new UnexpectedFrameException(f);
}
byte[] fragment = f.Payload;
m_command.AppendBodyFragment(fragment);
if ((ulong)fragment.Length > m_remainingBodyBytes)
{
throw new MalformedFrameException
(string.Format("Overlong content body received - {0} bytes remaining, {1} bytes received",
m_remainingBodyBytes,
fragment.Length));
}
m_remainingBodyBytes -= (ulong)fragment.Length;
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.Complete:
default:
#if NETFX_CORE
Debug.WriteLine("Received frame in invalid state {0}; {1}", m_state, f);
#else
Trace.Fail(string.Format("Received frame in invalid state {0}; {1}", m_state, f));
#endif
return(null);
}
}
public Command HandleFrame(Frame f)
{
switch (m_state)
{
case AssemblyState.ExpectingMethod:
{
if (f.Type != Constants.FrameMethod)
{
throw new UnexpectedFrameException(f);
}
m_command.Method = m_protocol.DecodeMethodFrom(f.GetReader());
m_state = m_command.Method.HasContent
? AssemblyState.ExpectingContentHeader
: AssemblyState.Complete;
return CompletedCommand();
}
case AssemblyState.ExpectingContentHeader:
{
if (f.Type != Constants.FrameHeader)
{
throw new UnexpectedFrameException(f);
}
NetworkBinaryReader reader = f.GetReader();
m_command.Header = m_protocol.DecodeContentHeaderFrom(reader);
m_remainingBodyBytes = m_command.Header.ReadFrom(reader);
UpdateContentBodyState();
return CompletedCommand();
}
case AssemblyState.ExpectingContentBody:
{
if (f.Type != Constants.FrameBody)
{
throw new UnexpectedFrameException(f);
}
byte[] fragment = f.Payload;
m_command.AppendBodyFragment(fragment);
if ((ulong)fragment.Length > m_remainingBodyBytes)
{
throw new MalformedFrameException
(string.Format("Overlong content body received - {0} bytes remaining, {1} bytes received",
m_remainingBodyBytes,
fragment.Length));
}
m_remainingBodyBytes -= (ulong)fragment.Length;
UpdateContentBodyState();
return CompletedCommand();
}
case AssemblyState.Complete:
default:
#if NETFX_CORE
Debug.WriteLine("Received frame in invalid state {0}; {1}", m_state, f);
#else
Trace.Fail(string.Format("Received frame in invalid state {0}; {1}", m_state, f));
#endif
return null;
}
}
private void Reset()
{
m_state = AssemblyState.ExpectingMethod;
m_method = null;
m_header = null;
m_body = null;
_offset = 0;
m_remainingBodyBytes = 0;
}
private void Reset()
{
_method = null;
_header = null;
_bodyBytes = null;
_body = ReadOnlyMemory <byte> .Empty;
_remainingBodyBytes = 0;
_offset = 0;
_state = AssemblyState.ExpectingMethod;
}
public Command HandleFrame(InboundFrame f)
{
switch (m_state)
{
case AssemblyState.ExpectingMethod:
{
if (!f.IsMethod())
{
throw new UnexpectedFrameException(f);
}
m_command.Method = m_protocol.DecodeMethodFrom(f.GetReader());
m_state = m_command.Method.HasContent
? AssemblyState.ExpectingContentHeader
: AssemblyState.Complete;
return(CompletedCommand());
}
case AssemblyState.ExpectingContentHeader:
{
if (!f.IsHeader())
{
throw new UnexpectedFrameException(f);
}
NetworkBinaryReader reader = f.GetReader();
m_command.Header = m_protocol.DecodeContentHeaderFrom(reader);
m_remainingBodyBytes = m_command.Header.ReadFrom(reader);
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.ExpectingContentBody:
{
if (!f.IsBody())
{
throw new UnexpectedFrameException(f);
}
m_command.AppendBodyFragment(f.Payload);
if ((ulong)f.Payload.Length > m_remainingBodyBytes)
{
throw new MalformedFrameException
(string.Format("Overlong content body received - {0} bytes remaining, {1} bytes received",
m_remainingBodyBytes,
f.Payload.Length));
}
m_remainingBodyBytes -= (ulong)f.Payload.Length;
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.Complete:
default:
return(null);
}
}
public Command HandleFrame(InboundFrame f)
{
switch (m_state)
{
case AssemblyState.ExpectingMethod:
if (!f.IsMethod())
{
throw new UnexpectedFrameException(f);
}
m_method = m_protocol.DecodeMethodFrom(f.Payload);
m_state = m_method.HasContent ? AssemblyState.ExpectingContentHeader : AssemblyState.Complete;
return(CompletedCommand());
case AssemblyState.ExpectingContentHeader:
if (!f.IsHeader())
{
throw new UnexpectedFrameException(f);
}
m_header = m_protocol.DecodeContentHeaderFrom(NetworkOrderDeserializer.ReadUInt16(f.Payload));
ulong totalBodyBytes = m_header.ReadFrom(f.Payload.Slice(2));
if (totalBodyBytes > MaxArrayOfBytesSize)
{
throw new UnexpectedFrameException(f);
}
m_remainingBodyBytes = (int)totalBodyBytes;
m_body = MemoryPool <byte> .Shared.Rent(m_remainingBodyBytes);
UpdateContentBodyState();
return(CompletedCommand());
case AssemblyState.ExpectingContentBody:
if (!f.IsBody())
{
throw new UnexpectedFrameException(f);
}
if (f.Payload.Length > m_remainingBodyBytes)
{
throw new MalformedFrameException($"Overlong content body received - {m_remainingBodyBytes} bytes remaining, {f.Payload.Length} bytes received");
}
f.Payload.CopyTo(m_body.Memory.Slice(_offset));
m_remainingBodyBytes -= f.Payload.Length;
_offset += f.Payload.Length;
UpdateContentBodyState();
return(CompletedCommand());
case AssemblyState.Complete:
default:
return(null);
}
}
private void Reset()
{
_methodBytes = ReadOnlyMemory <byte> .Empty;
_rentedMethodArray = null;
_commandId = default;
_header = null;
_bodyBytes = null;
_body = ReadOnlyMemory <byte> .Empty;
_remainingBodyBytes = 0;
_offset = 0;
_state = AssemblyState.ExpectingMethod;
}
protected internal void _robot_state_cb(AssemblyState msg)
{
lock (this)
{
_last_robot_state = _stopwatch.ElapsedMilliseconds;
// TODO: check on real robot?
_ready = msg.enabled; //msg.ready;
_enabled = msg.enabled;
_stopped = msg.stopped;
_error = msg.error;
_estop_source = msg.estop_source;
_operational_mode = RobotOperationalMode.cobot;
}
}
private void UpdateContentBodyState()
{
m_state = (m_remainingBodyBytes > 0)
? AssemblyState.ExpectingContentBody
: AssemblyState.Complete;
}
private void Reset()
{
m_state = AssemblyState.ExpectingMethod;
m_command = new Command();
m_remainingBodyBytes = 0;
}
private void UpdateContentBodyState()
{
m_state = (m_remainingBodyBytes > 0)
? AssemblyState.ExpectingContentBody
: AssemblyState.Complete;
}
private void Reset()
{
m_state = AssemblyState.ExpectingMethod;
m_command = new Command();
m_remainingBodyBytes = 0;
}
static AssemblyState CreateAssemblyState(DmdAssembly assembly)
{
var state = new AssemblyState(assembly);
return(assembly.GetOrCreateData(() => state));
}
public Command HandleFrame(InboundFrame f)
{
switch (m_state)
{
case AssemblyState.ExpectingMethod:
{
if (!f.IsMethod())
{
throw new UnexpectedFrameException(f);
}
m_method = m_protocol.DecodeMethodFrom(f.GetReader());
m_state = m_method.HasContent
? AssemblyState.ExpectingContentHeader
: AssemblyState.Complete;
return(CompletedCommand());
}
case AssemblyState.ExpectingContentHeader:
{
if (!f.IsHeader())
{
throw new UnexpectedFrameException(f);
}
NetworkBinaryReader reader = f.GetReader();
m_header = m_protocol.DecodeContentHeaderFrom(reader);
var totalBodyBytes = m_header.ReadFrom(reader);
if (totalBodyBytes > MaxArrayOfBytesSize)
{
throw new UnexpectedFrameException(f);
}
m_remainingBodyBytes = (int)totalBodyBytes;
m_body = new byte[m_remainingBodyBytes];
m_bodyStream = new MemoryStream(m_body, true);
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.ExpectingContentBody:
{
if (!f.IsBody())
{
throw new UnexpectedFrameException(f);
}
if (f.Payload.Length > m_remainingBodyBytes)
{
throw new MalformedFrameException
(string.Format("Overlong content body received - {0} bytes remaining, {1} bytes received",
m_remainingBodyBytes,
f.Payload.Length));
}
m_bodyStream.Write(f.Payload, 0, f.Payload.Length);
m_remainingBodyBytes -= f.Payload.Length;
UpdateContentBodyState();
return(CompletedCommand());
}
case AssemblyState.Complete:
default:
#if NETFX_CORE
Debug.WriteLine("Received frame in invalid state {0}; {1}", m_state, f);
#else
//Trace.Fail(string.Format("Received frame in invalid state {0}; {1}", m_state, f));
#endif
return(null);
}
}
