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 rospy
00023 from visualization_msgs.msg import Marker
00024 from geometry_msgs.msg import Point
00025 import tf
00026 
00027 class SkeletonMarkers():
00028     def __init__(self):
00029         rospy.init_node('markers_from_tf')
00030                 
00031         rospy.loginfo("Initializing Skeleton Markers Node...")
00032         
00033         rate = rospy.get_param('~rate', 20)
00034         r = rospy.Rate(rate)
00035         
00036         # There is usually no need to change the fixed frame from the default
00037         self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
00038         
00039         # Get the list of skeleton frames we want to track
00040         self.skeleton_frames = rospy.get_param('~skeleton_frames', '')
00041 
00042         # Initialize the tf listener
00043         tf_listener = tf.TransformListener()
00044         
00045         # Define a marker publisher
00046         marker_pub = rospy.Publisher('skeleton_markers', Marker, queue_size=5)
00047         
00048         # Intialize the markers
00049         self.initialize_markers()
00050         
00051         # Make sure we see the openni_depth_frame
00052         tf_listener.waitForTransform(self.fixed_frame, self.fixed_frame, rospy.Time(), rospy.Duration(60.0))
00053         
00054         # A flag to track when we have detected a skeleton
00055         skeleton_detected = False
00056         
00057         # Begin the main loop
00058         while not rospy.is_shutdown():
00059             # Set the markers header
00060             self.markers.header.stamp = rospy.Time.now()
00061                         
00062             # Clear the markers point list
00063             self.markers.points = list()
00064             
00065             # Check to see if a skeleton is detected
00066             while not skeleton_detected: 
00067                 # Assume we can at least see the head frame               
00068                 frames = [f for f in tf_listener.getFrameStrings() if f.startswith('head_')]
00069                 
00070                 try:
00071                     # If the head frame is visible, pluck off the
00072                     # user index from the name
00073                     head_frame = frames[0]
00074                     user_index = head_frame.replace('head_', '')
00075                     
00076                     # Make sure we have a transform between the head
00077                     # and the fixed frame
00078                     try:
00079                         (trans, rot)  = tf_listener.lookupTransform(self.fixed_frame, head_frame, rospy.Time(0))
00080                         skeleton_detected = True
00081 
00082                     except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00083                         skeleton_detected = False
00084                         rospy.loginfo("User index: " + str(user_index))
00085                         r.sleep()
00086                 except:
00087                     skeleton_detected = False
00088             
00089             # Loop through the skeleton frames
00090             for frame in self.skeleton_frames:
00091                 # Append the user_index to the frame name
00092                 skel_frame = frame + "_" + str(user_index)
00093 
00094                 # We only need the origin of each skeleton frame
00095                 # relative to the fixed frame
00096                 position = Point()
00097                                         
00098                 # Get the transformation from the fixed frame
00099                 # to the skeleton frame
00100                 try:
00101                     (trans, rot)  = tf_listener.lookupTransform(self.fixed_frame, skel_frame, rospy.Time(0))
00102                     position.x = trans[0]
00103                     position.y = trans[1]
00104                     position.z = trans[2]
00105                                                             
00106                     # Set a marker at the origin of this frame
00107                     self.markers.points.append(position)
00108                 except:
00109                     pass
00110                 
00111             # Publish the set of markers
00112             marker_pub.publish(self.markers)
00113                               
00114             r.sleep()
00115             
00116     def initialize_markers(self):
00117         # Set various parameters
00118         scale = rospy.get_param('~scale', 0.07)
00119         lifetime = rospy.get_param('~lifetime', 0) # 0 is forever
00120         ns = rospy.get_param('~ns', 'skeleton_markers')
00121         id = rospy.get_param('~id', 0)
00122         color = rospy.get_param('~color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00123         
00124         # Initialize the marker points list
00125         self.markers = Marker()
00126         self.markers.header.frame_id = self.fixed_frame
00127         self.markers.ns = ns
00128         self.markers.id = id
00129         self.markers.type = Marker.POINTS
00130         self.markers.action = Marker.ADD
00131         self.markers.lifetime = rospy.Duration(lifetime)
00132         self.markers.scale.x = scale
00133         self.markers.scale.y = scale
00134         self.markers.color.r = color['r']
00135         self.markers.color.g = color['g']
00136         self.markers.color.b = color['b']
00137         self.markers.color.a = color['a']
00138         
00139 if __name__ == '__main__':
00140     try:
00141         SkeletonMarkers()
00142     except rospy.ROSInterruptException:
00143         pass


skeleton_markers
Author(s): Patrick Goebel
autogenerated on Fri Aug 28 2015 13:09:12