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);
}
