diff --git a/Scripts/TF/TFSaver_Rework.cs b/Scripts/TF/TFSaver_Rework.cs new file mode 100644 index 0000000..a5e779b --- /dev/null +++ b/Scripts/TF/TFSaver_Rework.cs @@ -0,0 +1,235 @@ +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 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); + } + + 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); + Debug.Log("[TFCache]: adding key " + key + " to the tf dictionary"); + 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; + 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; + + + 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; + lock (stampedDict) + { + 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 (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 + { + 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: 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 new file mode 100644 index 0000000..6953c7d --- /dev/null +++ b/Scripts/TF/TfSaver_V2.cs @@ -0,0 +1,1158 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using Messages.geometry_msgs; +using Messages.tf; +using tf.net; +using Ros_CSharp; +using System; +using System.Linq; +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(); + private Queue static_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; + public bool getReverse; + + 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 (static_transforms) + { + static_transforms.Enqueue(msg); + } + } //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,false); + StartCoroutine(Remove(t)); + } + } + } + + //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,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.GetTransformV3(link, child, nsecs); + } + else + { + m = tree.GetTransformV3(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; + string str = m.transform.translation.x + " " + m.transform.translation.y + " " + m.transform.translation.z + " "; + //Debug.Log(str); + pub.publish(tfPub); + } + } + + + 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 bool isStatic; + public SortedList frames; + public Dictionary children; + + 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; + } +} + +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,false); + 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; + } + //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 + " "; + } + + + 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(); + + 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(); + 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; + + 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 (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 > 0) + { + + + 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.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); + //Debug.Log(a.transform.translation.x + " " + a.transform.translation.y + " " + a.transform.translation.z + " "); + if (a == null || par == null) + { + return null; + } + newTs.transform = AddTwoTransformsReverse(par.transform, newTs.transform); + } + 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; + } + + 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; + } + } + + //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() + { + 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 (n.isStatic) // just get the one frame stored there + { + 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; + 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, 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) + { + + 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 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 = 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) + { + 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 :) + 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; + } + + 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); + + /*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(); + 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 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; + + 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(); + 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); + + 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, bool isStat) + { + 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,isStat); + 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,isStat); + 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 (isStat) + { + nodeWithChildId.frames.Add(0, ts); + } + if (!nodeWithChildId.frames.ContainsKey(time)) + { + nodeWithChildId.frames.Add(time, ts); + } + else + { + nodeWithChildId.frames.Remove(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/TF/TransformPublisher.cs b/Scripts/TF/TransformPublisher.cs index ef8c719..6ebe126 100644 --- a/Scripts/TF/TransformPublisher.cs +++ b/Scripts/TF/TransformPublisher.cs @@ -5,13 +5,16 @@ 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; private Publisher tfPub; public GameObject trackedObject; + public GameObject dummyObject; + public string frame_id; public string child_frame_id; @@ -32,9 +35,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 +69,7 @@ void Update () { tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; + tfPub.publish(tfmsg); } } 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: