Python msg.TransformStamped类代码示例

def sendTransform(self, translation, rotation, time, child, parent):
        """
        :param translation: the translation of the transformtion as a tuple (x, y, z)
        :param rotation: the rotation of the transformation as a tuple (x, y, z, w)
        :param time: the time of the transformation, as a rospy.Time()
        :param child: child frame in tf, string
        :param parent: parent frame in tf, string

        Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
        """

        t = TransformStamped()
        t.header.frame_id = parent
        t.header.stamp = time
        t.child_frame_id = child
        t.transform.translation.x = translation[0]
        t.transform.translation.y = translation[1]
        t.transform.translation.z = translation[2]

        t.transform.rotation.x = rotation[0]
        t.transform.rotation.y = rotation[1]
        t.transform.rotation.z = rotation[2]
        t.transform.rotation.w = rotation[3]

        tfm = tfMessage([t])
        self.pub_tf.publish(tfm)

def pose_to_tf_msg(parent, child, position, orientation):
  ts = TransformStamped()
  ts.header.frame_id = parent
  ts.child_frame_id = child
  ts.transform.rotation = Quaternion(*orientation)
  ts.transform.translation = Vector3(*position)
  return ts

 def broadcast(self):
     f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb")
     self.camera_transform = pickle.load(f)
     f.close()
     #SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa
     offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])
     self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset
     transform = tfx.inverse_tf(self.camera_transform)
     pt = transform.translation
     rot = transform.rotation
     msg = TransformStamped()
     msg.header.stamp = rospy.Time.now()
     msg.transform.rotation.x = rot.x
     msg.transform.rotation.y = rot.y
     msg.transform.rotation.z = rot.z
     msg.transform.rotation.w = rot.w
     msg.child_frame_id = "left_BC"   
     msg.transform.translation.x = pt.x
     msg.transform.translation.y = pt.y
     msg.transform.translation.z = pt.z
     msg.header.frame_id = "registration_brick_right"
     msg.header.stamp = rospy.Time.now()
     print('boo')
     while not rospy.is_shutdown():
         msg.header.stamp = rospy.Time.now()
         self.pub.publish([msg])
         rospy.sleep(0.1)

 def on_result(self, boxes, classes):
     rospy.logdebug("on_result")
     msg = ObjectDetection()
     msg.header = boxes.header
     for box, label, prob in zip(boxes.boxes, classes.label_names, classes.label_proba):
         if not label:
             continue
         pose = Object6DPose()
         pose.pose = box.pose
         pose.reliability = prob
         pose.type = label
         msg.objects.append(pose)
         if self.publish_tf:
             t = TransformStamped()
             t.header = box.header
             t.child_frame_id = label
             t.transform.translation.x = box.pose.position.x
             t.transform.translation.y = box.pose.position.y
             t.transform.translation.z = box.pose.position.z
             t.transform.rotation.x = box.pose.orientation.x
             t.transform.rotation.y = box.pose.orientation.y
             t.transform.rotation.z = box.pose.orientation.z
             t.transform.rotation.w = box.pose.orientation.w
             self.tfb.sendTransform(t)
     self.pub_detect.publish(msg)

    def tag_callback(self, msg_tag):
        # Listen for the transform of the tag in the world
        avg = PoseAverage.PoseAverage()
        for tag in msg_tag.detections:
            try:
                Tt_w = self.tfbuf.lookup_transform(self.world_frame, "tag_{id}".format(id=tag.id), rospy.Time(), rospy.Duration(1))
                Mtbase_w=self.transform_to_matrix(Tt_w.transform)
                Mt_tbase = tr.concatenate_matrices(tr.translation_matrix((0,0,0.17)), tr.euler_matrix(0,0,np.pi))
                Mt_w = tr.concatenate_matrices(Mtbase_w,Mt_tbase)
                Mt_r=self.pose_to_matrix(tag.pose)
                Mr_t=np.linalg.inv(Mt_r)
                Mr_w=np.dot(Mt_w,Mr_t)
                Tr_w = self.matrix_to_transform(Mr_w)
                avg.add_pose(Tr_w)
                self.publish_sign_highlight(tag.id)
            except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
                rospy.logwarn("Error looking up transform for tag_%s", tag.id)
                rospy.logwarn(ex.message)

        Tr_w =  avg.get_average() # Average of the opinions

        # Broadcast the robot transform
        if Tr_w is not None:
            # Set the z translation, and x and y rotations to 0
            Tr_w.translation.z = 0
            rot = Tr_w.rotation
            rotz=tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2]
            (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0, 0, rotz)
            T = TransformStamped()
            T.transform = Tr_w
            T.header.frame_id = self.world_frame
            T.header.stamp = rospy.Time.now()
            T.child_frame_id = self.duckiebot_frame
            self.pub_tf.publish(TFMessage([T]))
            self.lifetimer = rospy.Time.now()

