From 6517a3a41f168722140e57791689d2f7ff4a50f4 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Tue, 12 Jul 2022 16:59:08 -0400 Subject: [PATCH 01/10] Create TFSaver_Rework.cs.meta --- Scripts/TF/TFSaver_Rework.cs | 231 ++++++++++++++++++++++++++++++ Scripts/TF/TFSaver_Rework.cs.meta | 11 ++ 2 files changed, 242 insertions(+) create mode 100644 Scripts/TF/TFSaver_Rework.cs create mode 100644 Scripts/TF/TFSaver_Rework.cs.meta diff --git a/Scripts/TF/TFSaver_Rework.cs b/Scripts/TF/TFSaver_Rework.cs new file mode 100644 index 0000000..a602cf5 --- /dev/null +++ b/Scripts/TF/TFSaver_Rework.cs @@ -0,0 +1,231 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using Messages.geometry_msgs; +using Messages.tf; +using tf.net; +using Ros_CSharp; +using System; +public class TFSaver_Rework : MonoBehaviour +{ + /* NOTES + * - The subset of dictionaries dont remove themselves, while they may end up empty I dont know if this + * would cause problems in situations with many different frame interactions + * + * + * + */ + public ROSCore rosmaster; + private NodeHandle nh = null; + private Subscriber tfsub, tfstaticsub; + private Queue transforms = new Queue(); + Dictionary> stampedDict = new Dictionary>(); + Dictionary stampedDictStatic = new Dictionary(); + + const double SEC_TO_NSEC = 1000000000f; + + Publisher pub; + + //Test Stuff + public uint time = 10, storageTime; + public string link, child; + public double seconds; + + void Start() + { + nh = rosmaster.getNodeHandle(); + + tfstaticsub = nh.subscribe("/tf_static", 0, tf_static_callback); + tfsub = nh.subscribe("/tf", 0, tf_callback); + pub = nh.advertise("/tf", 10); + } + + private void tf_callback(tfMessage msg) + { + //Queues recieved transforms to be added to the dictionary + lock (transforms) + { + transforms.Enqueue(msg); + } + } + + private void tf_static_callback(tfMessage msg) + { //update existing statics - todo + lock (stampedDictStatic) + { + foreach (Messages.geometry_msgs.TransformStamped t in msg.transforms) + { + string key = t.header.frame_id + t.child_frame_id; + Debug.Log("found " + key); + if (stampedDictStatic.ContainsKey(key)) + { + stampedDictStatic.Remove(key); + stampedDictStatic.Add(key, t); + } + else + { + stampedDictStatic.Add(key, t); + } + + } + } + } //work later + + void Update() + { + //Update frame dictionary according to queue of transforms + Dictionary tfs = new Dictionary(); + lock (transforms) + { + while (transforms.Count > 0) + { + Messages.tf.tfMessage tm = transforms.Dequeue(); + + foreach (Messages.geometry_msgs.TransformStamped t in tm.transforms) + { + + double time = (double)t.header.stamp.data.sec + ((double)t.header.stamp.data.nsec / SEC_TO_NSEC); + string key = t.header.frame_id + t.child_frame_id; + lock (stampedDict) + { + + if (!stampedDict.ContainsKey(key)) + { + SortedList dict = new SortedList(); + dict.Add(time, t); + stampedDict.Add(key, dict); + StartCoroutine(RemoveTransform(key, time)); + + } + else + { + if (!stampedDict[key].ContainsKey(time)) + { + stampedDict[key].Add(time, t); + StartCoroutine(RemoveTransform(key, time)); + } + + } + } + + } + } + } + + double nsecs = (double)ROS.GetTime().data.sec + ((double)ROS.GetTime().data.nsec / SEC_TO_NSEC) - seconds; + + Messages.geometry_msgs.TransformStamped m = getTransformStamped(link, child,nsecs); + if(m != null) + { + //Debug.Log("{" + m.transform.translation.x + "," + m.transform.translation.y + "," + m.transform.translation.z + "}"); + //Debug.Log(" !" + m.header.stamp.data.sec + "." + m.header.stamp.data.nsec); + + m.child_frame_id = child + "_delayed"; + m.Serialized = null; + + + Messages.tf.tfMessage tfPub = new Messages.tf.tfMessage(); + tfPub.transforms = new Messages.geometry_msgs.TransformStamped[1]; + tfPub.transforms[0] = m; + pub.publish(tfPub); + } + + + } + + public Messages.geometry_msgs.TransformStamped getTransformStamped(string baseFrame, string childFrame, double nTime) + { + string key = baseFrame + childFrame; + + if (!stampedDict.ContainsKey(key)) + { + Debug.Log("No records of stamped transforms between these two frames found."); + return null; + } + else + { + if(nTime == 0) //Return latest stamped transform + { + int i = stampedDict[key].Count; + return stampedDict[key].Values[i-1]; + } + + + if (stampedDict[key].ContainsKey(nTime)) //You got the exact time, thats crazy + { + return stampedDict[key][nTime]; + } + else + { + int numKeys = stampedDict[key].Count; + if (stampedDict[key].Keys[0] > nTime || stampedDict[key].Keys[numKeys - 1] < nTime) //Time is ahead or before any records + { + Debug.Log("The time you are trying to access is either ahead of our records or before"); + return null; + } + else + { + if (numKeys == 1)// One value in this key + { + return stampedDict[key][0]; + } + else if (numKeys > 1)// More than one value + { + int min = 0, max = numKeys; + while (max - min >= 2) + { + int mid = (max + min) / 2; + if(stampedDict[key].Keys[mid] > nTime) + { + max = mid; + } + else + { + min = mid; + } + } + + double toReturnKey = stampedDict[key].Keys[(max + min) / 2]; + //Debug.Log(toReturnKey + " " + nTime); + return stampedDict[key][toReturnKey]; + } + + + } + } + } + return null; + } + + + public Messages.geometry_msgs.TransformStamped getTransformStampedStatic(string baseFrame, string childFrame) + { + string key = baseFrame + childFrame; + + lock (stampedDictStatic) + { + if (stampedDictStatic.ContainsKey(key)) + { + //Debug.Log(key + " found!"); + //Debug.Log(stampedDictStatic[key].transform.translation.x); + return stampedDictStatic[key]; + } + else + { + //Debug.Log("Static Transform log of " + key + " was not found"); + return null; + } + } + } + IEnumerator RemoveTransform(string frameskey, double nTime) + { + //Remove stored transform after designated time + yield return new WaitForSeconds(storageTime); + + stampedDict[frameskey].Remove(nTime); + //Debug.Log(frameskey + time + " removed!"); + } +} + + + diff --git a/Scripts/TF/TFSaver_Rework.cs.meta b/Scripts/TF/TFSaver_Rework.cs.meta new file mode 100644 index 0000000..c22a33c --- /dev/null +++ b/Scripts/TF/TFSaver_Rework.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 34c946311e8959243b6b382f71b94292 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From 26efe8c83cea414d7c0afcfff73ba4f6309ca6f3 Mon Sep 17 00:00:00 2001 From: Jacob Epstein Date: Fri, 15 Jul 2022 15:17:42 -0400 Subject: [PATCH 02/10] Fixed small bugs with tfsaver - Everything breaks when there are no tfs for a certain key - locked the dictionary in the getTransformStamped function --- Scripts/TF/TFSaver_Rework.cs | 104 ++++++++++++++++++----------------- 1 file changed, 54 insertions(+), 50 deletions(-) diff --git a/Scripts/TF/TFSaver_Rework.cs b/Scripts/TF/TFSaver_Rework.cs index a602cf5..a5e779b 100644 --- a/Scripts/TF/TFSaver_Rework.cs +++ b/Scripts/TF/TFSaver_Rework.cs @@ -27,7 +27,7 @@ public class TFSaver_Rework : MonoBehaviour Publisher pub; //Test Stuff - public uint time = 10, storageTime; + public uint storageTime = 10; public string link, child; public double seconds; @@ -38,9 +38,9 @@ void Start() tfstaticsub = nh.subscribe("/tf_static", 0, tf_static_callback); tfsub = nh.subscribe("/tf", 0, tf_callback); pub = nh.advertise("/tf", 10); - } + } - private void tf_callback(tfMessage msg) + private void tf_callback(tfMessage msg) { //Queues recieved transforms to be added to the dictionary lock (transforms) @@ -94,6 +94,7 @@ void Update() SortedList dict = new SortedList(); dict.Add(time, t); stampedDict.Add(key, dict); + Debug.Log("[TFCache]: adding key " + key + " to the tf dictionary"); StartCoroutine(RemoveTransform(key, time)); } @@ -113,13 +114,15 @@ void Update() } double nsecs = (double)ROS.GetTime().data.sec + ((double)ROS.GetTime().data.nsec / SEC_TO_NSEC) - seconds; - - Messages.geometry_msgs.TransformStamped m = getTransformStamped(link, child,nsecs); - if(m != null) - { + if (nsecs < 0) { Debug.Log("[TFCache]: Haven't waited enough yet I guess"); return; } + Messages.geometry_msgs.TransformStamped m = getTransformStamped(link, child, nsecs); + if (m != null) + { //Debug.Log("{" + m.transform.translation.x + "," + m.transform.translation.y + "," + m.transform.translation.z + "}"); //Debug.Log(" !" + m.header.stamp.data.sec + "." + m.header.stamp.data.nsec); - + //m.header = new Messages.std_msgs.Header(); + //m.header.frame_id = link; + //m.header.stamp = ROS.GetTime(); m.child_frame_id = child + "_delayed"; m.Serialized = null; @@ -128,72 +131,73 @@ void Update() tfPub.transforms = new Messages.geometry_msgs.TransformStamped[1]; tfPub.transforms[0] = m; pub.publish(tfPub); - } - - + } } public Messages.geometry_msgs.TransformStamped getTransformStamped(string baseFrame, string childFrame, double nTime) { string key = baseFrame + childFrame; - - if (!stampedDict.ContainsKey(key)) + lock (stampedDict) { - Debug.Log("No records of stamped transforms between these two frames found."); - return null; - } - else - { - if(nTime == 0) //Return latest stamped transform - { - int i = stampedDict[key].Count; - return stampedDict[key].Values[i-1]; - } - - - if (stampedDict[key].ContainsKey(nTime)) //You got the exact time, thats crazy + if (!stampedDict.ContainsKey(key)) { - return stampedDict[key][nTime]; + Debug.Log("No records of stamped transforms between these two frames found."); + return null; } else { - int numKeys = stampedDict[key].Count; - if (stampedDict[key].Keys[0] > nTime || stampedDict[key].Keys[numKeys - 1] < nTime) //Time is ahead or before any records + if (nTime == 0) //Return latest stamped transform + { + int i = stampedDict[key].Count; + return stampedDict[key].Values[i - 1]; + } + + + if (stampedDict[key].ContainsKey(nTime)) //You got the exact time, thats crazy { - Debug.Log("The time you are trying to access is either ahead of our records or before"); - return null; + return stampedDict[key][nTime]; } else { - if (numKeys == 1)// One value in this key + int numKeys = stampedDict[key].Count; + if (numKeys == 0) { Debug.Log("[TFCache]: Currently no tfs for key " + key); return null; } + if (stampedDict[key].Keys[0] > nTime || stampedDict[key].Keys[numKeys - 1] < nTime) //Time is ahead or before any records { - return stampedDict[key][0]; + Debug.Log("The time you are trying to access is either ahead of our records or before"); + return null; } - else if (numKeys > 1)// More than one value + else { - int min = 0, max = numKeys; - while (max - min >= 2) + if (numKeys == 1)// One value in this key { - int mid = (max + min) / 2; - if(stampedDict[key].Keys[mid] > nTime) - { - max = mid; - } - else - { - min = mid; - } + return stampedDict[key][0]; } + else if (numKeys > 1)// More than one value + { + int min = 0, max = numKeys; + while (max - min >= 2) + { + int mid = (max + min) / 2; + if (stampedDict[key].Keys[mid] > nTime) + { + max = mid; + } + else + { + min = mid; + } + } - double toReturnKey = stampedDict[key].Keys[(max + min) / 2]; - //Debug.Log(toReturnKey + " " + nTime); - return stampedDict[key][toReturnKey]; - } + double toReturnKey = stampedDict[key].Keys[(max + min) / 2]; + //Debug.Log(toReturnKey + " " + nTime); + return stampedDict[key][toReturnKey]; + } + } } } - } + } return null; } From 2e3712184234c1ea3eedcb6ab46c7a0c44a4e334 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Mon, 1 Aug 2022 16:05:23 -0400 Subject: [PATCH 03/10] TfSaver_V2 added, TfVisualizer checkboxes added, service client script added Included TfSaver_V2.cs for unparallel transforms. TfVisualizer updated to have check boxes in inspector menu to toggle frame visibility Included Service Tutorial script i made a while back --- Scripts/TF/TfSaver_V2.cs | 514 ++++++++++++++++++ Scripts/TF/TfSaver_V2.cs.meta | 11 + Scripts/TF/TfVisualizer.cs | 9 +- Scripts/Tutorials/TutorialServiceClient.cs | 38 ++ Scripts/Tutorials/TutorialServiceServer.cs | 29 + .../Tutorials/TutorialServiceServer.cs.meta | 11 + 6 files changed, 611 insertions(+), 1 deletion(-) create mode 100644 Scripts/TF/TfSaver_V2.cs create mode 100644 Scripts/TF/TfSaver_V2.cs.meta create mode 100644 Scripts/Tutorials/TutorialServiceClient.cs create mode 100644 Scripts/Tutorials/TutorialServiceServer.cs create mode 100644 Scripts/Tutorials/TutorialServiceServer.cs.meta diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs new file mode 100644 index 0000000..c9c0b49 --- /dev/null +++ b/Scripts/TF/TfSaver_V2.cs @@ -0,0 +1,514 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using Messages.geometry_msgs; +using Messages.tf; +using tf.net; +using Ros_CSharp; +using System; +public class TfSaver_V2 : MonoBehaviour +{ + /* NOTES + * - The subset of dictionaries dont remove themselves, while they may end up empty I dont know if this + * would cause problems in situations with many different frame interactions + * + * + * + */ + public ROSCore rosmaster; + private NodeHandle nh = null; + private Subscriber tfsub, tfstaticsub; + private Queue transforms = new Queue(); + BinaryTree tree; + Dictionary stampedDictStatic = new Dictionary(); + + const double SEC_TO_NSEC = 1000000000f; + + Publisher pub; + + public string upperFrameId; + + //Test Stuff + public uint storageTime = 10; + public string link, child; + public double seconds; + + void Start() + { + nh = rosmaster.getNodeHandle(); + + tfstaticsub = nh.subscribe("/tf_static", 0, tf_static_callback); + tfsub = nh.subscribe("/tf", 0, tf_callback); + pub = nh.advertise("/tf", 10); + + tree = new BinaryTree(upperFrameId); + //StartCoroutine(print()); + } + + private void tf_callback(tfMessage msg) + { + //Queues recieved transforms to be added to the dictionary + lock (transforms) + { + transforms.Enqueue(msg); + } + } + + private void tf_static_callback(tfMessage msg) + { //update existing statics - todo + lock (stampedDictStatic) + { + foreach (Messages.geometry_msgs.TransformStamped t in msg.transforms) + { + string key = t.header.frame_id + t.child_frame_id; + Debug.Log("found " + key); + if (stampedDictStatic.ContainsKey(key)) + { + stampedDictStatic.Remove(key); + stampedDictStatic.Add(key, t); + } + else + { + stampedDictStatic.Add(key, t); + } + + } + } + } //work later + + void Update() + { + //Update frame dictionary according to queue of transforms + Dictionary tfs = new Dictionary(); + lock (transforms) + { + while (transforms.Count > 0) + { + Messages.tf.tfMessage tm = transforms.Dequeue(); + + foreach (Messages.geometry_msgs.TransformStamped t in tm.transforms) + { + + tree.AddTransform(t); + StartCoroutine(Remove(t)); + } + } + } + + double nsecs = (double)ROS.GetTime().data.sec + ((double)ROS.GetTime().data.nsec / SEC_TO_NSEC) - seconds; + if (nsecs < 0) { Debug.Log("[TFCache]: Haven't waited enough yet I guess"); return; } + Messages.geometry_msgs.TransformStamped m = tree.GetTransform(link, child, nsecs); + if (m != null) + { + //Debug.Log("{" + m.transform.translation.x + "," + m.transform.translation.y + "," + m.transform.translation.z + "}"); + //Debug.Log(" !" + m.header.stamp.data.sec + "." + m.header.stamp.data.nsec); + //m.header = new Messages.std_msgs.Header(); + //m.header.frame_id = link; + //m.header.stamp = ROS.GetTime(); + m.child_frame_id = child + "_delayed"; + m.Serialized = null; + + + Messages.tf.tfMessage tfPub = new Messages.tf.tfMessage(); + tfPub.transforms = new Messages.geometry_msgs.TransformStamped[1]; + tfPub.transforms[0] = m; + pub.publish(tfPub); + } + } + + + + public Messages.geometry_msgs.TransformStamped getTransformStampedStatic(string baseFrame, string childFrame) + { + string key = baseFrame + childFrame; + + lock (stampedDictStatic) + { + if (stampedDictStatic.ContainsKey(key)) + { + //Debug.Log(key + " found!"); + //Debug.Log(stampedDictStatic[key].transform.translation.x); + return stampedDictStatic[key]; + } + else + { + //Debug.Log("Static Transform log of " + key + " was not found"); + return null; + } + } + } + IEnumerator Remove(TransformStamped ts) + { + //Remove stored transform after designated time + yield return new WaitForSeconds(storageTime); + + tree.RemoveTransform(ts); + //Debug.Log(frameskey + time + " removed!"); + } + + IEnumerator print() + { + yield return new WaitForSeconds(4); + tree.PrintTree(); + StartCoroutine(print()); + } +} + + + +class Node +{ + public string frame_id; + public string parent_id; + public SortedList frames; + public Dictionary children; + + public Node(string id,string parentId) + { + this.frame_id = id; + this.parent_id = parentId; + this.frames = new SortedList(); + this.children = new Dictionary(); + } +} + +class BinaryTree +{ + private const double SEC_TO_NSEC = 1000000000f; + public Node root { get; set; } + + public BinaryTree() + { + this.root = null; + } + + public BinaryTree(Node n) + { + this.root = n; + } + + public BinaryTree(string top_frame_id) + { + Node r = new Node(top_frame_id,null); + this.root = r; + + } + + public Node Find(Node n, string id) + { + //Anytime this function is used please becareful of null returns! + Queue q = new Queue(); + List visited = new List(); + + //Bfs + q.Enqueue(n); + while (q.Count > 0) + { + + Node currNode = q.Dequeue(); + visited.Add(currNode); + + if(currNode.frame_id == id) + { + return currNode; + } + + foreach (KeyValuePair childEntry in currNode.children) + { + if (!visited.Contains(childEntry.Value)) + { + q.Enqueue(childEntry.Value); + } + } + } + + //Debug.Log("Could not find " + id); + return null; + } + //Mainly for testing reasons + public void PrintTree() + { + Queue q = new Queue(); + List visited = new List(); + string tree = ""; + //Bfs + q.Enqueue(this.root); + while (q.Count > 0) + { + + Node currNode = q.Dequeue(); + visited.Add(currNode); + + tree += currNode.frame_id + " "; + foreach (KeyValuePair childEntry in currNode.children) + { + if (!visited.Contains(childEntry.Value)) + { + q.Enqueue(childEntry.Value); + } + } + } + + Debug.Log(tree); + } + + private TransformStamped SearchFramesForTransform(Node n, double time) + { + SortedList stampedDict = n.frames; + + if (stampedDict.ContainsKey(time)) //You got the exact time, thats crazy + { + return stampedDict[time]; + } + else + { + if (time == 0) //Return latest stamped transform + { + int i = stampedDict.Count; + return stampedDict.Values[i - 1]; + } + + int numKeys = stampedDict.Count; + if (numKeys == 0) { Debug.Log("[TFCache]: Currently no tfs for key " + n.frame_id); return null; } + if (stampedDict.Keys[0] > time || stampedDict.Keys[numKeys - 1] < time) //Time is ahead or before any records + { + Debug.Log("The time you are trying to access is either ahead of our records or before"); + return null; + } + else + { + if (numKeys == 1)// One value in this key + { + return stampedDict[0]; + } + else if (numKeys > 1)// More than one value + { + int min = 0, max = numKeys; + while (max - min >= 2) + { + int mid = (max + min) / 2; + if (stampedDict.Keys[mid] > time) + { + max = mid; + } + else + { + min = mid; + } + } + + double toReturnKey = stampedDict.Keys[(max + min) / 2]; + //Debug.Log(toReturnKey + " " + nTime); + return stampedDict[toReturnKey]; + } + + return null; + } + } + } + + + public TransformStamped GetTransform(string parent_id,string child_id,double time) + { + + Node childNode = Find(this.root, child_id); + Node parentNode = Find(this.root, parent_id); + + if(parentNode == null || childNode == null) + { + Debug.Log("Parent or Child not found"); + return null; + } + + if (parentNode.children.ContainsKey(child_id)) //No gap between frames + { + return SearchFramesForTransform(childNode, time); + } + else //Gap between frames :( + { + Debug.Log(parent_id + " " + child_id); + //Queue up parent nodes + Queue qNodes = new Queue(); + qNodes.Enqueue(childNode); + Node currNode = Find(parentNode, childNode.parent_id); + qNodes.Enqueue(currNode); + while(currNode.parent_id != parent_id) + { + currNode = Find(parentNode, currNode.parent_id); + if(currNode == null) + { + Debug.Log("Intermedian link " + currNode.parent_id + " not made yet"); + return null; + } + //Debug.Log(currNode.frame_id + " queued!"); + qNodes.Enqueue(currNode); + } + Debug.Log(qNodes.Count); + //Make a new transform + TransformStamped ts = new TransformStamped(); + ts.header = new Messages.std_msgs.Header(); + ts.header.stamp = new Messages.std_msgs.Time(); + ts.header.frame_id = parent_id; + ts.child_frame_id = child_id; + ts.header.stamp.data.sec = (uint)time; + //Queue of all transforms + Queue qTransforms = new Queue(); + while(qNodes.Count > 0) + { + Node currQNode = qNodes.Dequeue(); + TransformStamped e = SearchFramesForTransform(currQNode, time); + if(e == null) + { + Debug.Log("Transform not found, returning null INFO:\nID: " + currQNode.frame_id); + return null; + } + qTransforms.Enqueue(e); + + } + //Add up positions and rotationsfor new reference position + TransformStamped before = qTransforms.Dequeue(); + Debug.Log(qTransforms.Count); + Messages.geometry_msgs.Transform newTransform = new Messages.geometry_msgs.Transform(); + bool isFirst = true; + while(qTransforms.Count > 0) + { + TransformStamped after = qTransforms.Dequeue(); + if (isFirst) + { + //math with before and after + newTransform = AddTwoTransforms(after.transform, before.transform); + isFirst = false; + } + else + { + newTransform = AddTwoTransforms(after.transform, newTransform); + } + } + + //return :) + ts.transform = newTransform; + + return ts; + } + } + + private Messages.geometry_msgs.Transform AddTwoTransforms(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) + { + Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); + c.translation = new Messages.geometry_msgs.Vector3(); + c.translation.x = a.translation.x + b.translation.x; + c.translation.y = a.translation.y + b.translation.y; + c.translation.z = a.translation.z + b.translation.z; + + UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); + UnityEngine.Quaternion bQ= new UnityEngine.Quaternion(); + aQ.x = (float)a.rotation.x; + aQ.y = (float)a.rotation.y; + aQ.z = (float)a.rotation.z; + aQ.w = (float)a.rotation.w; + + bQ.x = (float)b.rotation.x; + bQ.y = (float)b.rotation.y; + bQ.z = (float)b.rotation.z; + bQ.w = (float)b.rotation.w; + + UnityEngine.Quaternion cQ = bQ * aQ; + + c.rotation = new Messages.geometry_msgs.Quaternion(); + + c.rotation.x = cQ.x; + c.rotation.y = cQ.y; + c.rotation.z = cQ.z; + c.rotation.w = cQ.w; + return c; + } + public void RemoveTransform(TransformStamped ts) + { + double time = (double)ts.header.stamp.data.sec + ((double)ts.header.stamp.data.nsec / SEC_TO_NSEC); + + if (this.root.frame_id == ts.header.frame_id) + { + if (this.root.children.ContainsKey(ts.child_frame_id)) + { + Node child = this.root.children[ts.child_frame_id]; + lock (child.frames) + { + if (child.frames.ContainsKey(time)) + { + child.frames.Remove(time); + //Debug.Log("removed"); + } + } + return; + } + } + + Node nodeWithChildId = Find(this.root, ts.child_frame_id); + + + if (!nodeWithChildId == null) + { + nodeWithChildId.frames.Remove(time); + //Debug.Log("removed"); + } + + } + public void AddTransform(TransformStamped ts) + { + double time = (double)ts.header.stamp.data.sec + ((double)ts.header.stamp.data.nsec / SEC_TO_NSEC); + + if(this.root.frame_id == ts.header.frame_id) + { + if (this.root.children.ContainsKey(ts.child_frame_id)) + { + Node child = this.root.children[ts.child_frame_id]; + lock (child.frames) + { + if (!child.frames.ContainsKey(time)) + { + child.frames.Add(time, ts); + } + } + + } + else + { + Node child = new Node(ts.child_frame_id,ts.header.frame_id); + child.frames.Add(time, ts); + this.root.children.Add(ts.child_frame_id, child); + } + return; + } + + + + Node nodeWithChildId = Find(this.root, ts.child_frame_id); + + if(nodeWithChildId == null) + { + Node parentNode = Find(this.root, ts.header.frame_id); + + Node child = new Node(ts.child_frame_id,ts.header.frame_id); + child.frames.Add(time, ts); + if(parentNode == null) + { + Debug.Log(ts.header.frame_id + " has not been logged yet to store + " + ts.child_frame_id); + return; + } + parentNode.children.Add(ts.child_frame_id, child); + + Debug.Log("Node created"); + } + else + { + if (!nodeWithChildId.frames.ContainsKey(time)) + { + nodeWithChildId.frames.Add(time, ts); + } + } + + } +} + + + diff --git a/Scripts/TF/TfSaver_V2.cs.meta b/Scripts/TF/TfSaver_V2.cs.meta new file mode 100644 index 0000000..d2c40fe --- /dev/null +++ b/Scripts/TF/TfSaver_V2.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 955ca029b4dfa53429f82fd815d3f4a9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Scripts/TF/TfVisualizer.cs b/Scripts/TF/TfVisualizer.cs index 860c6f1..c36c235 100644 --- a/Scripts/TF/TfVisualizer.cs +++ b/Scripts/TF/TfVisualizer.cs @@ -85,7 +85,10 @@ private void tf_callback(tfMessage msg) private bool IsVisible(string child_frame_id) { - //TODO rviz style checkboxes? + if (tree.ContainsKey(child_frame_id)) + { + return tree[child_frame_id].GetComponent().isVisible; + } return true; } @@ -181,6 +184,10 @@ void Update() tree[tf.child_frame_id].GetChild(0).localScale = new Vector3(axis_scale, axis_scale, axis_scale); } } + else + { + tree[tf.child_frame_id].gameObject.SetActive(false); + } } AxesHider.update(show_axes); diff --git a/Scripts/Tutorials/TutorialServiceClient.cs b/Scripts/Tutorials/TutorialServiceClient.cs new file mode 100644 index 0000000..4977c5f --- /dev/null +++ b/Scripts/Tutorials/TutorialServiceClient.cs @@ -0,0 +1,38 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using Ros_CSharp; +using Messages; + +public class TutorialServiceClient : MonoBehaviour { + + public ROSCore rosmaster; + private NodeHandle nh; + private ServiceClient test; + + + + // Use this for initialization + void Start () { + nh = rosmaster.getNodeHandle(); + + + test = nh.serviceClient("testservice"); + } + + // Update is called once per frame + void Update () { + + Messages.std_srvs.Trigger service_request = new Messages.std_srvs.Trigger(); + + if (test.call(service_request)) + { + Debug.Log(service_request.resp.message); + } + else + { + Debug.Log("Failed to the service"); + } + + } +} diff --git a/Scripts/Tutorials/TutorialServiceServer.cs b/Scripts/Tutorials/TutorialServiceServer.cs new file mode 100644 index 0000000..7d2b228 --- /dev/null +++ b/Scripts/Tutorials/TutorialServiceServer.cs @@ -0,0 +1,29 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using Ros_CSharp; +using Messages; + + +/// +/// NOT FINISHED :3 +/// +public class TutorialServiceServer : MonoBehaviour { + + public ROSCore rosmaster; + private NodeHandle nh = null; + + void Start () { + nh = rosmaster.getNodeHandle(); + + } + + void Update () { + //ServiceServer serv = nh.advertiseService("/test_service", TestServ); + } + + private void TestServ() + { + + } +} diff --git a/Scripts/Tutorials/TutorialServiceServer.cs.meta b/Scripts/Tutorials/TutorialServiceServer.cs.meta new file mode 100644 index 0000000..9aad5b0 --- /dev/null +++ b/Scripts/Tutorials/TutorialServiceServer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: fc98d7efd70292a40bbd1bb342060ed0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From ad1428f2229c3f5fdbacc3232ef37ed3082138d5 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Mon, 1 Aug 2022 16:28:10 -0400 Subject: [PATCH 04/10] Added isVisible, added parentheses in v2 scripts --- Scripts/TF/TfIsVisible.cs | 8 ++++++++ Scripts/TF/TfIsVisible.cs.meta | 11 +++++++++++ Scripts/TF/TfSaver_V2.cs | 2 +- 3 files changed, 20 insertions(+), 1 deletion(-) create mode 100644 Scripts/TF/TfIsVisible.cs create mode 100644 Scripts/TF/TfIsVisible.cs.meta diff --git a/Scripts/TF/TfIsVisible.cs b/Scripts/TF/TfIsVisible.cs new file mode 100644 index 0000000..96ecdae --- /dev/null +++ b/Scripts/TF/TfIsVisible.cs @@ -0,0 +1,8 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +public class TfIsVisible : MonoBehaviour { + + public bool isVisible = true; +} diff --git a/Scripts/TF/TfIsVisible.cs.meta b/Scripts/TF/TfIsVisible.cs.meta new file mode 100644 index 0000000..7ee8f7e --- /dev/null +++ b/Scripts/TF/TfIsVisible.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 371f3ae405f707a4db57853e6ea09e03 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index c9c0b49..11e7ca6 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -446,7 +446,7 @@ public void RemoveTransform(TransformStamped ts) Node nodeWithChildId = Find(this.root, ts.child_frame_id); - if (!nodeWithChildId == null) + if (!(nodeWithChildId == null)) { nodeWithChildId.frames.Remove(time); //Debug.Log("removed"); From 4a6ce90df0373b4224b01904a98c25636e6899e3 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Tue, 2 Aug 2022 14:51:00 -0400 Subject: [PATCH 05/10] Reverse saving included, no guarantees it works though --- Scripts/TF/TfSaver_V2.cs | 246 ++++++++++++++++++++++++++++++++------- 1 file changed, 202 insertions(+), 44 deletions(-) diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index 11e7ca6..aca9965 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -19,6 +19,7 @@ public class TfSaver_V2 : MonoBehaviour private NodeHandle nh = null; private Subscriber tfsub, tfstaticsub; private Queue transforms = new Queue(); + private Queue static_transforms = new Queue(); BinaryTree tree; Dictionary stampedDictStatic = new Dictionary(); @@ -32,6 +33,7 @@ public class TfSaver_V2 : MonoBehaviour public uint storageTime = 10; public string link, child; public double seconds; + public bool getReverse; void Start() { @@ -56,24 +58,10 @@ private void tf_callback(tfMessage msg) private void tf_static_callback(tfMessage msg) { //update existing statics - todo - lock (stampedDictStatic) - { - foreach (Messages.geometry_msgs.TransformStamped t in msg.transforms) - { - string key = t.header.frame_id + t.child_frame_id; - Debug.Log("found " + key); - if (stampedDictStatic.ContainsKey(key)) - { - stampedDictStatic.Remove(key); - stampedDictStatic.Add(key, t); - } - else - { - stampedDictStatic.Add(key, t); - } - - } - } + lock (static_transforms) + { + static_transforms.Enqueue(msg); + } } //work later void Update() @@ -95,9 +83,32 @@ void Update() } } + //Static queue + lock (static_transforms) + { + while(static_transforms.Count > 0){ + Messages.tf.tfMessage tm = static_transforms.Dequeue(); + + foreach (Messages.geometry_msgs.TransformStamped t in tm.transforms) + { + + tree.AddTransform(t); + StartCoroutine(Remove(t)); + } + } + } + double nsecs = (double)ROS.GetTime().data.sec + ((double)ROS.GetTime().data.nsec / SEC_TO_NSEC) - seconds; if (nsecs < 0) { Debug.Log("[TFCache]: Haven't waited enough yet I guess"); return; } - Messages.geometry_msgs.TransformStamped m = tree.GetTransform(link, child, nsecs); + Messages.geometry_msgs.TransformStamped m; + if (!getReverse) + { + m = tree.GetTransform(link, child, nsecs); + } + else + { + m = tree.GetTransformReverse(link, child, nsecs); + } if (m != null) { //Debug.Log("{" + m.transform.translation.x + "," + m.transform.translation.y + "," + m.transform.translation.z + "}"); @@ -105,7 +116,7 @@ void Update() //m.header = new Messages.std_msgs.Header(); //m.header.frame_id = link; //m.header.stamp = ROS.GetTime(); - m.child_frame_id = child + "_delayed"; + m.child_frame_id = m.child_frame_id + "_delayed"; m.Serialized = null; @@ -116,27 +127,7 @@ void Update() } } - - - public Messages.geometry_msgs.TransformStamped getTransformStampedStatic(string baseFrame, string childFrame) - { - string key = baseFrame + childFrame; - lock (stampedDictStatic) - { - if (stampedDictStatic.ContainsKey(key)) - { - //Debug.Log(key + " found!"); - //Debug.Log(stampedDictStatic[key].transform.translation.x); - return stampedDictStatic[key]; - } - else - { - //Debug.Log("Static Transform log of " + key + " was not found"); - return null; - } - } - } IEnumerator Remove(TransformStamped ts) { //Remove stored transform after designated time @@ -349,7 +340,7 @@ public TransformStamped GetTransform(string parent_id,string child_id,double tim ts.header = new Messages.std_msgs.Header(); ts.header.stamp = new Messages.std_msgs.Time(); ts.header.frame_id = parent_id; - ts.child_frame_id = child_id; + ts.child_frame_id = child_id; ts.header.stamp.data.sec = (uint)time; //Queue of all transforms Queue qTransforms = new Queue(); @@ -365,10 +356,12 @@ public TransformStamped GetTransform(string parent_id,string child_id,double tim qTransforms.Enqueue(e); } - //Add up positions and rotationsfor new reference position + //Add up positions and rotations for new reference position TransformStamped before = qTransforms.Dequeue(); Debug.Log(qTransforms.Count); Messages.geometry_msgs.Transform newTransform = new Messages.geometry_msgs.Transform(); + newTransform.translation = new Messages.geometry_msgs.Vector3(); + newTransform.rotation = new Messages.geometry_msgs.Quaternion(); bool isFirst = true; while(qTransforms.Count > 0) { @@ -376,13 +369,137 @@ public TransformStamped GetTransform(string parent_id,string child_id,double tim if (isFirst) { //math with before and after + newTransform = AddTwoTransforms(after.transform, before.transform); + isFirst = false; } else + { + newTransform = AddTwoTransforms(after.transform, before.transform); + } + } + + //return :) + ts.transform = newTransform; + + return ts; + } + } + public TransformStamped GetTransformReverse(string parent_id,string child_id,double time) + { + + Node childNode = Find(this.root, child_id); + Node parentNode = Find(this.root, parent_id); + + if(parentNode == null || childNode == null) + { + Debug.Log("Parent or Child not found"); + return null; + } + + if (parentNode.children.ContainsKey(child_id)) //No gap between frames + { + TransformStamped tf = SearchFramesForTransform(childNode, time); + if(tf == null) + { + return null; + } + tf.transform.translation.x *= -1; + tf.transform.translation.y *= -1; + tf.transform.translation.z *= -1; + tf.child_frame_id = parent_id; + tf.header.frame_id = child_id; + + return tf; + } + else //Gap between frames :( + { + Debug.Log(parent_id + " " + child_id); + //Queue up parent nodes + Queue qNodes = new Queue(); + Node currNode = Find(parentNode, childNode.parent_id); + qNodes.Enqueue(currNode); + while(currNode.parent_id != parent_id) + { + currNode = Find(parentNode, currNode.parent_id); + if(currNode == null) { - newTransform = AddTwoTransforms(after.transform, newTransform); + Debug.Log("Intermedian link " + currNode.parent_id + " not made yet"); + return null; + } + //Debug.Log(currNode.frame_id + " queued!"); + qNodes.Enqueue(currNode); + } + //Reverse the queue + Stack sNodes = new Stack(); + sNodes.Push(parentNode); + while(!(qNodes.Count > 0)) + { + sNodes.Push(qNodes.Dequeue()); + } + Debug.Log(sNodes.Count); + //Make a new transform + TransformStamped ts = new TransformStamped(); + ts.header = new Messages.std_msgs.Header(); + ts.header.stamp = new Messages.std_msgs.Time(); + ts.header.frame_id = child_id; + ts.child_frame_id = parent_id; + ts.header.stamp.data.sec = (uint)time; + //Queue of all transforms + Queue qTransforms = new Queue(); + while(sNodes.Count > 0) + { + Node currQNode = sNodes.Pop(); + TransformStamped e = SearchFramesForTransform(currQNode, time); + if(e == null) + { + if (currQNode.frame_id == this.root.frame_id) + { + e = new TransformStamped(); + e.transform = new Messages.geometry_msgs.Transform(); + e.transform.translation = new Messages.geometry_msgs.Vector3(); + e.transform.rotation = new Messages.geometry_msgs.Quaternion(); + e.transform.translation.x = 0; + e.transform.translation.y = 0; + e.transform.translation.z = 0; + + e.transform.rotation.x = 0; + e.transform.rotation.y = 0; + e.transform.rotation.z = 0; + e.transform.rotation.w = 0; + } + else + { + Debug.Log("Transform not found, returning null INFO:\nID: " + currQNode.frame_id); + return null; + } + } + qTransforms.Enqueue(e); + + } + //Add up positions and rotations for new reference position + TransformStamped before = qTransforms.Dequeue(); + Debug.Log(qTransforms.Count); + Messages.geometry_msgs.Transform newTransform = new Messages.geometry_msgs.Transform(); + newTransform.translation = new Messages.geometry_msgs.Vector3(); + newTransform.rotation = new Messages.geometry_msgs.Quaternion(); + bool isFirst = true; + while(qTransforms.Count > 0) + { + TransformStamped after = qTransforms.Dequeue(); + if (isFirst) + { + //math with before and after + + newTransform = AddTwoTransformsReverse(after.transform, before.transform); + + isFirst = false; } + else + { + newTransform = AddTwoTransformsReverse(after.transform, before.transform); + } } //return :) @@ -422,6 +539,42 @@ private Messages.geometry_msgs.Transform AddTwoTransforms(Messages.geometry_msgs c.rotation.w = cQ.w; return c; } + private Messages.geometry_msgs.Transform AddTwoTransformsReverse(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) + { + Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); + c.translation = new Messages.geometry_msgs.Vector3(); + Debug.Log("a " + (a.translation == null).ToString()); + Debug.Log("b " + (b.translation == null).ToString()); + Debug.Log("c " + (c.translation == null).ToString()); + + c.translation.x = (a.translation.x + b.translation.x) * -1; + c.translation.y = (a.translation.y + b.translation.y) * -1; + c.translation.z = (a.translation.z + b.translation.z) * -1; + + UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); + UnityEngine.Quaternion bQ= new UnityEngine.Quaternion(); + aQ.x = (float)a.rotation.x; + aQ.y = (float)a.rotation.y; + aQ.z = (float)a.rotation.z; + aQ.w = (float)a.rotation.w; + + bQ.x = (float)b.rotation.x; + bQ.y = (float)b.rotation.y; + bQ.z = (float)b.rotation.z; + bQ.w = (float)b.rotation.w; + + UnityEngine.Quaternion cQ = aQ * bQ; + + c.rotation = new Messages.geometry_msgs.Quaternion(); + + c.rotation.x = cQ.x; + c.rotation.y = cQ.y; + c.rotation.z = cQ.z; + c.rotation.w = cQ.w; + return c; + } + + public void RemoveTransform(TransformStamped ts) { double time = (double)ts.header.stamp.data.sec + ((double)ts.header.stamp.data.nsec / SEC_TO_NSEC); @@ -504,7 +657,12 @@ public void AddTransform(TransformStamped ts) if (!nodeWithChildId.frames.ContainsKey(time)) { nodeWithChildId.frames.Add(time, ts); - } + } + else + { + nodeWithChildId.frames.Remove(time); + nodeWithChildId.frames.Add(time, ts); + } } } From 7099c736cb2b32f7b5bfbc86b717b3a050e47956 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Tue, 2 Aug 2022 16:24:20 -0400 Subject: [PATCH 06/10] Overload to GetTransform that takes ros time instead of a double --- Scripts/TF/TfSaver_V2.cs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index aca9965..73502ee 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -298,7 +298,14 @@ private TransformStamped SearchFramesForTransform(Node n, double time) } } - + public TransformStamped GetTransform(string parent_id, string child_id, Messages.std_msgs.Time time) + { + + double new_fortmat = (double)time.data.sec + ((double)time.data.nsec / SEC_TO_NSEC); + return GetTransform(parent_id, child_id, new_fortmat); + } + + public TransformStamped GetTransform(string parent_id,string child_id,double time) { From d806977dd8ea0aad6105b72a6bf92b65e5864c66 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Thu, 4 Aug 2022 12:38:15 -0400 Subject: [PATCH 07/10] isStatic added to node, should have fixed static frames. Also get frame v3 added, not entirely working though --- Scripts/TF/TfSaver_V2.cs | 397 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 379 insertions(+), 18 deletions(-) diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index 73502ee..7d2e12d 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -77,7 +77,7 @@ void Update() foreach (Messages.geometry_msgs.TransformStamped t in tm.transforms) { - tree.AddTransform(t); + tree.AddTransform(t,false); StartCoroutine(Remove(t)); } } @@ -92,22 +92,24 @@ void Update() foreach (Messages.geometry_msgs.TransformStamped t in tm.transforms) { - tree.AddTransform(t); + tree.AddTransform(t,true); StartCoroutine(Remove(t)); } } } + //tree.GetPathOfTwoNodes("/hat", "/robert"); + double nsecs = (double)ROS.GetTime().data.sec + ((double)ROS.GetTime().data.nsec / SEC_TO_NSEC) - seconds; if (nsecs < 0) { Debug.Log("[TFCache]: Haven't waited enough yet I guess"); return; } Messages.geometry_msgs.TransformStamped m; if (!getReverse) { - m = tree.GetTransform(link, child, nsecs); + m = tree.GetTransformV3(link, child, nsecs); } else { - m = tree.GetTransformReverse(link, child, nsecs); + m = tree.GetTransformV3(link, child, nsecs); } if (m != null) { @@ -116,7 +118,7 @@ void Update() //m.header = new Messages.std_msgs.Header(); //m.header.frame_id = link; //m.header.stamp = ROS.GetTime(); - m.child_frame_id = m.child_frame_id + "_delayed"; + m.child_frame_id = child + "_delayed"; m.Serialized = null; @@ -151,15 +153,17 @@ class Node { public string frame_id; public string parent_id; + public bool isStatic; public SortedList frames; public Dictionary children; - public Node(string id,string parentId) + public Node(string id,string parentId,bool isstatic) { this.frame_id = id; this.parent_id = parentId; this.frames = new SortedList(); this.children = new Dictionary(); + this.isStatic = isstatic; } } @@ -180,7 +184,7 @@ public BinaryTree(Node n) public BinaryTree(string top_frame_id) { - Node r = new Node(top_frame_id,null); + Node r = new Node(top_frame_id,null,true); this.root = r; } @@ -216,6 +220,322 @@ public Node Find(Node n, string id) //Debug.Log("Could not find " + id); return null; } + //New Algorithm Shtuffs - Very unfinished, very much doesnt work,pathing does, math doesnt :( + public List GetPathFromRoot(string id) + { + Queue q = new Queue(); + List visited = new List(); + + //Get List of Entire Bfs + q.Enqueue(this.root); + while (q.Count > 0) + { + + Node currNode = q.Dequeue(); + visited.Add(currNode); + + if(currNode.frame_id == id) + { + break; + } + + foreach (KeyValuePair childEntry in currNode.children) + { + if (!visited.Contains(childEntry.Value)) + { + q.Enqueue(childEntry.Value); + } + } + } + + Node currLevel = visited[0]; + Stack path = new Stack(); + for (int i = visited.Count - 1; i >= 0; i--) + { + if(visited[i].frame_id == id || visited[i].frame_id == currLevel.parent_id) + { + path.Push(visited[i]); + currLevel = visited[i]; + } + } + + if(path.Count == 0) + { + Debug.Log("Path empty"); + return null; + } + + List p = new List(); + + foreach (Node n in path) + { + p.Add(n); + } + //Debug.Log(p); + + string path_string = ""; + foreach(Node n in p) + { + path_string += n.frame_id + " "; + } + + Debug.Log(path_string); + + return p; + } + + public TransformStamped GetTransformV3(string parent_id, string child_id,double time) + { + Node childNode = Find(this.root, child_id); + Node parentNode = Find(this.root, parent_id); + + if (parentNode == null || childNode == null) + { + Debug.Log("Parent or Child not found"); + return null; + } + + if (parentNode.children.ContainsKey(child_id)) //No gap between frames + { + return SearchFramesForTransform(childNode, time); + } + + List path = GetPathOfTwoNodes(parent_id, child_id); + Queue pathQ = new Queue(); + + foreach(Node n in path) + { + pathQ.Enqueue(n); + } + + if(path == null) + { + return null; + } + + TransformStamped newTs = new TransformStamped(); + newTs.transform = new Messages.geometry_msgs.Transform(); + newTs.header = new Messages.std_msgs.Header(); + newTs.transform.translation = new Messages.geometry_msgs.Vector3(); + newTs.transform.rotation = new Messages.geometry_msgs.Quaternion(); + newTs.header.stamp = new Messages.std_msgs.Time(); + + newTs.header.frame_id = parent_id; + newTs.child_frame_id = child_id; + newTs.header.stamp.data.sec = (uint)time; + + + Node before = pathQ.Dequeue(); + TransformStamped e = SearchFramesForTransform(before, time); + if(e == null) + { + return null; + } + newTs.transform = e.transform; + + if(newTs.transform == null) + { + return null; + } + + //maybe print the q + + while (pathQ.Count > 1) + { + Debug.Log(pathQ.Count); + Node after = pathQ.Dequeue(); + + if(before.parent_id == after.frame_id) //if a is parent of b + { + TransformStamped a = SearchFramesForTransform(after, time); + if(a == null) + { + return null; + } + newTs.transform = AddTwoTransforms(a.transform,newTs.transform); + }else if(before.frame_id == after.parent_id)//if b is parent of a + { + TransformStamped a = SearchFramesForTransform(after, time); + if (a == null) + { + return null; + } + newTs.transform = AddTwoTransformsReverse(a.transform, newTs.transform); + } + else //same level + { + TransformStamped a = SearchFramesForTransform(after, time); + if (a == null) + { + return null; + } + newTs.transform = AddTwoTransformsSiblings(a.transform, newTs.transform); + } + + before = after; + } + + Debug.Log("we dont geet here do we?"); + return newTs; + } + + public List GetPathOfTwoNodes(string id_a, string id_b) + { + bool isDirectLine = false, isAUnderB = false; + string commonAncestorId = ""; + //Get Paths + List path_a = GetPathFromRoot(id_a); //paths are returned, dest->parent->parent->...->root + List path_b = GetPathFromRoot(id_b); + + + + if(path_a == null || path_b == null) + { + Debug.Log("Either of the two nodes doesnt exist"); + return null; + } + + //Find Common Ancestor id + + if(path_a.Count < path_b.Count || path_a.Count == path_b.Count) + { + for(int i = 0; i < path_a.Count; i++) + { + if (path_a[i].frame_id == path_b[i].frame_id) + { + commonAncestorId = path_a[i].frame_id; + continue; + } + break; + } + } + else + { + for (int i = 0; i < path_b.Count; i++) + { + if (path_a[i].frame_id == path_b[i].frame_id) + { + commonAncestorId = path_a[i].frame_id; + continue; + } + break; + } + } + + Debug.Log(commonAncestorId); + + //Direct Line Test + foreach (Node n in path_a) + { + if (n.frame_id == id_b) + { + isAUnderB = true; + isDirectLine = true; + } + } + + foreach(Node n in path_b) + { + if(n.frame_id == id_a) + { + isDirectLine = true; + } + } + + //Calculate New Path + List newPath = new List(); + + if (isDirectLine) + { + if(path_b.Count > path_a.Count) + { + bool reachedAncestor = false; + foreach(Node n in path_b) + { + if(n.frame_id == commonAncestorId && !reachedAncestor) + { + reachedAncestor = true; + } + + if (reachedAncestor) + { + newPath.Add(n); + } + } + } + else + { + bool reachedAncestor = false; + foreach (Node n in path_a) + { + if (n.frame_id == commonAncestorId && !reachedAncestor) + { + reachedAncestor = true; + } + + if (reachedAncestor) + { + newPath.Add(n); + } + } + } + } + else + { + //reverse go up first path till we hit common ancestor + //then add second path - common ancestor + bool reachedAncestor = false; + for (int i = path_a.Count - 1; i >= 0; i--) + { + if(path_a[i].frame_id == commonAncestorId && !reachedAncestor) + { + reachedAncestor = true; + } + + if (!reachedAncestor) + { + newPath.Add(path_a[i]); + } + } + + reachedAncestor = false; + foreach (Node n in path_b) + { + if (reachedAncestor) + { + newPath.Add(n); + } + + if (n.frame_id == commonAncestorId && !reachedAncestor) + { + reachedAncestor = true; + } + + } + + } + //Reverse Direct Line Tree if a is lower than b + if (isAUnderB && isDirectLine) + { + newPath.Reverse(); + + } + + //Debug + string newPathIds = ""; + + foreach(Node n in newPath) + { + newPathIds += n.frame_id + " "; + } + + + + Debug.Log("New Path: " + newPathIds); + + return newPath; + } + //Mainly for testing reasons public void PrintTree() { @@ -253,6 +573,10 @@ private TransformStamped SearchFramesForTransform(Node n, double time) } else { + if (n.isStatic) // just get the one frame stored there + { + return stampedDict[0]; + } if (time == 0) //Return latest stamped transform { int i = stampedDict.Count; @@ -318,6 +642,8 @@ public TransformStamped GetTransform(string parent_id,string child_id,double tim return null; } + + if (parentNode.children.ContainsKey(child_id)) //No gap between frames { return SearchFramesForTransform(childNode, time); @@ -445,7 +771,7 @@ public TransformStamped GetTransformReverse(string parent_id,string child_id,dou { sNodes.Push(qNodes.Dequeue()); } - Debug.Log(sNodes.Count); + //Debug.Log(sNodes.Count); //Make a new transform TransformStamped ts = new TransformStamped(); ts.header = new Messages.std_msgs.Header(); @@ -513,19 +839,50 @@ public TransformStamped GetTransformReverse(string parent_id,string child_id,dou ts.transform = newTransform; return ts; - } + } } - private Messages.geometry_msgs.Transform AddTwoTransforms(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) + private Messages.geometry_msgs.Transform AddTwoTransforms(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) { + Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); + c.translation = new Messages.geometry_msgs.Vector3(); + c.translation.x = a.translation.x + b.translation.x; + c.translation.y = a.translation.y + b.translation.y; + c.translation.z = a.translation.z + b.translation.z; + + UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); + UnityEngine.Quaternion bQ = new UnityEngine.Quaternion(); + aQ.x = (float)a.rotation.x; + aQ.y = (float)a.rotation.y; + aQ.z = (float)a.rotation.z; + aQ.w = (float)a.rotation.w; + + bQ.x = (float)b.rotation.x; + bQ.y = (float)b.rotation.y; + bQ.z = (float)b.rotation.z; + bQ.w = (float)b.rotation.w; + + UnityEngine.Quaternion cQ = bQ * aQ; + + c.rotation = new Messages.geometry_msgs.Quaternion(); + + c.rotation.x = cQ.x; + c.rotation.y = cQ.y; + c.rotation.z = cQ.z; + c.rotation.w = cQ.w; + return c; + } + + private Messages.geometry_msgs.Transform AddTwoTransformsSiblings(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) + { Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); c.translation = new Messages.geometry_msgs.Vector3(); - c.translation.x = a.translation.x + b.translation.x; - c.translation.y = a.translation.y + b.translation.y; - c.translation.z = a.translation.z + b.translation.z; + c.translation.x = b.translation.x - a.translation.x; + c.translation.y = b.translation.y - a.translation.y; + c.translation.z = b.translation.z - a.translation.z; UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); - UnityEngine.Quaternion bQ= new UnityEngine.Quaternion(); + UnityEngine.Quaternion bQ = new UnityEngine.Quaternion(); aQ.x = (float)a.rotation.x; aQ.y = (float)a.rotation.y; aQ.z = (float)a.rotation.z; @@ -545,7 +902,7 @@ private Messages.geometry_msgs.Transform AddTwoTransforms(Messages.geometry_msgs c.rotation.z = cQ.z; c.rotation.w = cQ.w; return c; - } + } private Messages.geometry_msgs.Transform AddTwoTransformsReverse(Messages.geometry_msgs.Transform a, Messages.geometry_msgs.Transform b) { Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); @@ -613,7 +970,7 @@ public void RemoveTransform(TransformStamped ts) } } - public void AddTransform(TransformStamped ts) + public void AddTransform(TransformStamped ts, bool isStat) { double time = (double)ts.header.stamp.data.sec + ((double)ts.header.stamp.data.nsec / SEC_TO_NSEC); @@ -633,7 +990,7 @@ public void AddTransform(TransformStamped ts) } else { - Node child = new Node(ts.child_frame_id,ts.header.frame_id); + Node child = new Node(ts.child_frame_id,ts.header.frame_id,isStat); child.frames.Add(time, ts); this.root.children.Add(ts.child_frame_id, child); } @@ -648,7 +1005,7 @@ public void AddTransform(TransformStamped ts) { Node parentNode = Find(this.root, ts.header.frame_id); - Node child = new Node(ts.child_frame_id,ts.header.frame_id); + Node child = new Node(ts.child_frame_id,ts.header.frame_id,isStat); child.frames.Add(time, ts); if(parentNode == null) { @@ -661,6 +1018,10 @@ public void AddTransform(TransformStamped ts) } else { + if (isStat) + { + nodeWithChildId.frames.Add(0, ts); + } if (!nodeWithChildId.frames.ContainsKey(time)) { nodeWithChildId.frames.Add(time, ts); From a7e50758c168229559aa25f4200951f62d1df4d8 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Thu, 4 Aug 2022 12:52:32 -0400 Subject: [PATCH 08/10] Changed to use right functions in update thread --- Scripts/TF/TfSaver_V2.cs | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index 7d2e12d..6dde088 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -105,11 +105,11 @@ void Update() Messages.geometry_msgs.TransformStamped m; if (!getReverse) { - m = tree.GetTransformV3(link, child, nsecs); + m = tree.GetTransform(link, child, nsecs); } else { - m = tree.GetTransformV3(link, child, nsecs); + m = tree.GetTransformReverse(link, child, nsecs); } if (m != null) { @@ -184,7 +184,7 @@ public BinaryTree(Node n) public BinaryTree(string top_frame_id) { - Node r = new Node(top_frame_id,null,true); + Node r = new Node(top_frame_id,null,false); this.root = r; } @@ -289,16 +289,7 @@ public TransformStamped GetTransformV3(string parent_id, string child_id,double Node childNode = Find(this.root, child_id); Node parentNode = Find(this.root, parent_id); - if (parentNode == null || childNode == null) - { - Debug.Log("Parent or Child not found"); - return null; - } - - if (parentNode.children.ContainsKey(child_id)) //No gap between frames - { - return SearchFramesForTransform(childNode, time); - } + List path = GetPathOfTwoNodes(parent_id, child_id); Queue pathQ = new Queue(); @@ -575,6 +566,7 @@ private TransformStamped SearchFramesForTransform(Node n, double time) { if (n.isStatic) // just get the one frame stored there { + Debug.Log(n.frame_id); return stampedDict[0]; } if (time == 0) //Return latest stamped transform From 9afede880e8b502fe4f22eb02dca7f65051a20e0 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Tue, 9 Aug 2022 14:27:37 -0400 Subject: [PATCH 09/10] The arbitrary frame search works completley. Edited TransformPublisher.cs to require a dummy game object to allow using local positions --- Scripts/TF/TfSaver_V2.cs | 194 +++++++++++++++++++++++++------ Scripts/TF/TransformPublisher.cs | 20 +++- 2 files changed, 179 insertions(+), 35 deletions(-) diff --git a/Scripts/TF/TfSaver_V2.cs b/Scripts/TF/TfSaver_V2.cs index 6dde088..6953c7d 100644 --- a/Scripts/TF/TfSaver_V2.cs +++ b/Scripts/TF/TfSaver_V2.cs @@ -6,6 +6,7 @@ using tf.net; using Ros_CSharp; using System; +using System.Linq; public class TfSaver_V2 : MonoBehaviour { /* NOTES @@ -93,7 +94,7 @@ void Update() { tree.AddTransform(t,true); - StartCoroutine(Remove(t)); + //StartCoroutine(Remove(t)); } } } @@ -105,11 +106,11 @@ void Update() Messages.geometry_msgs.TransformStamped m; if (!getReverse) { - m = tree.GetTransform(link, child, nsecs); + m = tree.GetTransformV3(link, child, nsecs); } else { - m = tree.GetTransformReverse(link, child, nsecs); + m = tree.GetTransformV3(link, child, nsecs); } if (m != null) { @@ -125,6 +126,8 @@ void Update() Messages.tf.tfMessage tfPub = new Messages.tf.tfMessage(); tfPub.transforms = new Messages.geometry_msgs.TransformStamped[1]; tfPub.transforms[0] = m; + string str = m.transform.translation.x + " " + m.transform.translation.y + " " + m.transform.translation.z + " "; + //Debug.Log(str); pub.publish(tfPub); } } @@ -279,31 +282,49 @@ public List GetPathFromRoot(string id) path_string += n.frame_id + " "; } - Debug.Log(path_string); return p; } + private bool checkDirectLine(List path) + { + if(path == null) + { + return false; + } + for(int i = 0; i < path.Count - 1; i++) + { + if(path[i].parent_id != path[i + 1].frame_id) + { + return false; + } + } + return true; + } + public TransformStamped GetTransformV3(string parent_id, string child_id,double time) { + //Get Path Node childNode = Find(this.root, child_id); Node parentNode = Find(this.root, parent_id); - - List path = GetPathOfTwoNodes(parent_id, child_id); Queue pathQ = new Queue(); - foreach(Node n in path) - { - pathQ.Enqueue(n); - } + bool isDirectLine = checkDirectLine(path); if(path == null) { return null; } + foreach(Node n in path) + { + pathQ.Enqueue(n); + } + + + //Setup new msg TransformStamped newTs = new TransformStamped(); newTs.transform = new Messages.geometry_msgs.Transform(); newTs.header = new Messages.std_msgs.Header(); @@ -314,59 +335,117 @@ public TransformStamped GetTransformV3(string parent_id, string child_id,double newTs.header.frame_id = parent_id; newTs.child_frame_id = child_id; newTs.header.stamp.data.sec = (uint)time; + + string q = ""; + foreach (Node n in pathQ) + { + q += n.frame_id + " "; + } + Debug.Log("Q: " + q); + //Debug.Log + //Reverse q for debug?? + //Queue pathQR = new Queue(pathQ.Reverse()); Node before = pathQ.Dequeue(); + + TransformStamped e = SearchFramesForTransform(before, time); - if(e == null) + if (before.frame_id == this.root.frame_id) + { + e = new TransformStamped(); + e.transform = new Messages.geometry_msgs.Transform(); + e.transform.translation = new Messages.geometry_msgs.Vector3(); + e.transform.rotation = new Messages.geometry_msgs.Quaternion(); + e.transform.translation.x = 0; + e.transform.translation.y = 0; + e.transform.translation.z = 0; + + e.transform.rotation.x = 0; + e.transform.rotation.y = 0; + e.transform.rotation.z = 0; + e.transform.rotation.w = 0; + + } + + if (e == null) { return null; } + newTs.transform = e.transform; if(newTs.transform == null) { return null; } - + bool directLine = false; //maybe print the q - - while (pathQ.Count > 1) + while (pathQ.Count > 0) { - Debug.Log(pathQ.Count); - Node after = pathQ.Dequeue(); + - if(before.parent_id == after.frame_id) //if a is parent of b + Node after = pathQ.Dequeue(); + //Debug.Log(before.frame_id); + if (before.frame_id == after.parent_id) //if a is parent of b { + //Debug.Log(before.frame_id + " is a parent of " + after.frame_id); + //Debug.Log(newTs.transform.translation.x + " " + newTs.transform.translation.y + " " + newTs.transform.translation.z + " "); + TransformStamped a = SearchFramesForTransform(after, time); + if(a == null) { return null; } + //Debug.Log(a.transform.translation.x + " " + a.transform.translation.y + " " + a.transform.translation.z + " "); newTs.transform = AddTwoTransforms(a.transform,newTs.transform); - }else if(before.frame_id == after.parent_id)//if b is parent of a + }else if(before.parent_id == after.frame_id && isDirectLine)//if b is parent of a { + //Debug.Log(before.frame_id + " is a child of " + after.frame_id); + //Debug.Log(newTs.transform.translation.x + " " + newTs.transform.translation.y + " " + newTs.transform.translation.z + " "); + TransformStamped par = SearchFramesForTransform(before, time); TransformStamped a = SearchFramesForTransform(after, time); - if (a == null) + //Debug.Log(a.transform.translation.x + " " + a.transform.translation.y + " " + a.transform.translation.z + " "); + if (a == null || par == null) { return null; } - newTs.transform = AddTwoTransformsReverse(a.transform, newTs.transform); + newTs.transform = AddTwoTransformsReverse(par.transform, newTs.transform); } - else //same level + else if (isDirectLine == false) //same level { + //Debug.Log(before.frame_id + " is a sibling of " + after.frame_id); + //Debug.Log(newTs.transform.translation.x + " " + newTs.transform.translation.y + " " + newTs.transform.translation.z + " "); TransformStamped a = SearchFramesForTransform(after, time); + //Debug.Log(a.transform.translation.x + " " + a.transform.translation.y + " " + a.transform.translation.z + " "); if (a == null) { return null; } newTs.transform = AddTwoTransformsSiblings(a.transform, newTs.transform); + } + else if (isDirectLine == true) + { + //Debug.Log(before.frame_id + " is a parent of " + after.frame_id); + //Debug.Log(newTs.transform.translation.x + " " + newTs.transform.translation.y + " " + newTs.transform.translation.z + " "); + + TransformStamped a = SearchFramesForTransform(after, time); + + if (a == null) + { + return null; + } + //Debug.Log(a.transform.translation.x + " " + a.transform.translation.y + " " + a.transform.translation.z + " "); + newTs.transform = AddTwoTransforms(a.transform, newTs.transform); + } + //Debug.Log("New:"); + //Debug.Log(newTs.transform.translation.x + " " + newTs.transform.translation.y + " " + newTs.transform.translation.z + " "); before = after; } - Debug.Log("we dont geet here do we?"); return newTs; } @@ -413,8 +492,6 @@ public List GetPathOfTwoNodes(string id_a, string id_b) } } - Debug.Log(commonAncestorId); - //Direct Line Test foreach (Node n in path_a) { @@ -522,7 +599,7 @@ public List GetPathOfTwoNodes(string id_a, string id_b) - Debug.Log("New Path: " + newPathIds); + //Debug.Log("New Path: " + newPathIds); return newPath; } @@ -569,6 +646,25 @@ private TransformStamped SearchFramesForTransform(Node n, double time) Debug.Log(n.frame_id); return stampedDict[0]; } + + if(n.frame_id == this.root.frame_id) + { + TransformStamped r = new TransformStamped(); + r.transform = new Messages.geometry_msgs.Transform(); + r.transform.translation = new Messages.geometry_msgs.Vector3(); + r.transform.rotation = new Messages.geometry_msgs.Quaternion(); + + r.transform.translation.x = 0; + r.transform.translation.y = 0; + r.transform.translation.z = 0; + + r.transform.rotation.x = 0; + r.transform.rotation.y = 0; + r.transform.rotation.z = 0; + r.transform.rotation.w = 0; + return r; + } + if (time == 0) //Return latest stamped transform { int i = stampedDict.Count; @@ -869,9 +965,35 @@ private Messages.geometry_msgs.Transform AddTwoTransformsSiblings(Messages.geome { Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); c.translation = new Messages.geometry_msgs.Vector3(); - c.translation.x = b.translation.x - a.translation.x; - c.translation.y = b.translation.y - a.translation.y; - c.translation.z = b.translation.z - a.translation.z; + c.translation.x = (a.translation.x - b.translation.x); + c.translation.y = (a.translation.y - b.translation.y); + c.translation.z = (a.translation.z - b.translation.z); + + /*UnityEngine.Vector3 cT = new UnityEngine.Vector3(); + UnityEngine.Vector3 aT= new UnityEngine.Vector3(); + UnityEngine.Vector3 bT = new UnityEngine.Vector3(); + + aT.x = (float)a.translation.x; + aT.y = (float)a.translation.y; + aT.z = (float)a.translation.z; + + bT.x = (float)b.translation.x; + bT.y = (float)b.translation.y; + bT.z = (float)b.translation.z; + + cT = bT - aT; + cT.Scale(new UnityEngine.Vector3(-1f,-1f,-1f));*? + //cT.x *= -1; + //cT.y *= -1; + //cT.z *= -1; + + Debug.Log(aT); + + c.translation.x = cT.x; + c.translation.y = cT.y; + c.translation.z = cT.z;*/ + + UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); UnityEngine.Quaternion bQ = new UnityEngine.Quaternion(); @@ -899,13 +1021,17 @@ private Messages.geometry_msgs.Transform AddTwoTransformsReverse(Messages.geomet { Messages.geometry_msgs.Transform c = new Messages.geometry_msgs.Transform(); c.translation = new Messages.geometry_msgs.Vector3(); - Debug.Log("a " + (a.translation == null).ToString()); - Debug.Log("b " + (b.translation == null).ToString()); - Debug.Log("c " + (c.translation == null).ToString()); + //Debug.Log("a " + (a.translation == null).ToString()); + //Debug.Log("b " + (b.translation == null).ToString()); + //Debug.Log("c " + (c.translation == null).ToString()); + + c.translation.x = a.translation.x + b.translation.x *-1; + c.translation.y = a.translation.y + b.translation.y * -1; + c.translation.z = a.translation.z + b.translation.z * -1; - c.translation.x = (a.translation.x + b.translation.x) * -1; - c.translation.y = (a.translation.y + b.translation.y) * -1; - c.translation.z = (a.translation.z + b.translation.z) * -1; + c.translation.x = -a.translation.x; + c.translation.y = -a.translation.y; + c.translation.z = -a.translation.z; UnityEngine.Quaternion aQ = new UnityEngine.Quaternion(); UnityEngine.Quaternion bQ= new UnityEngine.Quaternion(); diff --git a/Scripts/TF/TransformPublisher.cs b/Scripts/TF/TransformPublisher.cs index ef8c719..e264e5d 100644 --- a/Scripts/TF/TransformPublisher.cs +++ b/Scripts/TF/TransformPublisher.cs @@ -12,6 +12,8 @@ public class TransformPublisher : MonoBehaviour { private Publisher tfPub; public GameObject trackedObject; + public GameObject dummyObject; + public string frame_id; public string child_frame_id; @@ -32,9 +34,24 @@ void Update () { Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1]; arr[0] = new Messages.geometry_msgs.TransformStamped(); + tfmsg.transforms = arr; Transform trans = trackedObject.transform; - emTransform ta = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id); + Transform dummyTrans = dummyObject.transform; + + dummyTrans.transform.position = trans.transform.localPosition; + dummyTrans.transform.rotation = trans.transform.rotation; + emTransform ta; + if (child_frame_id != "/base_link") + { + + //Debug.Log(trans.localPosition.ToString()); + //Debug.Log(dummyTrans.localRotation.ToString()); + ta = new emTransform(dummyTrans, ROS.GetTime(), frame_id, child_frame_id); + } + else { ta = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id); } + + Messages.std_msgs.Header hdr = new Messages.std_msgs.Header(); hdr.frame_id = frame_id; @@ -51,6 +68,7 @@ void Update () { tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; + tfPub.publish(tfmsg); } } From d974dae45d98c1a780253854da513cf0bdc20231 Mon Sep 17 00:00:00 2001 From: DannyMacha25 <91552897+DannyMacha25@users.noreply.github.com> Date: Tue, 9 Aug 2022 14:30:06 -0400 Subject: [PATCH 10/10] Just put a note in the Tf Publisher script --- Scripts/TF/TransformPublisher.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Scripts/TF/TransformPublisher.cs b/Scripts/TF/TransformPublisher.cs index e264e5d..6ebe126 100644 --- a/Scripts/TF/TransformPublisher.cs +++ b/Scripts/TF/TransformPublisher.cs @@ -5,7 +5,8 @@ using tf.net; public class TransformPublisher : MonoBehaviour { - + //Be sure to keep any children of the main link out of a scene tree, but any further children + // must be actual children in the scene tree. Dont rlly know why, just kinda works that way public ROSCore rosmaster; private NodeHandle nh = null;