internal void Publish(MessageAndSerializerFunc msg)
{
lock (gate)
{
publishQueue.TryOnNext(msg);
}
}
internal void publish(MessageAndSerializerFunc msg)
{
lock ( publish_queue_mutex )
{
publish_queue.Enqueue(msg);
}
}
public override void EnqueueMessage(MessageAndSerializerFunc holder)
{
if (!outbox.TryOnNext(holder))
{
// TODO: handle queue full case
}
}
internal bool EnqueueMessage(MessageAndSerializerFunc holder)
{
lock (gate)
{
if (Dropped)
{
return(false);
}
}
uint seq = IncrementSequence();
if (HasHeader)
{
object h = holder.msg.GetType().GetTypeInfo().GetField("header").GetValue(holder.msg);
std_msgs.Header header;
if (h == null)
{
header = new std_msgs.Header();
}
else
{
header = (std_msgs.Header)h;
}
header.seq = seq;
if (header.stamp == null)
{
header.stamp = ROS.GetTime();
}
if (header.frame_id == null)
{
header.frame_id = "";
}
holder.msg.GetType().GetTypeInfo().GetField("header").SetValue(holder.msg, header);
}
holder.msg.connection_header = connectionHeader.Values;
lock (gate)
{
foreach (SubscriberLink link in subscriberLinks)
{
link.EnqueueMessage(holder);
}
}
if (Latch)
{
lastMessage = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true);
}
return(true);
}
public override void EnqueueMessage(MessageAndSerializerFunc holder)
{
lock (gate)
{
if (dropped)
{
return;
}
}
if (subscriber != null)
{
subscriber.HandleMessage(holder.msg, holder.serialize, holder.nocopy);
}
}
private async Task WriteMessage(MessageAndSerializerFunc message)
{
if (message.msg.Serialized == null)
{
message.msg.Serialized = message.serfunc();
}
int length = message.msg.Serialized.Length;
await connection.WriteBlock(BitConverter.GetBytes(length), 0, 4, cancel).ConfigureAwait(false);
await connection.WriteBlock(message.msg.Serialized, 0, length, cancel).ConfigureAwait(false);
Stats.MessagesSent++;
Stats.BytesSent += length + 4;
Stats.MessageDataSent += length;
}
internal override void EnqueueMessage(MessageAndSerializerFunc holder)
{
lock ( outbox )
{
if (maxQueue > 0 && outbox.Count >= maxQueue)
{
outbox.Dequeue();
queueFull = true;
}
else
{
queueFull = false;
}
outbox.Enqueue(holder);
}
StartMessageWrite(false);
}
private void StartMessageWrite(bool immediateWrite)
{
MessageAndSerializerFunc holder = null;
if (writingMessage || !headerWritten)
{
return;
}
lock ( outbox )
{
if (outbox.Count > 0)
{
writingMessage = true;
holder = outbox.Dequeue();
}
if (outbox.Count < maxQueue)
{
queueFull = false;
}
}
if (holder != null)
{
if (holder.msg.Serialized == null)
{
holder.msg.Serialized = holder.serfunc();
}
byte[] outbuf = new byte[holder.msg.Serialized.Length + 4];
Array.Copy(holder.msg.Serialized, 0, outbuf, 4, holder.msg.Serialized.Length);
Array.Copy(BitConverter.GetBytes(holder.msg.Serialized.Length), outbuf, 4);
stats.messagesSent++;
//ROS.Debug()("Message backlog = " + (triedtosend - stats.messages_sent));
stats.bytesSent += outbuf.Length;
stats.messageDataSent += outbuf.Length;
connection.write(outbuf, outbuf.Length, OnMessageWritten, immediateWrite);
}
}
internal virtual void EnqueueMessage(MessageAndSerializerFunc holder)
{
throw new NotImplementedException();
}
public override void EnqueueMessage(MessageAndSerializerFunc holder)
{
outbox.TryOnNext(holder); // queue will drop oldest messages when full
}
public abstract void EnqueueMessage(MessageAndSerializerFunc holder);
