def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1.0
t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0)
t.header.stamp = builtin_interfaces.msg.Time(sec=2)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2),
rclpy.time.Duration(seconds=2))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PyKDL.Vector(1, 2, 3)
out = b.transform(
tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.x(), 0)
self.assertEqual(out.y(), -2)
self.assertEqual(out.z(), -3)
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
out = b.transform(
tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.p.x(), 0)
self.assertEqual(out.p.y(), -2)
self.assertEqual(out.p.z(), -3)
# TODO(tfoote) check values of rotation
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.vel.x(), 1)
self.assertEqual(out.vel.y(), -8)
self.assertEqual(out.vel.z(), 2)
self.assertEqual(out.rot.x(), 4)
self.assertEqual(out.rot.y(), -5)
self.assertEqual(out.rot.z(), -6)
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.force.x(), 1)
self.assertEqual(out.force.y(), -2)
self.assertEqual(out.force.z(), -3)
self.assertEqual(out.torque.x(), 4)
self.assertEqual(out.torque.y(), -8)
self.assertEqual(out.torque.z(), -4)
def convert():
v = PyKDL.Vector(1, 2, 3)
vs = tf2_ros.Stamped(v, rospy.Time(2), 'a')
vs2 = tf2_ros.convert(vs, PyKDL.Vector)
vs2[1] = 100
print vs2
print v
def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1
t.transform.rotation.x = 1
t.header.stamp = rospy.Time(2.0)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a', 'b', rospy.Time(2.0),
rospy.Duration(2.0))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PyKDL.Vector(1, 2, 3)
out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.x(), 0)
self.assertEqual(out.y(), -2)
self.assertEqual(out.z(), -3)
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
print(out)
self.assertEqual(out.p.x(), 0)
self.assertEqual(out.p.y(), -2)
self.assertEqual(out.p.z(), -3)
# TODO(tfoote) check values of rotation
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.vel.x(), 1)
self.assertEqual(out.vel.y(), -8)
self.assertEqual(out.vel.z(), 2)
self.assertEqual(out.rot.x(), 4)
self.assertEqual(out.rot.y(), -5)
self.assertEqual(out.rot.z(), -6)
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.force.x(), 1)
self.assertEqual(out.force.y(), -2)
self.assertEqual(out.force.z(), -3)
self.assertEqual(out.torque.x(), 4)
self.assertEqual(out.torque.y(), -8)
self.assertEqual(out.torque.z(), -4)
def convert_vector(vector):
"""Convert a generic stamped triplet message to a stamped PyKDL Vector.
:param vector: The message to convert.
:return: The timestamped converted PyKDL vector.
:rtype: PyKDL.Vector
"""
return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id)
def test_convert(self):
v = PyKDL.Vector(1, 2, 3)
vs = tf2_ros.Stamped(v, rospy.Time(2), 'a')
vs2 = tf2_ros.convert(vs, PyKDL.Vector)
self.assertEqual(vs.x(), 1)
self.assertEqual(vs.y(), 2)
self.assertEqual(vs.z(), 3)
self.assertEqual(vs2.x(), 1)
self.assertEqual(vs2.y(), 2)
self.assertEqual(vs2.z(), 3)
def test_convert(self):
p = tf2_ros.Stamped(PyKDL.Vector(1, 2, 3),
builtin_interfaces.msg.Time(sec=0), 'my_frame')
msg = tf2_ros.convert(p, PointStamped)
p2 = tf2_ros.convert(msg, PyKDL.Vector)
self.assertEqual(p2[0], p[0])
self.assertEqual(p2[1], p[1])
self.assertEqual(p2[2], p[2])
self.assertEqual(p2.header, p.header)
def from_msg_vector(msg):
"""Convert a PointStamped message to a stamped PyKDL Vector.
:param msg: The PointStamped message to convert.
:type msg: geometry_msgs.msg.PointStamped
:return: The timestamped converted PyKDL vector.
:rtype: PyKDL.Vector
"""
vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z)
return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id)
def test_convert(self):
v = PyKDL.Vector(1,2,3)
vs = tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a')
vs2 = tf2_ros.convert(vs, PyKDL.Vector)
self.assertEqual(vs.x(), 1)
self.assertEqual(vs.y(), 2)
self.assertEqual(vs.z(), 3)
self.assertEqual(vs2.x(), 1)
self.assertEqual(vs2.y(), 2)
self.assertEqual(vs2.z(), 3)
def test_convert(self):
p = tf2_ros.Stamped(PyKDL.Vector(1, 2, 3), rospy.Time(), 'my_frame')
print(p)
msg = tf2_ros.convert(p, PointStamped)
print(msg)
p2 = tf2_ros.convert(msg, PyKDL.Vector)
print(p2)
p2[0] = 100
print(p)
print(p2)
print(p2.header)
def main():
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1
t.transform.rotation.x = 1
t.header.stamp = rospy.Time(2.0)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
print b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0))
v = PyKDL.Vector(1, 2, 3)
print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
def convert_vector(vector):
return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp,
vector.header.frame_id)
def from_msg_vector(msg):
vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z)
return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id)
