markers_from_openni_tracker.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 import re
00028 
00029 class SkeletonMarkers():
00030     def __init__(self):
00031         rospy.init_node('markers_from_tf')
00032                 
00033         rospy.loginfo("Initializing Skeleton Markers Node...")
00034         
00035         rate = rospy.get_param('~rate', 20)
00036         r = rospy.Rate(rate)
00037         tf_prefix = rospy.get_param('~tf_prefix', '')
00038         skeleton_frames = rospy.get_param('~skeleton_frames', '')
00039         self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
00040         
00041         # Normalize the tf_prefix variable with / at the beginning and end
00042         tf_prefix = '/' + tf_prefix + '/'
00043         tf_prefix = re.sub('/+', '/', tf_prefix)
00044         
00045         # Normalize the tf_prefix variable with / at the beginning and end
00046         self.fixed_frame = '/' + tf_prefix + '/' + self.fixed_frame
00047         self.fixed_frame = re.sub('/+', '/', self.fixed_frame)
00048 
00049         # Initialize tf listener
00050         tf_listener = tf.TransformListener()
00051         
00052         # Define a marker publisher
00053         marker_pub = rospy.Publisher('skeleton_markers', Marker)
00054         
00055         # Intialize the markers
00056         self.init_markers()
00057         
00058         # The openni_tracker identifies each skeleton frame with a user_id beginning
00059         # with 1 and incrementing as new users are tracked.
00060         user_id = None
00061         
00062         # Get the list of all skeleton frames from tf
00063         #skeleton_frames = [f for f in tf_listener.getFrameStrings() if f.startswith("/" + self.tf_prefix)]
00064         
00065                
00066         # Begin the main loop
00067         while not rospy.is_shutdown():
00068             # Get a list of all frames
00069             all_frames = [f for f in tf_listener.getFrameStrings()]
00070             
00071             # Test for the user ID to track
00072             try:
00073                 test = all_frames.index(tf_prefix + 'torso')
00074                 user_id = ""
00075             except:
00076                 try:
00077                     test = all_frames.index(tf_prefix + 'torso_1')
00078                     user_id = "_1"
00079                 except:
00080                     pass
00081             
00082             if user_id is None:
00083                 r.sleep
00084                 continue
00085             
00086             # Get all skeleton frames for the detected user user
00087             user_frames = list()
00088     
00089             for frame in skeleton_frames:
00090                 qualified_frame = tf_prefix + frame + user_id
00091                 try:
00092                     all_frames.index(qualified_frame)
00093                     user_frames.append(qualified_frame)
00094                 except:
00095                     pass
00096                 
00097             #tf_listener.waitForTransform(self.fixed_frame, user_frames[0], rospy.Time(), rospy.Duration(25.0))
00098 
00099             # Set the markers header
00100             self.markers.header.stamp = rospy.Time.now()
00101             
00102             # Clear the markers point list
00103             self.markers.points = list()
00104             
00105             # Loop through the skeleton frames
00106             for frame in user_frames:
00107                 if frame == self.fixed_frame:
00108                     continue
00109                 # Find the position of the frame's origin relative to the fixed frame.
00110                 try:
00111                     position = Point()
00112                     
00113                     # Get the transformation from the fixed frame to the skeleton frame
00114                     (trans, rot)  = tf_listener.lookupTransform(self.fixed_frame, frame, rospy.Time(0))
00115                     position.x = trans[0]
00116                     position.y = trans[1]
00117                     position.z = trans[2]
00118                                         
00119                     # Set a marker at the origin of this frame
00120                     self.markers.points.append(position)
00121                 except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00122                     rospy.logerr("tf error when looking up " + frame + ' and ' + self.fixed_frame)
00123                     continue
00124             
00125             # Publish the set of markers
00126             marker_pub.publish(self.markers)                       
00127             r.sleep()
00128             
00129     def init_markers(self):
00130         # Set various parameters
00131         scale = rospy.get_param('~scale', 0.07)
00132         lifetime = rospy.get_param('~lifetime', 0) # 0 is forever
00133         ns = rospy.get_param('~ns', 'skeleton_markers')
00134         id = rospy.get_param('~id', 0)
00135         color = rospy.get_param('~color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00136         
00137         # Initialize the marker points list
00138         self.markers = Marker()
00139         self.markers.header.frame_id = self.fixed_frame
00140         self.markers.ns = ns
00141         self.markers.id = id
00142         self.markers.type = Marker.POINTS
00143         self.markers.action = Marker.ADD
00144         self.markers.lifetime = rospy.Duration(lifetime)
00145         self.markers.scale.x = scale
00146         self.markers.scale.y = scale
00147         self.markers.color.r = color['r']
00148         self.markers.color.g = color['g']
00149         self.markers.color.b = color['b']
00150         self.markers.color.a = color['a']
00151         
00152 if __name__ == '__main__':
00153     try:
00154         SkeletonMarkers()
00155     except rospy.ROSInterruptException:
00156         pass


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