markers_from_tf.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Convert a skeleton transform tree to a list of visualization markers for RViz.
00005         
00006     Created for the Pi Robot Project: http://www.pirobot.org
00007     Copyright (c) 2011 Patrick Goebel.  All rights reserved.
00008 
00009     This program is free software; you can redistribute it and/or modify
00010     it under the terms of the GNU General Public License as published by
00011     the Free Software Foundation; either version 2 of the License, or
00012     (at your option) any later version.
00013     
00014     This program is distributed in the hope that it will be useful,
00015     but WITHOUT ANY WARRANTY; without even the implied warranty of
00016     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017     GNU General Public License for more details at:
00018     
00019     http://www.gnu.org/licenses/gpl.html
00020 """
00021 
00022 import roslib; roslib.load_manifest('skeleton_markers')
00023 import rospy
00024 from visualization_msgs.msg import Marker
00025 from geometry_msgs.msg import Point
00026 import tf
00027 
00028 class SkeletonMarkers():
00029     def __init__(self):
00030         rospy.init_node('markers_from_tf')
00031                 
00032         rospy.loginfo("Initializing Skeleton Markers Node...")
00033         
00034         rate = rospy.get_param('~rate', 20)
00035         r = rospy.Rate(rate)
00036         
00037         # The tf_prefix needs to be set in the launch file for both
00038         # the openni_tracker node and the markers_from_tf node
00039         self.tf_prefix = rospy.get_param('~tf_prefix', '/skeleton')
00040         
00041         # There is usually no need to change the fixed frame from the default
00042         self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
00043         
00044         # We need to prepend the tf_prefix to the fixed frame
00045         self.fixed_frame = self.tf_prefix + '/' + self.fixed_frame
00046 
00047         # Initialize the tf listener
00048         tf_listener = tf.TransformListener()
00049         
00050         # Define a marker publisher
00051         marker_pub = rospy.Publisher('skeleton_markers', Marker)
00052         
00053         # Intialize the markers
00054         self.initialize_markers()
00055         
00056         # Make sure we see the openni_depth_frame
00057         tf_listener.waitForTransform(self.fixed_frame, self.fixed_frame, rospy.Time(), rospy.Duration(60.0))
00058                
00059         # Begin the main loop
00060         while not rospy.is_shutdown():
00061             # Get the list of all skeleton frames from tf
00062             skeleton_frames = [f for f in tf_listener.getFrameStrings() if f.startswith(self.tf_prefix)]
00063             
00064             # If we don't have any frames yet, wait and try again
00065             if len(skeleton_frames) == 0:
00066                 r.sleep()
00067                 continue
00068         
00069             # Set the markers header
00070             self.markers.header.stamp = rospy.Time.now()
00071                         
00072             # Clear the markers point list
00073             self.markers.points = list()
00074             
00075             # Loop through the skeleton frames
00076             for frame in skeleton_frames:
00077                 if frame == self.fixed_frame:
00078                     continue
00079                 
00080                 # Find the position of the frame's origin relative to the fixed frame.
00081                 try:
00082                     position = Point()
00083                     
00084                     # Get the transformation from the fixed frame to the skeleton frame
00085                     (trans, rot)  = tf_listener.lookupTransform(self.fixed_frame, frame, rospy.Time(0))
00086                     position.x = trans[0]
00087                     position.y = trans[1]
00088                     position.z = trans[2]
00089                                                             
00090                     # Set a marker at the origin of this frame
00091                     self.markers.points.append(position)
00092                 except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00093                     rospy.logerr("tf error when looking up " + frame + ' and ' + self.fixed_frame)
00094                     continue
00095             
00096             # Publish the set of markers
00097             marker_pub.publish(self.markers)                       
00098             r.sleep()
00099             
00100     def initialize_markers(self):
00101         # Set various parameters
00102         scale = rospy.get_param('~scale', 0.07)
00103         lifetime = rospy.get_param('~lifetime', 0) # 0 is forever
00104         ns = rospy.get_param('~ns', 'skeleton_markers')
00105         id = rospy.get_param('~id', 0)
00106         color = rospy.get_param('~color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00107         
00108         # Initialize the marker points list
00109         self.markers = Marker()
00110         self.markers.header.frame_id = self.fixed_frame
00111         self.markers.ns = ns
00112         self.markers.id = id
00113         self.markers.type = Marker.POINTS
00114         self.markers.action = Marker.ADD
00115         self.markers.lifetime = rospy.Duration(lifetime)
00116         self.markers.scale.x = scale
00117         self.markers.scale.y = scale
00118         self.markers.color.r = color['r']
00119         self.markers.color.g = color['g']
00120         self.markers.color.b = color['b']
00121         self.markers.color.a = color['a']
00122         
00123 if __name__ == '__main__':
00124     try:
00125         SkeletonMarkers()
00126     except rospy.ROSInterruptException:
00127         pass


skeleton_markers
Author(s): Patrick Goebel
autogenerated on Mon Oct 6 2014 07:41:01