public void TestMethod1(bool istest = false) { emTransform a2b = new emTransform( new emQuaternion(), new emVector3(0.0, 0.0, 1.0),//1.0, 0.0, 0.5), when, "a", "b"); emTransform b2c = new emTransform( new emQuaternion(), new emVector3(0.0, 0.0, -0.5),//-1.0, -0.5, 1.0), when, "b", "c"); emTransform c2d = new emTransform( emQuaternion.FromRPY(new emVector3(0.0, 0.0, Math.PI / 4.0)), new emVector3(1.0, 0.0, 0.0), when, "c", "d"); bool setsuccess = transformer.setTransform(a2b) && transformer.setTransform(b2c) && transformer.setTransform(c2d); #if !CONSOLEMODE Assert.IsTrue(setsuccess); #else if (!setsuccess) { throw new Exception("Failed to set transforms"); } #endif Console.WriteLine("DO SOMETHING SMART WITH TRANSFORMPOINT"); }
// Update is called once per frame void Update() { Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage(); 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); Messages.std_msgs.Header hdr = new Messages.std_msgs.Header(); hdr.frame_id = frame_id; hdr.stamp = ROS.GetTime(); if (!using_gazebo) { hdr.stamp.data.sec += 18000; } tfmsg.transforms[0].header = hdr; tfmsg.transforms[0].child_frame_id = child_frame_id; tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform(); tfmsg.transforms[0].transform.translation = ta.origin.ToMsg(); //tfmsg.transforms[0].transform.translation.z += 1.0; tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; tfPub.publish(tfmsg); }
private void Update() { transform.Rotate(Time.deltaTime * 10 * rot_x, Time.deltaTime * 10 * rot_y, Time.deltaTime * 10 * rot_z); emTransform emt = new emTransform(TF); for (int i = 0; i < theCloud.Length; i++) { transform.SetPositionAndRotation(new Vector3(((Vector3)emt.UnityPosition).x, ((Vector3)emt.UnityPosition).y, ((Vector3)emt.UnityPosition).z), (Quaternion)emt.UnityRotation); if (theCloud[i].isStarted) { theCloud[i].UpdateMesh(); } else { print("Cloud not yet started"); } } unity_tf_msg.polygon.points[0].x = transform.position.x; unity_tf_msg.polygon.points[0].y = transform.position.y; unity_tf_msg.polygon.points[0].z = transform.position.z; unity_tf_msg.polygon.points[1].x = transform.rotation.eulerAngles.x; unity_tf_msg.polygon.points[1].y = transform.rotation.eulerAngles.y; unity_tf_msg.polygon.points[1].z = transform.rotation.eulerAngles.z; unity_tf_msg.polygon.points[2].x = transform.lossyScale.x; unity_tf_msg.polygon.points[2].y = transform.lossyScale.y; unity_tf_msg.polygon.points[2].z = transform.lossyScale.z; }
void Update() { if (publishtf) { _tfmsg.transforms[0].header.stamp = ROS.GetTime(); _tfmsg.transforms[0].header.stamp.data.sec += 18000; //windows time is dumb //Debug.Log("Current time" + ROS.GetTime().data.sec); tfPub.publish(_tfmsg); } if (hj == null) { hj = GameObject.Find(frame_id); } else if (!interactableItem.IsInteracting()) { if (!startedInteracting) { //Debug.Log("Not interacting!"); Vector3 t = hj.transform.position; Quaternion q = hj.transform.rotation; transform.position = t; transform.rotation = q; } if (startedInteracting) { Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage(); Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1]; arr[0] = new Messages.geometry_msgs.TransformStamped(); tfmsg.transforms = arr; //Transform trans = trackedObj.transform; emTransform ta = new emTransform(transform, ROS.GetTime(), "/world", "/look_at_frame"); Messages.std_msgs.Header hdr = new Messages.std_msgs.Header(); hdr.frame_id = "/world"; hdr.stamp = ROS.GetTime(); hdr.stamp.data.sec += 18000; tfmsg.transforms[0].header = hdr; tfmsg.transforms[0].child_frame_id = "/look_at_frame"; tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform(); tfmsg.transforms[0].transform.translation = ta.origin.ToMsg(); tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; tfPub.publish(tfmsg); _tfmsg = tfmsg; publishtf = true; startedInteracting = false; } } else { startedInteracting = true; } }
static void Main(string[] args) { ROS.Init(args, "tf_example"); NodeHandle nh = new NodeHandle(); ROS.Info("This node will create a Transformer to compare lookup results between four source/target frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"); tfer = new Transformer(false); #region tf_echo results /* * tf_echo camera_link camera_rgb_frame * (0.0,-0.045,0.0) * (0,0,0,1) */ emTransform result1 = new emTransform() {basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0, -0.045, 0), child_frame_id = "camera_rgb_frame", frame_id = "camera_link"}; /* * tf_echo camera_link camera_rgb_optical_frame * (0.0,-0.045,0.0) * (-0.5,0.5,-0.5,0.5) */ emTransform result2 = new emTransform() { basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link" }; /* * tf_echo camera_rgb_frame camera_depth_frame * (0.0,0.25,0.0) * (0,0,0,1) */ emTransform result3 = new emTransform() { basis = new emQuaternion(0,0,0,1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame" }; /* * tf_echo camera_rgb_optical_frame camera_depth_frame * (-0.25,0.0,0.0) * (0.5,-0.5,0.5,0.5) */ emTransform result4 = new emTransform() { basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame" }; #endregion emTransform test1 = null, test2 = null, test3 = null, test4 = null; do { if (test1 == null || !string.Equals(result1.ToString(), test1.ToString())) test1 = testLookup(result1); if (test2 == null || !string.Equals(result2.ToString(), test2.ToString())) test2 = testLookup(result2); if (test3 == null || !string.Equals(result3.ToString(), test3.ToString())) test3 = testLookup(result3); if (test4 == null || !string.Equals(result4.ToString(), test4.ToString())) test4 = testLookup(result4); Thread.Sleep(1000); } while (!string.Equals(result1.ToString(), test1.ToString()) || !string.Equals(result2.ToString(), test2.ToString()) || !string.Equals(result3.ToString(), test3.ToString()) || !string.Equals(result4.ToString(), test4.ToString())); Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit"); Console.ReadLine(); ROS.shutdown(); }
// Update is called once per frame void Update() { if (hj == null) { hj = GameObject.Find(frame_id); } else if (!interactableItem.IsInteracting()) { if (!startedInteracting) { //Debug.Log("Not interacting!"); Vector3 t = hj.transform.position; Quaternion q = hj.transform.rotation; transform.position = t; transform.rotation = q; } if (startedInteracting) { //Debug.Log("Stopped Interacting!"); emTransform ta = new emTransform(transform); //ta.UnityPosition += new Vector3(0.1f,0.1f,0.1f); Messages.ihmc_msgs.HandTrajectoryRosMessage msg = new Messages.ihmc_msgs.HandTrajectoryRosMessage(); msg.unique_id = 1; msg.robot_side = (byte)side; msg.base_for_control = 0; Messages.ihmc_msgs.SE3TrajectoryPointRosMessage trajectory = new Messages.ihmc_msgs.SE3TrajectoryPointRosMessage(); trajectory.position = ta.origin.ToMsg(); //trajectory.position.z += 1.0; trajectory.orientation = ta.basis.ToMsg(); trajectory.time = 2.0; Debug.Log("Sending X: " + ta.origin.ToMsg().x + " Y: " + ta.origin.ToMsg().y + " Z: " + ta.origin.ToMsg().z); Messages.ihmc_msgs.SE3TrajectoryPointRosMessage[] trajectorys = { trajectory }; msg.taskspace_trajectory_points = trajectorys; msg.Serialize(true); pub.publish(msg); startedInteracting = false; } } else { startedInteracting = true; } }
private static emTransform testLookup(emTransform intendedResult) { if (!tfer.waitForTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, new Duration(new TimeData(10, 0)), null)) return null; emTransform ret = new emTransform(); if (tfer.lookupTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, out ret)) { Console.WriteLine("*** " + intendedResult.frame_id + " ==> " + intendedResult.child_frame_id + " ***"); Console.WriteLine("********************** IDEAL *********************"); Console.WriteLine(intendedResult); Console.WriteLine("********************** ACTUAL ********************"); Console.WriteLine(ret); Console.WriteLine("***************************************************\n\n"); return ret; } return null; }
private static emTransform testLookup(emTransform intendedResult) { if (!tfer.waitForTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, new Duration(new TimeData(1, 0)), null)) { return(null); } emTransform ret = new emTransform(); if (tfer.lookupTransform(intendedResult.frame_id, intendedResult.child_frame_id, intendedResult.stamp, out ret)) { Console.WriteLine("*** " + intendedResult.frame_id + " ==> " + intendedResult.child_frame_id + " ***"); Console.WriteLine("********************** IDEAL *********************"); Console.WriteLine(intendedResult); Console.WriteLine("********************** ACTUAL ********************"); Console.WriteLine(ret); Console.WriteLine("***************************************************\n\n"); return(ret); } return(null); }
static void Main(string[] args) { ROS.Init(args, "tf_example"); NodeHandle nh = new NodeHandle(); ROS.Info("This node will create a Transformer to compare lookup results between four source/target frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"); string[] nodes = null; while (ROS.ok && !ROS.shutting_down && (!master.getNodes(ref nodes) || !nodes.Contains("/camera/driver"))) { ROS.Error("For this to work, you need to \"roslaunch openni2_launch openni2.launch\" on a PC with an ASUS Xtion or PrimeSense Carmine sensor plugged into it, and connect to the same master"); Thread.Sleep(2000); } if (ROS.ok && !ROS.shutting_down) { tfer = new Transformer(false); #region tf_echo results /* * tf_echo camera_link camera_rgb_frame * (0.0,-0.045,0.0) * (0,0,0,1) */ emTransform result1 = new emTransform() { basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0, -0.045, 0), child_frame_id = "camera_rgb_frame", frame_id = "camera_link" }; /* * tf_echo camera_link camera_rgb_optical_frame * (0.0,-0.045,0.0) * (-0.5,0.5,-0.5,0.5) */ emTransform result2 = new emTransform() { basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link" }; /* * tf_echo camera_rgb_frame camera_depth_frame * (0.0,0.25,0.0) * (0,0,0,1) */ emTransform result3 = new emTransform() { basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame" }; /* * tf_echo camera_rgb_optical_frame camera_depth_frame * (-0.25,0.0,0.0) * (0.5,-0.5,0.5,0.5) */ emTransform result4 = new emTransform() { basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame" }; #endregion emTransform test1 = null, test2 = null, test3 = null, test4 = null; do { if (test1 == null || !string.Equals(result1.ToString(), test1.ToString())) { test1 = testLookup(result1); } if (!ROS.ok || ROS.shutting_down) { break; } if (test2 == null || !string.Equals(result2.ToString(), test2.ToString())) { test2 = testLookup(result2); } if (!ROS.ok || ROS.shutting_down) { break; } if (test3 == null || !string.Equals(result3.ToString(), test3.ToString())) { test3 = testLookup(result3); } if (!ROS.ok || ROS.shutting_down) { break; } if (test4 == null || !string.Equals(result4.ToString(), test4.ToString())) { test4 = testLookup(result4); } Thread.Sleep(100); } while (ROS.ok && !ROS.shutting_down && (test1 == null || !string.Equals(result1.ToString(), test1.ToString()) || test2 == null || !string.Equals(result2.ToString(), test2.ToString()) || test3 == null || !string.Equals(result3.ToString(), test3.ToString()) || test4 == null || !string.Equals(result4.ToString(), test4.ToString()))); } if (ROS.ok && !ROS.shutting_down) { Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit"); Console.ReadLine(); ROS.shutdown(); } }
static void Main(string[] args) { ROS.Init(args, "tf_example"); NodeHandle nh = new NodeHandle(); ROS.Info("This node will create a Transformer to compare lookup results between four source/target frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"); tfer = new Transformer(false); #region tf_echo results /* * tf_echo camera_link camera_rgb_frame * (0.0,-0.045,0.0) * (0,0,0,1) */ emTransform result1 = new emTransform() { basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0, -0.045, 0), child_frame_id = "camera_rgb_frame", frame_id = "camera_link" }; /* * tf_echo camera_link camera_rgb_optical_frame * (0.0,-0.045,0.0) * (-0.5,0.5,-0.5,0.5) */ emTransform result2 = new emTransform() { basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link" }; /* * tf_echo camera_rgb_frame camera_depth_frame * (0.0,0.25,0.0) * (0,0,0,1) */ emTransform result3 = new emTransform() { basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame" }; /* * tf_echo camera_rgb_optical_frame camera_depth_frame * (-0.25,0.0,0.0) * (0.5,-0.5,0.5,0.5) */ emTransform result4 = new emTransform() { basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame" }; #endregion emTransform test1 = null, test2 = null, test3 = null, test4 = null; do { if (test1 == null || !string.Equals(result1.ToString(), test1.ToString())) { test1 = testLookup(result1); } if (test2 == null || !string.Equals(result2.ToString(), test2.ToString())) { test2 = testLookup(result2); } if (test3 == null || !string.Equals(result3.ToString(), test3.ToString())) { test3 = testLookup(result3); } if (test4 == null || !string.Equals(result4.ToString(), test4.ToString())) { test4 = testLookup(result4); } Thread.Sleep(1000); } while (!string.Equals(result1.ToString(), test1.ToString()) || !string.Equals(result2.ToString(), test2.ToString()) || !string.Equals(result3.ToString(), test3.ToString()) || !string.Equals(result4.ToString(), test4.ToString())); Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit"); Console.ReadLine(); ROS.shutdown(); }