return msg tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame( PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) # PointStamped def do_transform_cloud(cloud, transform): res = cloud.copy() t_kdl = transform_to_kdl(transform) for p_in, p_out in [read_cloud(cloud), read_cloud(res)]: p = t_kdl * PyKDL.Vector(p_in.x, p_in.y, p_in.z) p_out.x = p[0] p_out.y = p[1] p_out.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud)
def __init__(self): self.registration = tf2_ros.TransformRegistration()
t.transform.translation.z)) # PointStamped def do_transform_point(point, transform): p = transform_to_kdl(transform) * PyKDL.Vector( point.point.x, point.point.y, point.point.z) res = PointStamped() res.point.x = p[0] res.point.y = p[1] res.point.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(PointStamped, do_transform_point) # Vector3Stamped def do_transform_vector3(vector3, transform): transform = copy.deepcopy(transform) transform.transform.translation.x = 0 transform.transform.translation.y = 0 transform.transform.translation.z = 0 p = transform_to_kdl(transform) * PyKDL.Vector( vector3.vector.x, vector3.vector.y, vector3.vector.z) res = Vector3Stamped() res.vector.x = p[0] res.vector.y = p[1] res.vector.z = p[2] res.header = transform.header
t.transform.translation.z)) # PointStamped def do_transform_point(point, transform): p = transform_to_kdl(transform) * PyKDL.Vector( point.point.x, point.point.y, point.point.z) res = PointStamped() res.point.x = p[0] res.point.y = p[1] res.point.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(PointStamped, do_transform_point) # Vector3Stamped def do_transform_vector3(vector3, transform): transform.transform.translation.x = 0.0 transform.transform.translation.y = 0.0 transform.transform.translation.z = 0.0 p = transform_to_kdl(transform) * PyKDL.Vector( vector3.vector.x, vector3.vector.y, vector3.vector.z) res = Vector3Stamped() res.vector.x = p[0] res.vector.y = p[1] res.vector.z = p[2] res.header = transform.header return res
PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) # Vector def do_transform_vector(vector, transform): res = transform_to_kdl(transform) * vector res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) def to_msg_vector(vector): msg = PointStamped() msg.header = vector.header msg.point.x = vector[0] msg.point.y = vector[1] msg.point.z = vector[2] return msg tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) def from_msg_vector(msg):
linear = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z) angular = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z) res = TwistStamped() res.twist.linear.x = linear[0] res.twist.linear.y = linear[1] res.twist.linear.z = linear[2] res.twist.angular.x = angular[0] res.twist.angular.y = angular[1] res.twist.angular.z = angular[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(TwistStamped, do_transform_twist) class GeometryMsgs(unittest.TestCase): 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)