Go to the documentation of this file.00001
00002
00003 """
00004 Publish a skeleton message as 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 skeleton_markers.msg import Skeleton
00024 from visualization_msgs.msg import Marker
00025 from geometry_msgs.msg import Point
00026
00027 class SkeletonMarkers():
00028 def __init__(self):
00029 rospy.init_node('skeleton_markers')
00030
00031 rospy.on_shutdown(self.shutdown)
00032
00033 rospy.loginfo("Initializing Skeleton Markers Node...")
00034
00035 self.rate = rospy.get_param('~rate', 20)
00036 self.scale = rospy.get_param('~scale', 0.07)
00037 self.lifetime = rospy.get_param('~lifetime', 0)
00038 self.ns = rospy.get_param('~ns', 'skeleton_markers')
00039 self.id = rospy.get_param('~id', 0)
00040 self.color = rospy.get_param('~color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0})
00041
00042 rate = rospy.Rate(self.rate)
00043
00044
00045 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00046
00047
00048 self.marker_pub = rospy.Publisher('skeleton_markers', Marker)
00049
00050
00051 self.markers = Marker()
00052 self.markers.ns = self.ns
00053 self.markers.id = self.id
00054 self.markers.type = Marker.POINTS
00055 self.markers.action = Marker.ADD
00056 self.markers.lifetime = rospy.Duration(self.lifetime)
00057 self.markers.scale.x = self.scale
00058 self.markers.scale.y = self.scale
00059 self.markers.color.r = self.color['r']
00060 self.markers.color.g = self.color['g']
00061 self.markers.color.b = self.color['b']
00062 self.markers.color.a = self.color['a']
00063
00064 while not rospy.is_shutdown():
00065 self.marker_pub.publish(self.markers)
00066 rate.sleep()
00067
00068 def skeleton_handler(self, msg):
00069 self.markers.header.frame_id = msg.header.frame_id
00070 self.markers.header.stamp = rospy.Time.now()
00071 self.markers.points = list()
00072 for joint in msg.name:
00073 p = Point()
00074 p = msg.position[msg.name.index(joint)]
00075 self.markers.points.append(p)
00076
00077 def shutdown(self):
00078 rospy.loginfo('Shutting down Skeleton Marker Node.')
00079
00080 if __name__ == '__main__':
00081 try:
00082 SkeletonMarkers()
00083 except rospy.ROSInterruptException:
00084 pass