def pack_transrot(translation, rotation, time, child, parent):
    """
    :param translation: the translation of the transformtion as a tuple (x, y, z)
    :param rotation: the rotation of the transformation as a tuple (x, y, z, w)
    :param time: the time of the transformation, as a rospy.Time()
    :param child: child frame in tf, string
    :param parent: parent frame in tf, string

    Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
    """

    t = TransformStamped()
    t.header.frame_id = parent
    t.header.stamp = time
    t.child_frame_id = child
    t.transform.translation.x = translation[0]
    t.transform.translation.y = translation[1]
    t.transform.translation.z = translation[2]

    rotation = np.array(rotation)
    rotation = rotation / np.linalg.norm(rotation, ord=2)
    t.transform.rotation.x = rotation[0]
    t.transform.rotation.y = rotation[1]
    t.transform.rotation.z = rotation[2]
    t.transform.rotation.w = rotation[3]
    
    return t

def post_tf(self, component_instance):
    component_name = component_instance.blender_obj.name
    frame_id = self._properties[component_name]['frame_id']
    child_frame_id = self._properties[component_name]['child_frame_id']

    euler = mathutils.Euler((component_instance.local_data['roll'],
                             component_instance.local_data['pitch'],
                             component_instance.local_data['yaw']))
    quaternion = euler.to_quaternion()

    t = TransformStamped()
    t.header.frame_id = frame_id
    t.header.stamp = rospy.Time.now()
    t.child_frame_id = child_frame_id
    t.transform.translation.x = component_instance.local_data['x']
    t.transform.translation.y = component_instance.local_data['y']
    t.transform.translation.z = component_instance.local_data['z']

    t.transform.rotation = quaternion

    tfm = tfMessage([t])

    for topic in self._topics:
        # publish the message on the correct topic    
        if str(topic.name) == str("/tf"):
            topic.publish(tfm)

def pack_transform(src_frame,dest_frame,trans,rot):
    transf = TransformStamped()    
    transf.header.frame_id = dest_frame
    transf.child_frame_id = src_frame
    transf.transform.translation = trans
    transf.transform.rotation = rot
    return transf

    def _toTransform(self, my_x, my_y):
        transform = TransformStamped()
        transform.header.stamp = rospy.Time.now()
        transform.header.frame_id = self.camera_frame
        transform.child_frame_id = self.blob.name

        (x, y, z) = self._projectTo3d(my_x, my_y)
        transform.transform.translation.x = x
        transform.transform.translation.y = y
        transform.transform.translation.z = z

        transform.transform.rotation.w = 1.0

        # If our parent frame is not the camera frame then an additional transformation is required.
        if self.parent_frame != self.camera_frame:
            point = PointStamped()
            point.header.frame_id = self.camera_frame
            point.header.stamp = rospy.Time(0)
            point.point.x = transform.transform.translation.x
            point.point.y = transform.transform.translation.y
            point.point.z = transform.transform.translation.z

            # Now we've gone from the regular camera frame to the correct parent_frame.
            point_transformed = self.listener.transformPoint(self.parent_frame, point)

            transform.header.frame_id = self.parent_frame
            transform.transform.translation.x = point_transformed.point.x
            transform.transform.translation.y = point_transformed.point.y
            transform.transform.translation.z = point_transformed.point.z

        return transform

def get_tree(frame, depth, step, frame_name, bush=False, idepth=0):
  branch_factor = get_tree.branch_factor
  twig_height   = get_tree.twig_height

  tfs = []
  F = kdl.Frame()

  for i in range(branch_factor):
    F.M = kdl.Rotation.RotZ(2.0*pi*i/branch_factor)
    F.p = F.M*kdl.Vector(step, 0, twig_height) + bush*frame.p

    msg = TransformStamped()
    msg.header.stamp = rospy.Time.now()
    msg.transform.translation = Vector3(*F.p)
    msg.transform.rotation = Quaternion(*(F.M.GetQuaternion()))

    fr_name = frame_name +str(i)
    msg.child_frame_id  = fr_name
    msg.header.frame_id = fr_name[:-(1 + idepth*bush)]

    tfs.append(msg)

    #recurse
    if depth > 1:
      tfs.extend(get_tree(F, depth - 1, step / 2.0,
                          fr_name, bush, idepth + 1))

  return tfs

