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 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); }
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo) { var logMessage = new Log { msg = message, name = ThisNode.Name, file = callerInfo.FilePath, function = callerInfo.MemberName, line = (uint)callerInfo.LineNumber, level = (byte)level, header = new Messages.std_msgs.Header { stamp = ROS.GetTime() } }; logMessage.topics = topicManager.GetAdvertisedTopics(); queue.TryOnNext(logMessage); }
internal void Append(string message, ROSOUT_LEVEL level, CallerInfo callerInfo) { var logMessage = new Log { msg = message, name = ThisNode.Name, file = callerInfo.FilePath, function = callerInfo.MemberName, line = (uint)callerInfo.LineNumber, level = ((byte)((int)level)), header = new Messages.std_msgs.Header() { stamp = ROS.GetTime() } }; TopicManager.Instance.getAdvertisedTopics(out logMessage.topics); lock (log_queue) log_queue.Enqueue(logMessage); }
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 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); }