skeleton_joint_publisher.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 """
00004     skeleton_joint_publisher.py - Version 1.0 2011-11-17
00005     
00006     Publish the position of a skeleton joint on the /target_point topic.
00007     
00008     Created for the Pi Robot Project: http://www.pirobot.org
00009     Copyright (c) 2011 Patrick Goebel.  All rights reserved.
00010 
00011     This program is free software; you can redistribute it and/or modify
00012     it under the terms of the GNU General Public License as published by
00013     the Free Software Foundation; either version 2 of the License, or
00014     (at your option) any later version.
00015     
00016     This program is distributed in the hope that it will be useful,
00017     but WITHOUT ANY WARRANTY; without even the implied warranty of
00018     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019     GNU General Public License for more details at:
00020     
00021     http://www.gnu.org/licenses/gpl.html
00022 """
00023 
00024 import roslib; roslib.load_manifest('pi_head_tracking_3d_part2')
00025 import rospy
00026 import tf
00027 from geometry_msgs.msg import PointStamped 
00028 
00029 class PubSkeletonJoint():
00030     def __init__(self):
00031         rospy.init_node('skeleton_joint_publisher', anonymous=True)
00032                 
00033         rate = rospy.get_param('~rate', 10)
00034         r = rospy.Rate(rate)
00035                 
00036         self.target_joint = rospy.get_param('~target_joint', 'left_hand')
00037         self.mirror_skeleton = rospy.get_param('~mirror_skeleton', False)
00038                 
00039         # Remap this frame in the launch file or command line if necessary
00040         target_topic = 'target_point'
00041         self.head_pan_link = 'head_pan_link'
00042             
00043         if self.mirror_skeleton:
00044             try:
00045                 if self.target_joint.index("left") != -1:
00046                     self.target_joint = self.target_joint.replace("left", "right")
00047                 else:
00048                     self.target_joint = self.target_joint.replace("right", "left")
00049             except:
00050                 pass
00051         
00052         # Initialize the target point
00053         target = PointStamped()
00054         
00055         # The target's frame is the skeleton joint we want to track plus the user ID which initially will be 1
00056         target.header.frame_id = self.target_joint + "_1"
00057         
00058         # The target's coordinates are just the origin of the target frame
00059         target.point.x = 0
00060         target.point.y = 0
00061         target.point.z = 0
00062 
00063         # Initialize tf listener
00064         self.tf = tf.TransformListener()
00065         
00066         # Define the target publisher
00067         target_pub = rospy.Publisher(target_topic, PointStamped)
00068         
00069         rospy.loginfo("Assume the Psi pose in front of your camera.  Waiting for skeleton to appear...")
00070         
00071         # Give the tf buffer a chance to fill up
00072         rospy.sleep(5.0)
00073         
00074         while not rospy.is_shutdown():
00075             try:
00076                 target.header.frame_id = self.target_joint + "_1"
00077                 self.tf.waitForTransform(self.head_pan_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
00078             except:
00079                 try:
00080                     target.header.frame_id = self.target_joint + "_2"
00081                     self.tf.waitForTransform(self.head_pan_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
00082                 except:
00083                     continue
00084 
00085             target.header.stamp = rospy.Time.now()
00086             target_pub.publish(target)
00087         
00088             r.sleep()
00089 
00090 if __name__ == '__main__':
00091     try:
00092         PubSkeletonJoint()
00093         rospy.spin()
00094     except rospy.ROSInterruptException:
00095         pass
00096 


pi_head_tracking_3d_part2
Author(s): Patrick Goebel
autogenerated on Mon Oct 6 2014 03:26:53