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)