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 void Add(long timestamp, Vector3 translation, Quaternion rotation) { TFFrame newEntry = new TFFrame(translation, rotation); // most likely case: we're just adding a newer transform to the end of the list if (m_Timestamps.Count == 0 || m_Timestamps[m_Timestamps.Count - 1] < timestamp) { m_Timestamps.Add(timestamp); m_Frames.Add(newEntry); m_GameObject.transform.localPosition = translation; m_GameObject.transform.localRotation = rotation; } else { int index = m_Timestamps.BinarySearch(timestamp); if (index < 0) { // no preexisting entry, but ~index gives us the position to insert the new entry m_Timestamps.Insert(~index, timestamp); m_Frames.Insert(~index, newEntry); } else { // we found an existing entry at the same timestamp!? Just replace the old one, I guess. m_Frames[index] = newEntry; } } // for now, just a lazy way to keep the buffer from growing infinitely: every 50 updates, discard the oldest 50 if (m_Timestamps.Count > 100) { m_Timestamps.RemoveRange(0, 50); m_Frames.RemoveRange(0, 50); } }
public void SetTFTrackingSettings(TFTrackingSettings tfTrackingType, HeaderMsg headerMsg) { switch (tfTrackingType.type) { case TFTrackingType.Exact: { TFFrame frame = TFSystem.instance.GetTransform(headerMsg, tfTrackingType.tfTopic); transform.position = frame.translation; transform.rotation = frame.rotation; } break; case TFTrackingType.TrackLatest: { transform.parent = TFSystem.instance.GetTransformObject(headerMsg.frame_id, tfTrackingType.tfTopic).transform; transform.localPosition = Vector3.zero; transform.localRotation = Quaternion.identity; } break; case TFTrackingType.None: transform.localPosition = Vector3.zero; transform.localRotation = Quaternion.identity; break; } }
public static TFFrame Lerp(TFFrame a, TFFrame b, float lerp) { return(new TFFrame { translation = Vector3.Lerp(a.translation, b.translation, lerp), rotation = Quaternion.Lerp(a.rotation, b.rotation, lerp) }); }
//添加元素 public void draw_Elements(Element[] elements, string name) { TFFrameList frame_list = new TFFrameListClass(); TFFrame frame = frame_list.AsTFFrame; for (int i = 0; i < elements.Length; i++) { frame.Add3DElement(elements[i]); } frame.SetName(name); TFApplicationList tfapp_list = new TFApplicationList(); TFApplication tfapp = tfapp_list.TFApplication; tfapp.ModelReferenceAddFrame(app.ActiveModelReference, frame); }
public TFFrame GetLocalTF(long time = 0) { // this stream has no data at all, so just report identity. if (m_Frames.Count == 0) { return(TFFrame.identity); } // if time is 0, just get the newest position if (time == 0) { return(m_Frames[m_Frames.Count - 1]); } int index = m_Timestamps.BinarySearch(time); if (index >= 0) { // no problem, we have an entry at this time return(m_Frames[index]); } index = ~index; if (index == 0) { // older than our first entry: just use the first one return(m_Frames[0]); } else if (index == m_Frames.Count) { // newer than our last entry: just use the last one return(m_Frames[m_Frames.Count - 1]); } else { // between two entries: interpolate float lerpValue = (time - m_Timestamps[index - 1]) / (float)(m_Timestamps[index] - m_Timestamps[index - 1]); return(TFFrame.Lerp(m_Frames[index - 1], m_Frames[index], lerpValue)); } }
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); } }
public TFFrame Compose(TFFrame child) { return(new TFFrame(TransformPoint(child.translation), rotation * child.rotation)); }