skeleton_joint_publisher.py

Go to the documentation of this file.

#! /usr/bin/env python

"""
    skeleton_joint_publisher.py - Version 1.0 2011-11-17
    
    Publish the position of a skeleton joint on the /target_point topic.
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2011 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import roslib; roslib.load_manifest('pi_head_tracking_3d_part2')
import rospy
import tf
from geometry_msgs.msg import PointStamped 

class PubSkeletonJoint():
    def __init__(self):
        rospy.init_node('skeleton_joint_publisher', anonymous=True)
                
        rate = rospy.get_param('~rate', 10)
        r = rospy.Rate(rate)
                
        self.target_joint = rospy.get_param('~target_joint', 'left_hand')
        self.mirror_skeleton = rospy.get_param('~mirror_skeleton', False)
                
        # Remap this frame in the launch file or command line if necessary
        target_topic = 'target_point'
        self.head_pan_link = 'head_pan_link'
            
        if self.mirror_skeleton:
            try:
                if self.target_joint.index("left") != -1:
                    self.target_joint = self.target_joint.replace("left", "right")
                else:
                    self.target_joint = self.target_joint.replace("right", "left")
            except:
                pass
        
        # Initialize the target point
        target = PointStamped()
        
        # The target's frame is the skeleton joint we want to track plus the user ID which initially will be 1
        target.header.frame_id = self.target_joint + "_1"
        
        # The target's coordinates are just the origin of the target frame
        target.point.x = 0
        target.point.y = 0
        target.point.z = 0

        # Initialize tf listener
        self.tf = tf.TransformListener()
        
        # Define the target publisher
        target_pub = rospy.Publisher(target_topic, PointStamped)
        
        rospy.loginfo("Assume the Psi pose in front of your camera.  Waiting for skeleton to appear...")
        
        # Give the tf buffer a chance to fill up
        rospy.sleep(5.0)
        
        while not rospy.is_shutdown():
            try:
                target.header.frame_id = self.target_joint + "_1"
                self.tf.waitForTransform(self.head_pan_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
            except:
                try:
                    target.header.frame_id = self.target_joint + "_2"
                    self.tf.waitForTransform(self.head_pan_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
                except:
                    continue

            target.header.stamp = rospy.Time.now()
            target_pub.publish(target)
        
            r.sleep()

if __name__ == '__main__':
    try:
        PubSkeletonJoint()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass