public TransformStorage(emTransform data, uint frame_id, uint child_frame_id) { rotation = data.basis; translation = data.origin; stamp = TimeCache.toLong(data.stamp.data); this.frame_id = frame_id; this.child_frame_id = child_frame_id; }
public void transformQuaternion(string target_frame, Stamped <emQuaternion> stamped_in, ref Stamped <emQuaternion> stamped_out) { emTransform trans = new emTransform(); lookupTransform(target_frame, stamped_in.frame_id, stamped_in.stamp, out trans); stamped_out.data = trans * stamped_in.data; stamped_out.stamp = trans.stamp; stamped_out.frame_id = target_frame; }
public void transformVector(string target_frame, Stamped <emVector3> stamped_in, ref Stamped <emVector3> stamped_out) { emTransform trans = new emTransform(); lookupTransform(target_frame, stamped_in.frame_id, stamped_in.stamp, out trans); emVector3 end = stamped_in.data; emVector3 origin = new emVector3(0, 0, 0); emVector3 output = (trans * end) - (trans * origin); stamped_out.data = output; stamped_out.stamp = trans.stamp; stamped_out.frame_id = target_frame; }
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; }
public bool setTransform(emTransform transform) { emTransform mapped_transform = new emTransform(transform.basis, transform.origin, transform.stamp, transform.frame_id, transform.child_frame_id); mapped_transform.child_frame_id = resolve(tf_prefix, transform.child_frame_id); mapped_transform.frame_id = resolve(tf_prefix, transform.frame_id); if (mapped_transform.child_frame_id == mapped_transform.frame_id) { return(false); } if (mapped_transform.child_frame_id == "/") { return(false); } if (mapped_transform.frame_id == "/") { return(false); } uint frame_number = lookupOrInsertFrameNumber(mapped_transform.frame_id); uint child_frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id); TimeCache parent_frame = null, frame = null; if (!frames.ContainsKey(frame_number)) { parent_frame = frames[frame_number] = new TimeCache(cache_time); } if (!frames.ContainsKey(child_frame_number)) { frame = frames[child_frame_number] = new TimeCache(cache_time); } else { //if we're revising a frame, that was previously labelled as having no parent, clear that knowledge from the time cache frame = frames[child_frame_number]; } return(frame.insertData(new TransformStorage(mapped_transform, frame_number, child_frame_number))); }
public bool lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string) { transform = null; string mapped_tgt = resolve(tf_prefix, target_frame); string mapped_src = resolve(tf_prefix, source_frame); if (mapped_tgt == mapped_src) { transform = new emTransform(); transform.origin = new emVector3(); transform.basis = new emQuaternion(); transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = ROS.GetTime(DateTime.Now); return(true); } TF_STATUS retval; uint target_id = getFrameIDInternal(mapped_tgt); uint source_id = getFrameIDInternal(mapped_src); TransformAccum accum = new TransformAccum(); retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string); if (retval != TF_STATUS.NO_ERROR) { error_string = error_string ?? "UNSPECIFIED"; switch (retval) { case TF_STATUS.CONNECTIVITY_ERROR: error_string = "NO CONNECTIONSZSZ: " + error_string; break; case TF_STATUS.EXTRAPOLATION_ERROR: error_string = "EXTRAPOLATION: " + error_string; break; case TF_STATUS.LOOKUP_ERROR: error_string = "LOOKUP: " + error_string; break; default: if (accum.result_quat == null || accum.result_vec == null) { error_string = "ACCUM WALK FAIL!"; } break; } } if (accum.result_vec != null && accum.result_quat != null) { transform = new emTransform(); transform.origin = accum.result_vec; transform.basis = accum.result_quat; transform.child_frame_id = mapped_src; transform.frame_id = mapped_tgt; transform.stamp = new Time(ROS.ticksToData((long)accum.time)); } return(retval == TF_STATUS.NO_ERROR); }
public bool lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform) { string error_string = null; bool result = lookupTransform(target_frame, source_frame, time, out transform, ref error_string); if (!result && error_string != null) { ROS.Error(error_string); } return(result); }
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(); } }