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)
