Displaying TF Data

edit

I'm looking to display tf data in a similar fashion to what rviz does (and how it displays an arrow from each child frame origin to parent frame origin). Right now, I subscribe to the /tf topic. Every time I receive a new tf message, I use tf.TransformListener to compute the positions for each arrow marker (see code below). When I try to display the newly created markers in real time, I find that the markers are considerably behind the tf tree. The transformPose function takes up a significant amount of time. Thus, I am wondering how rviz displays tf data using markers in real team. They must have a more efficient method?

def callback_tf(self, msg):
    self.pub_rm.publish(self.getMarkers(msg))

def getMarkers(self, msg):
    mks = MarkerArray()
    counter = 0
    for transform in msg.transforms:

        child_frame = transform.child_frame_id
        parent_frame = transform.header.frame_id

        if self.tf_listener.frameExists(BASE_LINK) and self.tf_listener.frameExists(parent_frame) and self.tf_listener.frameExists(child_frame):

            parent_pose = PoseStamped()
            parent_pose.header.frame_id = parent_frame
            child_pose = PoseStamped()
            child_pose.header.frame_id = child_frame

            parent_pose_base = self.tf_listener.transformPose(
                BASE_LINK, parent_pose)
            child_pose_base = self.tf_listener.transformPose(
                BASE_LINK, child_pose)

            link = Marker()
            link.header.frame_id = BASE_LINK
            link.header.stamp = transform.header.stamp
            link.ns = 'link_markers'
            link.id = counter
            link.type = Marker.ARROW
            link.action = 0
            link.pose.position.x = 0
            link.pose.position.y = 0
            link.pose.position.z = 0
            link.pose.orientation.x = 0
            link.pose.orientation.y = 0
            link.pose.orientation.z = 0
            link.pose.orientation.w = 1
            link.points = [child_pose_base.pose.position,
                           parent_pose_base.pose.position]
            mks.markers.append(link)

        else:
            rospy.loginfo(
                'RobotMarkersPublisher error: one or more of frame [world, %s, %s] does not exist', child_frame, parent_frame)
        counter += 1

    return mks