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


pi_tracker
Author(s): Patrick Goebel
autogenerated on Fri Aug 28 2015 12:00:12