internal bool addCallback <M>(SubscriptionCallbackHelper <M> helper, string md5sum, CallbackQueueInterface queue,
uint queue_size, bool allow_concurrent_callbacks, string topiclol) where M : IRosMessage, new()
{
lock (md5sum_mutex)
{
if (this.md5sum == "*" && md5sum != "*")
{
this.md5sum = md5sum;
}
}
if (md5sum != "*" && md5sum != this.md5sum)
{
return(false);
}
lock (callbacks_mutex)
{
CallbackInfo <M> info = new CallbackInfo <M> {
helper = helper, callback = queue, subscription_queue = new Callback <M>(helper.callback().func, topiclol, queue_size, allow_concurrent_callbacks)
};
//if (!helper.isConst())
//{
++nonconst_callbacks;
//}
callbacks.Add(info);
if (latched_messages.Count > 0)
{
lock (publisher_links_mutex)
{
foreach (PublisherLink link in publisher_links)
{
if (link.Latched)
{
if (latched_messages.ContainsKey(link))
{
LatchInfo latch_info = latched_messages[link];
IMessageDeserializer des = new IMessageDeserializer(helper, latch_info.message,
latch_info.connection_header);
bool was_full = false;
((Callback <M>)info.subscription_queue).push((SubscriptionCallbackHelper <M>)info.helper, (MessageDeserializer <M>)des, true,
ref was_full,
latch_info.receipt_time);
((Callback <M>)info.subscription_queue).topic = topiclol;
if (!was_full)
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
}
}
}
}
return(true);
}
internal ulong handleMessage(IRosMessage msg, bool ser, bool nocopy, IDictionary connection_header,
PublisherLink link)
{
IRosMessage t = null;
ulong drops = 0;
TimeData receipt_time = ROS.GetTime().data;
if (msg.Serialized != null) //will be null if self-subscribed
{
msg.Deserialize(msg.Serialized);
}
lock (callbacks_mutex)
{
foreach (ICallbackInfo info in callbacks)
{
MsgTypes ti = info.helper.type;
if (nocopy || ser)
{
t = msg;
t.connection_header = msg.connection_header;
t.Serialized = null;
bool was_full = false;
bool nonconst_need_copy = callbacks.Count > 1;
info.subscription_queue.pushitgood(info.helper, t, nonconst_need_copy, ref was_full, receipt_time);
if (was_full)
{
++drops;
}
else
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
}
if (t != null && link.Latched)
{
LatchInfo li = new LatchInfo
{
message = t,
link = link,
connection_header = connection_header,
receipt_time = receipt_time
};
if (latched_messages.ContainsKey(link))
{
latched_messages[link] = li;
}
else
{
latched_messages.Add(link, li);
}
}
return(drops);
}
internal bool AddCallback(
ISubscriptionCallbackHelper helper,
string md5sum,
ICallbackQueue queue,
int queueSize,
bool allowConcurrentCallbacks,
string topic
)
{
lock (gate)
{
if (this.Md5Sum == "*" && md5sum != "*")
{
this.Md5Sum = md5sum;
}
if (md5sum != "*" && md5sum != this.Md5Sum)
{
return(false);
}
var info = new CallbackInfo
{
Helper = helper,
CallbackQueue = queue,
SubscriptionQueue = new Callback(helper.Callback.SendEvent, topic, queueSize, allowConcurrentCallbacks)
};
callbacks.Add(info);
if (latchedMessages.Count > 0)
{
string ti = info.Helper.type;
var receiptTime = ROS.GetTime().data;
foreach (PublisherLink link in publisherLinks)
{
if (link.Latched)
{
if (latchedMessages.ContainsKey(link))
{
LatchInfo latch_info = latchedMessages[link];
bool wasFull = false;
bool nonconst_need_copy = callbacks.Count > 1;
info.SubscriptionQueue.AddToCallbackQueue(info.Helper, latchedMessages[link].Message, nonconst_need_copy, ref wasFull, receiptTime);
if (!wasFull)
{
info.CallbackQueue.AddCallback(info.SubscriptionQueue, helper);
}
}
}
}
}
return(true);
}
}
internal bool addCallback <M>(SubscriptionCallbackHelper <M> helper, string md5sum, CallbackQueueInterface queue,
uint queue_size, bool allow_concurrent_callbacks, string topiclol) where M : IRosMessage, new()
{
lock (md5sum_mutex)
{
if (this.md5sum == "*" && md5sum != "*")
{
this.md5sum = md5sum;
}
}
if (md5sum != "*" && md5sum != this.md5sum)
{
return(false);
}
lock (callbacks_mutex)
{
CallbackInfo <M> info = new CallbackInfo <M> {
helper = helper, callback = queue, subscription_queue = new Callback <M>(helper.callback().func, topiclol, queue_size, allow_concurrent_callbacks)
};
//if (!helper.isConst())
//{
++nonconst_callbacks;
//}
callbacks.Add(info);
if (latched_messages.Count > 0)
{
MsgTypes ti = info.helper.type;
lock (publisher_links_mutex)
{
foreach (PublisherLink link in publisher_links)
{
if (link.Latched)
{
if (latched_messages.ContainsKey(link))
{
LatchInfo latch_info = latched_messages[link];
bool was_full = false;
bool nonconst_need_copy = false; //callbacks.Count > 1;
info.subscription_queue.pushitgood(info.helper, latched_messages[link].message, nonconst_need_copy, ref was_full, ROS.GetTime().data);
if (!was_full)
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
}
}
}
}
return(true);
}
internal long HandleMessage(
RosMessage msg,
bool ser,
bool nocopy,
IDictionary <string, string> connectionHeader,
PublisherLink link
)
{
RosMessage t = null;
long drops = 0;
TimeData receipt_time = ROS.GetTime().data;
if (msg.Serialized != null) // will be null if self-subscribed
{
msg.Deserialize(msg.Serialized);
}
lock (gate)
{
foreach (CallbackInfo info in callbacks)
{
string ti = info.Helper.type;
if (nocopy || ser)
{
t = msg;
t.connection_header = msg.connection_header;
t.Serialized = null;
bool wasFull = false;
bool nonconst_need_copy = callbacks.Count > 1;
info.SubscriptionQueue.AddToCallbackQueue(info.Helper, t, nonconst_need_copy, ref wasFull, receipt_time);
if (wasFull)
{
++drops;
}
else
{
info.CallbackQueue.AddCallback(info.SubscriptionQueue, info.SubscriptionQueue);
}
}
}
}
if (t != null && link.Latched)
{
LatchInfo li = new LatchInfo
{
Message = t,
Link = link,
ConnectionHeader = connectionHeader,
ReceiptTime = receipt_time
};
if (latchedMessages.ContainsKey(link))
{
latchedMessages[link] = li;
}
else
{
latchedMessages.Add(link, li);
}
}
return(drops);
}
internal ulong handleMessage(IRosMessage msg, bool ser, bool nocopy, IDictionary connection_header,
PublisherLink link)
{
lock (callbacks_mutex)
{
ulong drops = 0;
cached_deserializers.Clear();
TimeData receipt_time = ROS.GetTime().data;
foreach (ICallbackInfo info in callbacks)
{
MsgTypes ti = info.helper.type;
if (nocopy || ser)
{
IMessageDeserializer deserializer = null;
if (cached_deserializers.ContainsKey(ti))
{
deserializer = cached_deserializers[ti];
}
else
{
deserializer = MakeDeserializer(ti, info.helper, msg, connection_header);
cached_deserializers.Add(ti, deserializer);
}
bool was_full = false;
bool nonconst_need_copy = callbacks.Count > 1;
//info.helper.callback().func(msg);
//info.helper.callback().pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time);
info.subscription_queue.pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time);
//push(info.helper, deserializer, nonconst_need_copy, ref was_full,receipt_time);
if (was_full)
{
++drops;
}
else
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
if (link.Latched)
{
LatchInfo li = new LatchInfo
{
message = msg,
link = link,
connection_header = connection_header,
receipt_time = receipt_time
};
if (latched_messages.ContainsKey(link))
{
latched_messages[link] = li;
}
else
{
latched_messages.Add(link, li);
}
}
cached_deserializers.Clear();
return(drops);
}
}
internal bool addCallback(
ISubscriptionCallbackHelper helper,
string md5sum,
ICallbackQueue queue,
int queue_size,
bool allow_concurrent_callbacks,
string topiclol
)
{
lock ( md5sum_mutex )
{
if (this.md5sum == "*" && md5sum != "*")
{
this.md5sum = md5sum;
}
}
if (md5sum != "*" && md5sum != this.md5sum)
{
return(false);
}
lock ( callbacks_mutex )
{
ICallbackInfo info = new ICallbackInfo
{
helper = helper,
callback = queue,
subscription_queue = new Callback(helper.Callback.SendEvent, topiclol, queue_size, allow_concurrent_callbacks)
};
//if (!helper.isConst())
//{
++nonconst_callbacks;
//}
callbacks.Add(info);
if (latched_messages.Count > 0)
{
string ti = info.helper.type;
lock ( publisher_links_mutex )
{
foreach (PublisherLink link in publisher_links)
{
if (link.Latched)
{
if (latched_messages.ContainsKey(link))
{
LatchInfo latch_info = latched_messages[link];
bool was_full = false;
bool nonconst_need_copy = callbacks.Count > 1;
info.subscription_queue.AddToCallbackQueue(info.helper, latched_messages[link].message, nonconst_need_copy, ref was_full, ROS.GetTime().data);
if (!was_full)
{
info.callback.AddCallback(info.subscription_queue);
}
}
}
}
}
}
}
return(true);
}
