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);
}
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");
}
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();
}
