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); }
public void lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string) { transform = new emTransform(); string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform.translation = new emVector3(); transform.rotation = new emQuaternion(); transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = ROS.GetTime(DateTime.Now); return; } lock (framemutex) { uint target_id = getFrameID(mapped_tgt); uint source_id = getFrameID(mapped_src); TransformAccum accum = new TransformAccum(); TF_STATUS retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string); if (error_string != null && retval != TF_STATUS.NO_ERROR) { switch (retval) { case TF_STATUS.CONNECTIVITY_ERROR: ROS.Error("NO CONNECTIONSZSZ: " + error_string); break; case TF_STATUS.EXTRAPOLATION_ERROR: ROS.Error("EXTRAPOLATION: " + error_string); break; case TF_STATUS.LOOKUP_ERROR: ROS.Error("LOOKUP: " + error_string); break; } } transform.translation = accum.result_vec; transform.rotation = accum.result_quat; transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = new Time { data = new TimeData { sec = (uint)(accum.time >> 32), nsec = (uint)(accum.time & 0xFFFFFFFF) } }; } }
internal bool EnqueueMessage(MessageAndSerializerFunc holder) { lock (subscriber_links_mutex) { if (Dropped) { return(false); } } uint seq = incrementSequence(); if (HasHeader) { object h = holder.msg.GetType().GetField("header").GetValue(holder.msg); m.Header header; if (h == null) { header = new m.Header(); } else { header = (m.Header)h; } header.seq = seq; if (header.stamp == null) { header.stamp = ROS.GetTime(); } if (header.frame_id == null) { header.frame_id = ""; } holder.msg.GetType().GetField("header").SetValue(holder.msg, header); } holder.msg.connection_header = connection_header.Values; lock (subscriber_links_mutex) foreach (SubscriberLink sub_link in subscriber_links) { sub_link.enqueueMessage(holder); } if (Latch) { last_message = new MessageAndSerializerFunc(holder.msg, holder.serfunc, false, true); } return(true); }
private void Append(string m, ROSOUT_LEVEL lvl, int level) { StackFrame sf = new StackTrace(new StackFrame(level, true)).GetFrame(0); Log logmsg = new Log { msg = m, name = this_node.Name, file = sf.GetFileName(), function = sf.GetMethod().Name, line = (uint)sf.GetFileLineNumber(), level = ((byte)((int)lvl)), header = new m.Header() { Stamp = ROS.GetTime() } }; TopicManager.Instance.getAdvertisedTopics(out logmsg.topics); lock (log_queue) log_queue.Enqueue(logmsg); }
public bool EnqueueMessage(IRosMessage msg) { lock (subscriber_links_mutex) { if (Dropped) { return(false); } } uint seq = incrementSequence(); if (HasHeader) { object h = msg.GetType().GetField("header").GetValue(msg); m.Header header; if (h == null) { header = new m.Header(); } else { header = (m.Header)h; } header.seq = seq; header.stamp = ROS.GetTime(); header.frame_id = new m.String(); msg.GetType().GetField("header").SetValue(msg, header); } msg.connection_header = connection_header.Values; lock (subscriber_links_mutex) foreach (SubscriberLink sub_link in subscriber_links) { sub_link.enqueueMessage(msg, true, false); } if (Latch) { last_message = msg; } return(true); }
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); } }
public IMessageEvent(IRosMessage msg) : this(msg, msg.connection_header, ROS.GetTime().data, true, DefaultCreator) { }
public MessageEvent (T msg) { message = msg; connectionHeader = new Dictionary<string, string> (); receiptTime = ROS.GetTime (); }
private TF_STATUS getLatestCommonTime(uint target_id, uint source_id, ref ulong time, ref string error_str) { if (target_id == source_id) { time = TimeCache.toLong(ROS.GetTime(DateTime.Now).data); return(TF_STATUS.NO_ERROR); } List <TimeAndFrameID> lct = new List <TimeAndFrameID>(); uint frame = source_id; TimeAndFrameID temp; uint depth = 0; ulong common_time = ulong.MaxValue; while (frame != 0) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameID latest = cache.getLatestTimeAndParent(); if (latest.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } lct.Add(latest); frame = latest.frame_id; if (frame == target_id) { time = common_time; if (time == ulong.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } frame = target_id; depth = 0; common_time = ulong.MaxValue; uint common_parent = 0; while (true) { TimeCache cache; if (!frames.ContainsKey(frame)) { break; } cache = frames[frame]; TimeAndFrameID latest = cache.getLatestTimeAndParent(); if (latest.frame_id == 0) { break; } if (latest.time != 0) { common_time = Math.Min(latest.time, common_time); } foreach (TimeAndFrameID tf in lct) { if (tf.frame_id == latest.frame_id) { common_parent = tf.frame_id; break; } } frame = latest.frame_id; if (frame == source_id) { time = common_time; if (time == uint.MaxValue) { time = 0; } return(TF_STATUS.NO_ERROR); } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_str != null) { error_str = "The tf tree is invalid because it contains a loop."; } return(TF_STATUS.LOOKUP_ERROR); } } if (common_parent == 0) { error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id]; return(TF_STATUS.CONNECTIVITY_ERROR); } for (int i = 0; i < lct.Count; i++) { if (lct[i].time != 0) { common_time = Math.Min(common_time, lct[i].time); } if (lct[i].frame_id == common_parent) { break; } } if (common_time == uint.MaxValue) { common_time = 0; } time = common_time; return(TF_STATUS.NO_ERROR); }
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); }