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


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