private void tf_callback(tfMessage msg)
{
lock (transforms)
{
transforms.Enqueue(msg);
DateTime now = DateTime.Now;
}
}
private void Update(tfMessage msg)
{
foreach (TransformStamped tf in msg.transforms)
{
if (!setTransform(new Transform(tf)))
{
ROS.Warn()("Failed to setTransform in transformer update function");
}
}
}
public tf_node()
{
if (additions == null)
{
additions = new Queue <tfMessage>();
}
if (tfhandle == null)
{
tfhandle = new NodeHandle();
}
if (updateThread == null)
{
updateThread = new Thread(() =>
{
while (ROS.ok)
{
Queue <tfMessage> local;
lock (addlock)
{
local = new Queue <tfMessage>(additions);
additions.Clear();
}
if (local.Count > 0)
{
while (local.Count > 0)
{
tfMessage msg = local.Dequeue();
foreach (gm.TransformStamped t in msg.transforms)
{
transformer.setTransform(new emTransform(t));
}
}
}
Thread.Sleep(10);
}
});
updateThread.Start();
}
tfhandle.subscribe <tfMessage>("/tf", 100, tfCallback);
}
private void tfCallback(tfMessage msg)
{
lock (addlock)
additions.Enqueue(msg);
}
