Go to the documentation of this file.00001
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
00034 if skeleton['confidence'][joint1] > 0.5 and skeleton['confidence'][joint2] > 0.5:
00035
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 = []
00054 dependent_joints = rospy.get_param("dependent_joints", {})
00055
00056
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
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