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 import re
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 tf_prefix = rospy.get_param('~tf_prefix', '')
00037 skeleton_frames = rospy.get_param('~skeleton_frames', '')
00038 self.fixed_frame = rospy.get_param('~fixed_frame', 'openni_depth_frame')
00039
00040
00041 tf_prefix = '/' + tf_prefix + '/'
00042 tf_prefix = re.sub('/+', '/', tf_prefix)
00043
00044
00045 self.fixed_frame = '/' + tf_prefix + '/' + self.fixed_frame
00046 self.fixed_frame = re.sub('/+', '/', self.fixed_frame)
00047
00048
00049 tf_listener = tf.TransformListener()
00050
00051
00052 marker_pub = rospy.Publisher('skeleton_markers', Marker)
00053
00054
00055 self.init_markers()
00056
00057
00058
00059 user_id = None
00060
00061
00062
00063
00064
00065
00066 while not rospy.is_shutdown():
00067
00068 all_frames = [f for f in tf_listener.getFrameStrings()]
00069
00070
00071 try:
00072 test = all_frames.index(tf_prefix + 'torso')
00073 user_id = ""
00074 except:
00075 try:
00076 test = all_frames.index(tf_prefix + 'torso_1')
00077 user_id = "_1"
00078 except:
00079 pass
00080
00081 if user_id is None:
00082 r.sleep
00083 continue
00084
00085
00086 user_frames = list()
00087
00088 for frame in skeleton_frames:
00089 qualified_frame = tf_prefix + frame + user_id
00090 try:
00091 all_frames.index(qualified_frame)
00092 user_frames.append(qualified_frame)
00093 except:
00094 pass
00095
00096
00097
00098
00099 self.markers.header.stamp = rospy.Time.now()
00100
00101
00102 self.markers.points = list()
00103
00104
00105 for frame in user_frames:
00106 if frame == self.fixed_frame:
00107 continue
00108
00109 try:
00110 position = Point()
00111
00112
00113 (trans, rot) = tf_listener.lookupTransform(self.fixed_frame, frame, rospy.Time(0))
00114 position.x = trans[0]
00115 position.y = trans[1]
00116 position.z = trans[2]
00117
00118
00119 self.markers.points.append(position)
00120 except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00121 rospy.logerr("tf error when looking up " + frame + ' and ' + self.fixed_frame)
00122 continue
00123
00124
00125 marker_pub.publish(self.markers)
00126 r.sleep()
00127
00128 def init_markers(self):
00129
00130 scale = rospy.get_param('~scale', 0.07)
00131 lifetime = rospy.get_param('~lifetime', 0)
00132 ns = rospy.get_param('~ns', 'skeleton_markers')
00133 id = rospy.get_param('~id', 0)
00134 color = rospy.get_param('~color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00135
00136
00137 self.markers = Marker()
00138 self.markers.header.frame_id = self.fixed_frame
00139 self.markers.ns = ns
00140 self.markers.id = id
00141 self.markers.type = Marker.POINTS
00142 self.markers.action = Marker.ADD
00143 self.markers.lifetime = rospy.Duration(lifetime)
00144 self.markers.scale.x = scale
00145 self.markers.scale.y = scale
00146 self.markers.color.r = color['r']
00147 self.markers.color.g = color['g']
00148 self.markers.color.b = color['b']
00149 self.markers.color.a = color['a']
00150
00151 if __name__ == '__main__':
00152 try:
00153 SkeletonMarkers()
00154 except rospy.ROSInterruptException:
00155 pass