public void AddCallback(CallbackInterface cb, long owner_id)
{
CallbackInfo info = new CallbackInfo {
Callback = cb, RemovalId = owner_id
};
//ROS.Debug()( $"[{ThisNode.Name}] CallbackQueue@{cbthread.ManagedThreadId}: Add callback owner: {owner_id} {cb.ToString()}" );
lock ( mutex )
{
if (!enabled)
{
return;
}
callbacks.Add(info);
//ROS.Debug()( $"[{ThisNode.Name}] 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();
}
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 Subscriber Subscribe(string topic, string messageType, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks = false)
{
if (callbackQueue == null)
{
callbackQueue = ROS.GlobalCallbackQueue;
}
var message = RosMessage.Generate(messageType);
var ops = new SubscribeOptions(topic, message.MessageType, message.MD5Sum(), queueSize, new SubscriptionCallbackHelper <RosMessage>(message.MessageType, cb.SendEvent))
{
callback_queue = callbackQueue,
allow_concurrent_callbacks = allowConcurrentCallbacks
};
ops.callback_queue.AddCallback(cb);
return(subscribe(ops));
}
/// <summary>
/// Creates a subscriber
/// </summary>
/// <typeparam name="M">Topic type</typeparam>
/// <param name="topic">Topic name</param>
/// <param name="queueSize">How many messages to qeueue</param>
/// <param name="cb">Function to fire when a message is recieved</param>
/// <param name="allowConcurrentCallbacks">Probably breaks things when true</param>
/// <returns>A subscriber</returns>
public Subscriber subscribe <M>(string topic, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks)
where M : RosMessage, new()
{
if (callbackQueue == null)
{
callbackQueue = ROS.GlobalCallbackQueue;
}
var ops = new SubscribeOptions <M>(topic, queueSize, cb.SendEvent)
{
callback_queue = callbackQueue,
allow_concurrent_callbacks = allowConcurrentCallbacks
};
ops.callback_queue.AddCallback(cb);
return(subscribe(ops));
}
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);
}
public void AddCallback(CallbackInterface cb, object owner = null)
{
if (cb == null)
{
throw new ArgumentNullException(nameof(cb));
}
lock (gate)
{
if (!enabled)
{
return;
}
callbacks.Add(new CallbackInfo {
Callback = cb, Owner = owner
});
NotifyOne();
}
}
public CallOneResult CallOne()
{
CallbackInfo call;
lock (gate)
{
if (!enabled)
{
return(CallOneResult.Disabled);
}
if (callbacks.Count == 0)
{
return(CallOneResult.Empty);
}
call = callbacks.Front;
callbacks.PopFront();
calling += 1;
}
var owner = call.Owner;
try
{
CallbackInterface cb = call.Callback;
var result = CallbackInterface.CallResult.Invalid;
if (owner != null)
{
Monitor.Enter(owner);
}
try
{
result = cb.Call();
}
catch (Exception ex)
{
ROS.Error()("Error during callback. Error: %s, Stacktrace: %s", ex.ToString(), ex.StackTrace);
}
if (result == CallbackInterface.CallResult.TryAgain)
{
lock (gate)
{
callbacks.PushBack(call);
}
return(CallOneResult.TryAgain);
}
return(CallOneResult.Called);
}
finally
{
if (owner != null)
{
Monitor.Exit(owner);
}
lock (gate)
{
calling -= 1;
}
}
}
public void AddCallback(CallbackInterface callback)
{
AddCallback(callback, callback.Uid);
}
protected ISubscriptionCallbackHelper(CallbackInterface Callback)
{
this.Callback = Callback;
}
public SubscriptionCallbackHelper(CallbackInterface q)
: base(q)
{
}
public Subscriber Subscribe(string topic, string messageType, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks = false) => SubscribeAsync(topic, messageType, queueSize, cb, allowConcurrentCallbacks).Result;
public Subscriber Subscribe <M>(string topic, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks) where M : RosMessage, new() => SubscribeAsync <M>(topic, queueSize, cb, allowConcurrentCallbacks).Result;
public async Task <Subscriber> SubscribeAsync(string topic, string messageType, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks = false)
{
if (callbackQueue == null)
{
callbackQueue = ROS.GlobalCallbackQueue;
}
var message = RosMessage.Generate(messageType);
var ops = new SubscribeOptions(topic, message.MessageType, message.MD5Sum(), queueSize, new SubscriptionCallbackHelper <RosMessage>(message.MessageType, cb.SendEvent))
{
CallbackQueue = callbackQueue,
AllowConcurrentCallbacks = allowConcurrentCallbacks
};
ops.CallbackQueue.AddCallback(cb, ops.CallbackHelper);
return(await SubscribeAsync(ops).ConfigureAwait(false));
}
/// <summary>
/// Creates a subscriber
/// </summary>
/// <typeparam name="M">Topic type</typeparam>
/// <param name="topic">Topic name</param>
/// <param name="queueSize">How many messages to qeueue</param>
/// <param name="cb">Function to fire when a message is recieved</param>
/// <param name="allowConcurrentCallbacks">Probably breaks things when true</param>
/// <returns>A subscriber</returns>
public async Task <Subscriber> SubscribeAsync <M>(string topic, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks)
where M : RosMessage, new()
{
if (callbackQueue == null)
{
callbackQueue = ROS.GlobalCallbackQueue;
}
var ops = new SubscribeOptions <M>(topic, queueSize, cb.SendEvent)
{
CallbackQueue = callbackQueue,
AllowConcurrentCallbacks = allowConcurrentCallbacks
};
ops.CallbackQueue.AddCallback(cb, ops.CallbackHelper);
return(await SubscribeAsync(ops).ConfigureAwait(false));
}
