pi_tracker_lib.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Common functions shared by nodes in the pi_tracker package.
00005     Can be used with the OpenNI tracker package in junction with a
00006     Kinect RGB-D camera.
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_tracker')
00025 import rospy
00026 from sensor_msgs.msg import JointState
00027 import xml.dom.minidom
00028 from math import pi
00029 import PyKDL as KDL
00030     
00031 def get_body_dimension(skeleton, joint1, joint2, default_length):
00032     try:
00033         # Compute a body dimension to use to scale gesture measurements.
00034         if skeleton['confidence'][joint1] > 0.5 and skeleton['confidence'][joint2] > 0.5:
00035             # The KDL Normalize() function returns the length of the vector being normalized.
00036             length = (skeleton['position'][joint1] - skeleton['position'][joint2]).Normalize()
00037             if length == 0:
00038                 length = default_length
00039         else:
00040             length = default_length
00041     except:
00042          length = default_length
00043         
00044     return length
00045     
00046 def get_joints():
00047     """ This function is take from the joint_state_publisher package written by David Lu!!
00048         See http://www.ros.org/wiki/joint_state_publisher
00049     """
00050     description = rospy.get_param("robot_description")
00051     robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
00052     free_joints = {}
00053     joint_list = [] # for maintaining the original order of the joints
00054     dependent_joints = rospy.get_param("dependent_joints", {})
00055     
00056     # Find all non-fixed joints.
00057     for child in robot.childNodes:
00058         if child.nodeType is child.TEXT_NODE:
00059             continue
00060         if child.localName == 'joint':
00061             jtype = child.getAttribute('type')
00062             if jtype == 'fixed':
00063                 continue
00064             name = child.getAttribute('name')
00065 
00066             if jtype == 'continuous':
00067                 minval = -pi
00068                 maxval = pi
00069             else:
00070                 limit = child.getElementsByTagName('limit')[0]
00071                 minval = float(limit.getAttribute('lower'))
00072                 maxval = float(limit.getAttribute('upper'))
00073 
00074             if name in dependent_joints:
00075                 continue
00076             if minval > 0 or maxval < 0:
00077                 zeroval = (maxval + minval)/2
00078             else:
00079                 zeroval = 0
00080 
00081             joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
00082             free_joints[name] = joint
00083             joint_list.append(name)
00084 
00085     joint_state = JointState()
00086     joint_state.header.stamp = rospy.Time.now()
00087 
00088     # Add Free Joints.
00089     for (name, joint) in free_joints.items():
00090         joint_state.name.append(str(name))
00091         joint_state.position.append(joint['value'])
00092         joint_state.velocity.append(0)
00093         
00094     return joint_state
00095         
00096 
00097     


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49