Displaying TF Data
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