/// <summary>
/// Convert TimeSpan to TimeData
/// </summary>
/// <param name="timestamp"> The timespan to convert to seconds/nanoseconds </param>
/// <returns> a time struct </returns>
public static TimeData GetTime(TimeSpan timestamp)
{
if (lastSimTimeReceived != default(TimeSpan))
{
timestamp = timestamp.Subtract(lastSimTimeReceived).Add(lastSimTime);
}
return(TimeData.FromTicks(timestamp.Ticks));
}
/// <summary>
/// Gets the current time or the last received sim time as std_msgs/Time
/// </summary>
/// <returns>Current time or the last received sim time as std_msgs/Time</returns>
public static std_msgs.Time Now()
{
if (SimTime.Instance.IsTimeSimulated && lastSimTime != default)
{
return(new std_msgs.Time(TimeData.FromTicks(lastSimTime.Ticks)));
}
return(ToTimeMessage(DateTime.UtcNow));
}
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);
}
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);
}
}
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();
}
public bool Equals(TimeData timer)
{
return(sec == timer.sec && nsec == timer.nsec);
}