def pack_pose(time, child, parent, matrix=None, trans=None, quat=None):

    if matrix is not None and (trans is None and quat is None):
        trans = tfs.translation_from_matrix(matrix)
        quat = tfs.quaternion_from_matrix(matrix)
    elif matrix is None and trans is not None and quat is not None:
        matrix = None  # nothing
    else:
        print 'invalid use'

    t = TransformStamped()
    t.header.frame_id = parent
    t.header.stamp = time
    t.child_frame_id = child
    t.transform.translation.x = trans[0]
    t.transform.translation.y = trans[1]
    t.transform.translation.z = trans[2]

    quat = numpy.array(quat)
    quat = quat / numpy.linalg.norm(quat, ord=2)
    t.transform.rotation.x = quat[0]
    t.transform.rotation.y = quat[1]
    t.transform.rotation.z = quat[2]
    t.transform.rotation.w = quat[3]

    return t

    def transformer(data):
        for marker in data.markers:
            if marker.subject_name:
                if marker.subject_name not in crazyflies:
                    crazyflies[marker.subject_name] = {}
                crazyflie = crazyflies[marker.subject_name]
                crazyflie[marker.marker_name] = marker.translation
                crazyflie["frame_id"] = str(data.frame_number)

        transforms = []
        for crazyflie_name in crazyflies:
            crazyflie = crazyflies[crazyflie_name]
            trans = TransformStamped()
            # trans.child_frame_id = crazyflie["frame_id"]
            trans.child_frame_id = "1"
            trans.header.frame_id = crazyflie["frame_id"]

            top = marker_to_vector(crazyflie["top"])
            bottom = marker_to_vector(crazyflie["bot"])
            left = marker_to_vector(crazyflie["left"])
            front = marker_to_vector(crazyflie["front"])

            center = get_center(top, left, bottom)
            rotation = get_rotation(center, front, top, left, bottom)

            mTrans = trans.transform.translation
            mTrans.x, mTrans.y, mTrans.z = center
            mRot = trans.transform.rotation
            mRot.x, mRot.y, mRot.z, mRot.w = rotation
            transforms.append(trans)

        msg.transforms = transforms
        pubtf.publish(msg)

    def _compute_transform(self, sensor_data, cur_time):
        parent_frame_id = "base_link"
        child_frame_id = self.name

        t = TransformStamped()
        t.header.stamp = cur_time
        t.header.frame_id = parent_frame_id
        t.child_frame_id = child_frame_id

        # for camera we reorient it to look at the same axis as the opencv projection
        # in order to get easy depth cloud for RGBD camera
        t.transform = carla_transform_to_ros_transform(
            self.carla_object.get_transform())

        rotation = t.transform.rotation
        quat = [rotation.x, rotation.y, rotation.z, rotation.w]
        quat_swap = tf.transformations.quaternion_from_matrix(
            [[0, 0, 1, 0],
            [-1, 0, 0, 0],
            [0, -1, 0, 0],
            [0, 0, 0, 1]])
        quat = tf.transformations.quaternion_multiply(quat, quat_swap)

        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.process_msg_fun('tf', t)

    def publishLatchTransform(self, arm):
        if arm == 'left':
            self.pub_tf_left = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True)
        else:
            self.pub_tf_right = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True)
            
        f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/transform_" + arm + \
            ".txt", "r")
        transform = f.readline().split()
        f.close()
        print(transform)
        # Send static link transforms
        msg = TransformStamped()

        msg.header.stamp = rospy.Time.now()
        msg.transform.rotation.x =  float(transform[3])
        msg.transform.rotation.y =  float(transform[4])
        msg.transform.rotation.z =  float(transform[5])
        msg.transform.rotation.w =  float(transform[6])
        msg.child_frame_id = "left_BC"   
        msg.transform.translation.x = float(transform[0])
        msg.transform.translation.y = float(transform[1])
        msg.transform.translation.z = float(transform[2])
        if arm == 'left':
            # msg.header.frame_id = "two_psm_base_link"
            msg.header.frame_id = "two_remote_center_link"
        else:
            msg.header.frame_id = "one_remote_center_link"
        while not rospy.is_shutdown():
                msg.header.stamp = rospy.Time.now()
                self.pub_tf_right.publish([msg])
                rospy.sleep(0.5)

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 = PointStamped()
    v.header.stamp = rospy.Time(2)
    v.header.frame_id = 'a'
    v.point.x = 1
    v.point.y = 2
    v.point.z = 3
    print b.transform(v, 'b')

    v = Vector3Stamped()
    v.header.stamp = rospy.Time(2)
    v.header.frame_id = 'a'
    v.vector.x = 1
    v.vector.y = 2
    v.vector.z = 3
    print b.transform(v, 'b')

    v = PoseStamped()
    v.header.stamp = rospy.Time(2)
    v.header.frame_id = 'a'
    v.pose.position.x = 1
    v.pose.position.y = 2
    v.pose.position.z = 3
    v.pose.orientation.x = 1
    print b.transform(v, 'b')