public void ReceiveTF_MultipleMessages_InterpolatesTimes()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic();
TFStream stream = topic.GetOrCreateFrame(simple_frame_id);
int time1_secs = 1000;
int time2_secs = 2000;
TimeMsg time1 = MakeTimeMsg(time1_secs, 0);
Vector3 unityPosition1 = new Vector3(1, 2, 3);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time1, parent_frame_id),
simple_frame_id,
new TransformMsg(unityPosition1.To <FLU>(), new QuaternionMsg()
)) }));
TimeMsg time2 = MakeTimeMsg(time2_secs, 0);
Vector3 unityPosition2 = new Vector3(2, 3, 4);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time2, parent_frame_id),
simple_frame_id,
new TransformMsg(unityPosition2.To <FLU>(), new QuaternionMsg()
)) }));
TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0);
Vector3 pointAtTime1 = stream.GetWorldTF(time1).translation;
Vector3 pointAtTime1_5 = stream.GetWorldTF(time1_5).translation;
Vector3 pointAtTime2 = stream.GetWorldTF(time2).translation;
Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2;
Assert.AreEqual(pointAtTime1, unityPosition1);
Assert.AreEqual(pointAtTime1_5, unityPosition1_5);
Assert.AreEqual(pointAtTime2, unityPosition2);
}
public void ReceiveTF_FrameWithParent_ReturnsSameTranslation()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem system = TFSystem.GetOrCreateInstance();
TFSystem.TFTopicState topic = system.GetOrCreateTFTopic();
TFStream stream = system.GetOrCreateFrame(composite_frame_id);
Assert.AreEqual(stream.Name, simple_frame_id);
Assert.AreEqual(stream.Parent.Name, parent_frame_id);
TimeMsg time = MakeTimeMsg(4567, 890);
Vector3 unityPosition = new Vector3(1, 2, 3);
topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg(
MakeHeaderMsg(time, parent_frame_id),
composite_frame_id,
new TransformMsg(unityPosition.To <FLU>(), new QuaternionMsg()
)) }));
TFFrame frame = stream.GetWorldTF(time.ToLongTime());
Assert.AreEqual(frame.translation.x, unityPosition.x);
Assert.AreEqual(frame.translation.y, unityPosition.y);
Assert.AreEqual(frame.translation.z, unityPosition.z);
Vector3 gameObjectPos = stream.GameObject.transform.position;
Assert.AreEqual(gameObjectPos.x, unityPosition.x);
Assert.AreEqual(gameObjectPos.y, unityPosition.y);
Assert.AreEqual(gameObjectPos.z, unityPosition.z);
}
public TFStream GetTransformStream(string frame_id)
{
TFStream result = null;
m_TransformTable.TryGetValue(frame_id, out result);
return(result);
}
public void NotifyChanged(TFStream stream)
{
foreach (Action <TFStream> callback in m_Listeners)
{
callback(stream);
}
}
public TFStream(TFStream parent, string name, string tfTopic)
{
Name = name;
TFTopic = tfTopic;
m_GameObject = new GameObject(name);
SetParent(parent);
}
void EnsureSettings(TFStream stream)
{
if (!m_ShowExpanded.ContainsKey(stream))
{
m_ShowExpanded.Add(stream, true);
m_ShowAxes.Add(stream, ShowTFAxesDefault);
m_ShowLinks.Add(stream, ShowTFLinksDefault);
m_ShowNames.Add(stream, ShowTFNamesDefault);
}
}
public void GetOrCreateFrame_Hierarchy_BuildsValidHierarchy()
{
ROSConnection ros = ROSConnection.GetOrCreateInstance();
ros.ConnectOnStart = false;
TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic();
TFStream stream = topic.GetOrCreateFrame(composite_frame_id);
GameObject gameObject = stream.GameObject;
Assert.IsNotNull(stream.GameObject);
Assert.AreEqual(stream.GameObject.name, simple_frame_id);
Assert.AreEqual(stream.GameObject.transform.parent.name, parent_frame_id);
TFStream stream2 = topic.GetOrCreateFrame(composite_frame_id2);
Assert.IsNotNull(stream2.GameObject);
Assert.AreEqual(stream2.GameObject.name, simple_frame_id2);
Assert.AreNotEqual(stream.GameObject, stream2.GameObject);
Assert.AreEqual(stream.GameObject.transform.parent, stream2.GameObject.transform.parent);
}
public void OnChanged(TFStream stream)
{
Drawing3d drawing;
if (!drawings.TryGetValue(stream.Name, out drawing))
{
drawing = Drawing3d.Create();
drawings[stream.Name] = drawing;
if (stream.Parent != null)
{
OnChanged(stream.Parent);
Drawing3d parentStream;
if (drawings.TryGetValue(stream.Parent.Name, out parentStream))
{
drawing.transform.parent = parentStream.transform;
}
}
}
TFFrame frame = stream.GetLocalTF();
drawing.transform.localPosition = frame.translation;
drawing.transform.localRotation = frame.rotation;
drawing.Clear();
EnsureSettings(stream);
if (m_ShowAxes[stream])
{
VisualizationUtils.DrawAxisVectors <FLU>(drawing, Vector3.zero.To <FLU>(), Quaternion.identity.To <FLU>(), axesScale, false);
}
if (m_ShowLinks[stream])
{
drawing.DrawLine(Quaternion.Inverse(frame.rotation) * -frame.translation, Vector3.zero, color, lineThickness);
}
if (m_ShowNames[stream])
{
drawing.DrawLabel(stream.Name, Vector3.zero, color);
}
}
void DrawTFStreamHierarchy(TFStream stream, int indent, bool globalChange)
{
GUI.changed = false;
EnsureSettings(stream);
GUILayout.BeginHorizontal();
GUILayout.Space(indent * k_IndentWidth);
/*#if UNITY_EDITOR
* GUIStyle style = new GUIStyle(UnityEditor.EditorStyles.foldout);
* style.fixedWidth = k_TFNameWidth;// - indent * k_IndentWidth;
* style.stretchWidth = false;
* m_ShowExpanded[stream] = UnityEditor.EditorGUILayout.Foldout(m_ShowExpanded[stream], stream.Name, true, style);// GUILayout.Width(k_TFNameWidth - indent * k_IndentWidth));
#else*/
if (GUILayout.Button(stream.Name, GUI.skin.label, GUILayout.Width(k_TFNameWidth - indent * k_IndentWidth)))
{
m_ShowExpanded[stream] = !m_ShowExpanded[stream];
}
//#endif
m_ShowAxes[stream] = GUILayout.Toggle(m_ShowAxes[stream], "", GUILayout.Width(k_CheckboxWidth));
m_ShowLinks[stream] = GUILayout.Toggle(m_ShowLinks[stream], "", GUILayout.Width(k_CheckboxWidth));
m_ShowNames[stream] = GUILayout.Toggle(m_ShowNames[stream], "", GUILayout.Width(k_CheckboxWidth));
GUILayout.EndHorizontal();
if (GUI.changed || globalChange)
{
TFSystem.instance.NotifyAllChanged(stream);
}
if (m_ShowExpanded[stream])
{
foreach (TFStream child in stream.Children)
{
DrawTFStreamHierarchy(child, indent + 1, globalChange);
}
}
}
public void SetParent(TFStream newParent)
{
if (Parent == newParent)
{
return;
}
if (Parent != null)
{
Parent.m_Children.Remove(this);
}
if (newParent != null)
{
m_GameObject.transform.parent = newParent.m_GameObject.transform;
newParent.m_Children.Add(this);
}
else
{
m_GameObject.transform.parent = null;
}
Parent = newParent;
}
public TFStream GetOrCreateTFStream(string frame_id)
{
TFStream tf;
while (frame_id.EndsWith("/"))
{
frame_id = frame_id.Substring(0, frame_id.Length - 1);
}
var slash = frame_id.LastIndexOf('/');
var singleName = slash == -1 ? frame_id : frame_id.Substring(slash + 1);
if (!m_TransformTable.TryGetValue(singleName, out tf) || tf == null)
{
if (slash <= 0)
{
// there's no slash, or only an initial slash - just create a new root object
// (set the parent later if and when we learn more)
tf = new TFStream(null, singleName, m_TFTopic);
}
else
{
var parent = GetOrCreateTFStream(frame_id.Substring(0, slash));
tf = new TFStream(parent, singleName, m_TFTopic);
}
m_TransformTable[singleName] = tf;
NotifyChanged(tf);
}
else if (slash > 0 && tf.Parent == null)
{
tf.SetParent(GetOrCreateTFStream(frame_id.Substring(0, slash)));
}
return(tf);
}
public GameObject GetTransformObject(string frame_id, string tfTopic = "/tf")
{
TFStream stream = GetOrCreateTFTopic(tfTopic).GetOrCreateTFStream(frame_id);
return(stream.GameObject);
}
public void NotifyAllChanged(TFStream stream)
{
GetOrCreateTFTopic(stream.TFTopic).NotifyAllChanged();
}
