/// <summary>
/// Creates a ROS Subscriber
/// </summary>
/// <param name="topic">Topic name to subscribe to</param>
/// <param name="nodeHandle">nodehandle</param>
/// <param name="cb">callback function to be fired when message is received</param>
public Subscriber(string topic, NodeHandle nodeHandle, ISubscriptionCallbackHelper cb)
: base(topic)
{
this.topic = topic;
this.nodehandle = new NodeHandle(nodeHandle);
this.helper = cb;
}
public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage message, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time)
{
if (was_full)
{
was_full = false;
}
var i = new Item
{
helper = helper,
message = message,
nonconst_need_copy = nonconst_need_copy,
receipt_time = receipt_time
};
lock ( queue )
{
if (FullNoLock())
{
queue.Dequeue();
was_full = true;
}
queue.Enqueue(i);
}
}
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 void RemoveCallback(ISubscriptionCallbackHelper helper)
{
lock (gate)
{
foreach (CallbackInfo info in callbacks)
{
if (info.Helper == helper)
{
info.SubscriptionQueue.Clear();
info.CallbackQueue.RemoveByOwner(helper);
callbacks.Remove(info);
break;
}
}
}
}
public void removeCallback(ISubscriptionCallbackHelper helper)
{
lock ( callbacks_mutex )
{
foreach (ICallbackInfo info in callbacks)
{
if (info.helper == helper)
{
info.subscription_queue.Clear();
info.callback.RemoveById(info.subscription_queue.Uid);
callbacks.Remove(info);
//if (!helper.isConst())
--nonconst_callbacks;
break;
}
}
}
}
public async Task <bool> Unsubscribe(string topic, ISubscriptionCallbackHelper sbch)
{
Subscription sub = null;
lock (gate)
{
if (shuttingDown)
{
return(false);
}
foreach (Subscription s in subscriptions)
{
if (s.Name == topic)
{
sub = s;
break;
}
}
}
if (sub == null)
{
return(false);
}
sub.RemoveCallback(sbch);
if (sub.NumCallbacks == 0)
{
lock (gate)
{
subscriptions.Remove(sub);
}
if (!await UnregisterSubscriber(topic))
{
this.logger.LogWarning("Couldn't unregister subscriber for topic [" + topic + "]");
}
sub.Dispose();
return(true);
}
return(true);
}
public bool unsubscribe(string topic, ISubscriptionCallbackHelper sbch)
{
Subscription sub = null;
lock ( subcriptionsMutex )
{
if (shuttingDown)
{
return(false);
}
foreach (Subscription s in subscriptions)
{
if (s.name == topic)
{
sub = s;
break;
}
}
}
if (sub == null)
{
return(false);
}
sub.removeCallback(sbch);
if (sub.NumCallbacks == 0)
{
lock ( subcriptionsMutex )
{
subscriptions.Remove(sub);
}
if (!unregisterSubscriber(topic))
{
ROS.Warn()($"[{ThisNode.Name}] Couldn't unregister subscriber for topic [{topic}]");
}
sub.shutdown();
return(true);
}
return(true);
}
public abstract void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time);
public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool wasFull, TimeData receiptTime)
{
throw new NotImplementedException();
}
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);
}
