public void AddCallback(CallbackInterface cb, long owner_id)
{
CallbackInfo info = new CallbackInfo {
Callback = cb, RemovalId = owner_id
};
//Logger.LogDebug($"CallbackQueue@{cbthread.ManagedThreadId}: Add callback owner: {owner_id} {cb.ToString()}");
lock (mutex)
{
if (!enabled)
{
return;
}
callbacks.Add(info);
//Logger.LogDebug($"CallbackQueue@{cbthread.ManagedThreadId}: Added");
count++;
}
lock (idInfoMutex)
{
if (!idInfo.ContainsKey(owner_id))
{
idInfo.Add(owner_id, new IDInfo {
calling_rw_mutex = new object(), id = owner_id
});
}
}
NotifyOne();
}
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);
}
}
public CallbackInfo SpliceOut(CallbackInfo info)
{
lock (_queue)
{
if (!_queue.Contains(info))
{
return(null);
}
_queue.RemoveAt(_queue.IndexOf(info));
return(info);
}
}
public void Enqueue(CallbackInfo info)
{
if (info.Callback == null)
{
return;
}
lock (_queue)
{
_queue.Add(info);
}
}
public CallbackInfo Dequeue()
{
lock (_queue)
{
if (_queue.Count == 0)
{
return(null);
}
CallbackInfo result = _queue[0];
_queue.RemoveAt(0);
return(result);
}
}
private CallOneResult CallOne(TLS tls)
{
CallbackInfo info = tls.Head;
if (info == null)
{
return(CallOneResult.Empty);
}
IDInfo idinfo = null;
idinfo = GetIdInfo(info.RemovalId);
if (idinfo != null)
{
CallbackInterface cb = info.Callback;
lock (idinfo.calling_rw_mutex)
{
CallbackInterface.CallResult result = CallbackInterface.CallResult.Invalid;
tls.SpliceOut(info);
if (!info.MarkedForRemoval)
{
try
{
result = cb.Call();
}
catch (Exception ex)
{
ROS.Error()($"[{ThisNode.Name}] Error during callback. Error: {ex.ToString()}, Stacktrace: {ex.StackTrace}");
}
}
if (result == CallbackInterface.CallResult.TryAgain && !info.MarkedForRemoval)
{
lock ( mutex )
{
callbacks.Add(info);
count++;
}
return(CallOneResult.TryAgain);
}
}
return(CallOneResult.Called);
}
CallbackInfo cbi = tls.SpliceOut(info);
if (cbi != null)
{
cbi.Callback.Call();
}
return(CallOneResult.Called);
}
public CallOneResult CallOne(TLS tls)
{
CallbackInfo info = tls.Head;
if (info == null)
{
return(CallOneResult.Empty);
}
IDInfo idinfo = null;
idinfo = GetIdInfo(info.RemovalId);
if (idinfo != null)
{
CallbackInterface cb = info.Callback;
lock (idinfo.calling_rw_mutex)
{
CallbackInterface.CallResult result = CallbackInterface.CallResult.Invalid;
tls.SpliceOut(info);
if (!info.MarkedForRemoval)
{
result = cb.Call();
}
if (result == CallbackInterface.CallResult.TryAgain && !info.MarkedForRemoval)
{
lock ( mutex )
{
callbacks.Add(info);
count++;
}
return(CallOneResult.TryAgain);
}
}
return(CallOneResult.Called);
}
CallbackInfo cbi = tls.SpliceOut(info);
if (cbi != null)
{
cbi.Callback.Call();
}
return(CallOneResult.Called);
}
