Go to the documentation of this file.00001
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
00037 self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
00038
00039
00040 self.skeleton_frames = rospy.get_param('~skeleton_frames', '')
00041
00042
00043 tf_listener = tf.TransformListener()
00044
00045
00046 marker_pub = rospy.Publisher('skeleton_markers', Marker, queue_size=5)
00047
00048
00049 self.initialize_markers()
00050
00051
00052 tf_listener.waitForTransform(self.fixed_frame, self.fixed_frame, rospy.Time(), rospy.Duration(60.0))
00053
00054
00055 skeleton_detected = False
00056
00057
00058 while not rospy.is_shutdown():
00059
00060 self.markers.header.stamp = rospy.Time.now()
00061
00062
00063 self.markers.points = list()
00064
00065
00066 while not skeleton_detected:
00067
00068 frames = [f for f in tf_listener.getFrameStrings() if f.startswith('head_')]
00069
00070 try:
00071
00072
00073 head_frame = frames[0]
00074 user_index = head_frame.replace('head_', '')
00075
00076
00077
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
00090 for frame in self.skeleton_frames:
00091
00092 skel_frame = frame + "_" + str(user_index)
00093
00094
00095
00096 position = Point()
00097
00098
00099
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
00107 self.markers.points.append(position)
00108 except:
00109 pass
00110
00111
00112 marker_pub.publish(self.markers)
00113
00114 r.sleep()
00115
00116 def initialize_markers(self):
00117
00118 scale = rospy.get_param('~scale', 0.07)
00119 lifetime = rospy.get_param('~lifetime', 0)
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
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